[
  {
    "path": ".gitattributes",
    "content": "# Ignore Python files in linguist\n*.py linguist-detectable=false\n\n# Images\n*.gif filter=lfs diff=lfs merge=lfs -text\n*.jpg filter=lfs diff=lfs merge=lfs -text\n*.png filter=lfs diff=lfs merge=lfs -text\n*.psd filter=lfs diff=lfs merge=lfs -text\n\n# Archives\n*.gz filter=lfs diff=lfs merge=lfs -text\n*.tar filter=lfs diff=lfs merge=lfs -text\n*.zip filter=lfs diff=lfs merge=lfs -text\n# Documents\n*.pdf filter=lfs diff=lfs merge=lfs -text\n# Numpy data\n*.npy filter=lfs diff=lfs merge=lfs -text\n# Debian package\n*.deb filter=lfs diff=lfs merge=lfs -text\n# Shared libraries\n*.so.* filter=lfs diff=lfs merge=lfs -text\n*.so filter=lfs diff=lfs merge=lfs -text\n*.a filter=lfs diff=lfs merge=lfs -text\n\n# trtexec\ntrtexec_x86_64 filter=lfs diff=lfs merge=lfs -text\n\n"
  },
  {
    "path": ".gitignore",
    "content": "# Ignore all pycache files\n**/__pycache__/**\n# Ignore ROS build  files\n**/build/**\n**/install/**\n**/cc_internals/**\n**/log/**\n"
  },
  {
    "path": "LICENSE",
    "content": "NVIDIA ISAAC ROS SOFTWARE LICENSE\n\nThis license is a legal agreement between you and NVIDIA Corporation (\"NVIDIA\") and governs the use of the NVIDIA Isaac ROS software and materials provided hereunder (“SOFTWARE”).\n\nThis license can be accepted only by an adult of legal age of majority in the country in which the SOFTWARE is used. \n\nIf you are entering into this license on behalf of a company or other legal entity, you represent that you have the legal authority to bind the entity to this license, in which case “you” will mean the entity you represent. \n\nIf you don’t have the required age or authority to accept this license, or if you don’t accept all the terms and conditions of this license, do not download, install or use the SOFTWARE. \n\nYou agree to use the SOFTWARE only for purposes that are permitted by (a) this license, and (b) any applicable law, regulation or generally accepted practices or guidelines in the relevant jurisdictions.\n\n1. LICENSE. Subject to the terms of this license, NVIDIA hereby grants you a non-exclusive, non-transferable license, without the right to sublicense (except as expressly provided in this license) to:\na. Install and use the SOFTWARE,\nb. Modify and create derivative works of sample or reference source code delivered in the SOFTWARE, and\nc. Distribute any part of the SOFTWARE (i) as incorporated into a software application that has material additional functionality beyond the included portions of the SOFTWARE, or (ii) unmodified in binary format, in each case subject to the distribution requirements indicated in this license.\n\n2. DISTRIBUTION REQUIREMENTS. These are the distribution requirements for you to exercise the distribution grant above:\n    a. The following notice shall be included in modifications and derivative works of source code distributed: “This software contains source code provided by NVIDIA Corporation.” \n    b. You agree to distribute the SOFTWARE subject to the terms at least as protective as the terms of this license, including (without limitation) terms relating to the license grant, license restrictions and protection of NVIDIA’s intellectual property rights. Additionally, you agree that you will protect the privacy, security and legal rights of your application users. \n    c. You agree to notify NVIDIA in writing of any known or suspected distribution or use of the SOFTWARE not in compliance with the requirements of this license, and to enforce the terms of your agreements with respect to the distributed portions of the SOFTWARE.\n3. AUTHORIZED USERS. You may allow employees and contractors of your entity or of your subsidiary(ies) to access and use the SOFTWARE from your secure network to perform work on your behalf. If you are an academic institution you may allow users enrolled or employed by the academic institution to access and use the SOFTWARE from your secure network. You are responsible for the compliance with the terms of this license by your authorized users. \n\n4. LIMITATIONS. Your license to use the SOFTWARE is restricted as follows:\n    a. The SOFTWARE is licensed for you to develop applications only for their use in systems with NVIDIA GPUs.\n    b. You may not reverse engineer, decompile or disassemble, or remove copyright or other proprietary notices from any portion of the SOFTWARE or copies of the SOFTWARE. \n    c. Except as expressly stated above in this license, you may not sell, rent, sublicense, transfer, distribute, modify, or create derivative works of any portion of the SOFTWARE. \n    d.  Unless you have an agreement with NVIDIA for this purpose, you may not indicate that an application created with the SOFTWARE is sponsored or endorsed by NVIDIA.\n    e.  You may not bypass, disable, or circumvent any technical limitation, encryption, security, digital rights management or authentication mechanism in the SOFTWARE.  \n    f. You may not use the SOFTWARE in any manner that would cause it to become subject to an open source software license. As examples, licenses that require as a condition of use, modification, and/or distribution that the SOFTWARE be: (i) disclosed or distributed in source code form; (ii) licensed for the purpose of making derivative works; or (iii) redistributable at no charge.\n    g.  You acknowledge that the SOFTWARE as delivered is not tested or certified by NVIDIA for use in connection with the design, construction, maintenance, and/or operation of any system where the use or failure of such system could result in a situation that threatens the safety of human life or results in catastrophic damages (each, a \"Critical Application\"). Examples of Critical Applications include use in avionics, navigation, autonomous vehicle applications, ai solutions for automotive products, military, medical, life support or other life critical applications. NVIDIA shall not be liable to you or any third party, in whole or in part, for any claims or damages arising from such uses. You are solely responsible for ensuring that any product or service developed with the SOFTWARE as a whole includes sufficient features to comply with all applicable legal and regulatory standards and requirements.\n    h. You agree to defend, indemnify and hold harmless NVIDIA and its affiliates, and their respective employees, contractors, agents, officers and directors, from and against any and all claims, damages, obligations, losses, liabilities, costs or debt, fines, restitutions and expenses (including but not limited to attorney’s fees and costs incident to establishing the right of indemnification) arising out of or related to your use of goods and/or services that include or utilize the SOFTWARE, or for use of the SOFTWARE outside of the scope of this license or not in compliance with its terms. \n\n5. UPDATES. NVIDIA may, at its option, make available patches, workarounds or other updates to this SOFTWARE. Unless the updates are provided with their separate governing terms, they are deemed part of the SOFTWARE licensed to you as provided in this license. \n\n6. PRE-RELEASE VERSIONS. SOFTWARE versions identified as alpha, beta, preview, early access or otherwise as pre-release may not be fully functional, may contain errors or design flaws, and may have reduced or different security, privacy, availability, and reliability standards relative to commercial versions of NVIDIA software and materials. You may use a pre-release SOFTWARE version at your own risk, understanding that these versions are not intended for use in production or business-critical systems.\n\n7. COMPONENTS UNDER OTHER LICENSES. The SOFTWARE may include NVIDIA or third-party components with separate legal notices or terms as may be described in proprietary notices accompanying the SOFTWARE, such as components governed by open source software licenses. If and to the extent there is a conflict between the terms in this license and the license terms associated with a component, the license terms associated with the component controls only to the extent necessary to resolve the conflict.  \n\n8. OWNERSHIP. \n\n8.1 NVIDIA reserves all rights, title and interest in and to the SOFTWARE not expressly granted to you under this license. NVIDIA and its suppliers hold all rights, title and interest in and to the SOFTWARE, including their respective intellectual property rights. The SOFTWARE is copyrighted and protected by the laws of the United States and other countries, and international treaty provisions.\n\n8.2 Subject to the rights of NVIDIA and its suppliers in the SOFTWARE, you hold all rights, title and interest in and to your applications and your derivative works of the sample or reference source code delivered in the SOFTWARE including their respective intellectual property rights. With respect to source code samples or reference source code licensed to you, NVIDIA and its affiliates are free to continue independently developing source code samples and you covenant not to sue NVIDIA, its affiliates or their licensees with respect to later versions of NVIDIA released source code.\n \n9. FEEDBACK. You may, but are not obligated to, provide to NVIDIA Feedback. “Feedback” means suggestions, fixes, modifications, feature requests or other feedback regarding the SOFTWARE. Feedback, even if designated as confidential by you, shall not create any confidentiality obligation for NVIDIA. NVIDIA and its designees have a perpetual, non-exclusive, worldwide, irrevocable license to use, reproduce, publicly display, modify, create derivative works of, license, sublicense, and otherwise distribute and exploit Feedback as NVIDIA sees fit without payment and without obligation or restriction of any kind on account of intellectual property rights or otherwise.\n\n10. NO WARRANTIES. THE SOFTWARE IS PROVIDED AS-IS. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW NVIDIA AND ITS AFFILIATES EXPRESSLY DISCLAIM ALL WARRANTIES OF ANY KIND OR NATURE, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, OR FITNESS FOR A PARTICULAR PURPOSE. NVIDIA DOES NOT WARRANT THAT THE SOFTWARE WILL MEET YOUR REQUIREMENTS OR THAT THE OPERATION THEREOF WILL BE UNINTERRUPTED OR ERROR-FREE, OR THAT ALL ERRORS WILL BE CORRECTED. \n\n11. LIMITATIONS OF LIABILITY. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW NVIDIA AND ITS AFFILIATES SHALL NOT BE LIABLE FOR ANY SPECIAL, INCIDENTAL, PUNITIVE OR CONSEQUENTIAL DAMAGES, OR FOR ANY LOST PROFITS, PROJECT DELAYS, LOSS OF USE, LOSS OF DATA OR LOSS OF GOODWILL, OR THE COSTS OF PROCURING SUBSTITUTE PRODUCTS, ARISING OUT OF OR IN CONNECTION WITH THIS LICENSE OR THE USE OR PERFORMANCE OF THE SOFTWARE, WHETHER SUCH LIABILITY ARISES FROM ANY CLAIM BASED UPON BREACH OF CONTRACT, BREACH OF WARRANTY, TORT (INCLUDING NEGLIGENCE), PRODUCT LIABILITY OR ANY OTHER CAUSE OF ACTION OR THEORY OF LIABILITY, EVEN IF NVIDIA HAS PREVIOUSLY BEEN ADVISED OF, OR COULD REASONABLY HAVE FORESEEN, THE POSSIBILITY OF SUCH DAMAGES. IN NO EVENT WILL NVIDIA’S AND ITS AFFILIATES TOTAL CUMULATIVE LIABILITY UNDER OR ARISING OUT OF THIS LICENSE EXCEED US$10.00. THE NATURE OF THE LIABILITY OR THE NUMBER OF CLAIMS OR SUITS SHALL NOT ENLARGE OR EXTEND THIS LIMIT. \n\n12. TERMINATION. Your rights under this license will terminate automatically without notice from NVIDIA if you fail to comply with any term and condition of this license or if you commence or participate in any legal proceeding against NVIDIA with respect to the SOFTWARE. NVIDIA may terminate this license with advance written notice to you, if NVIDIA decides to no longer provide the SOFTWARE in a country or, in NVIDIA’s sole discretion, the continued use of it is no longer commercially viable. Upon any termination of this license, you agree to promptly discontinue use of the SOFTWARE and destroy all copies in your possession or control. Your prior distributions in accordance with this license are not affected by the termination of this license. All provisions of this license will survive termination, except for the license granted to you. \n\n13. APPLICABLE LAW. This license will be governed in all respects by the laws of the United States and of the State of Delaware, without regard to the conflicts of laws principles. The United Nations Convention on Contracts for the International Sale of Goods is specifically disclaimed. You agree to all terms of this license in the English language. The state or federal courts residing in Santa Clara County, California shall have exclusive jurisdiction over any dispute or claim arising out of this license. Notwithstanding this, you agree that NVIDIA shall still be allowed to apply for injunctive remedies or urgent legal relief in any jurisdiction. \n\n14. NO ASSIGNMENT. This license and your rights and obligations thereunder may not be assigned by you by any means or operation of law without NVIDIA’s permission. Any attempted assignment not approved by NVIDIA in writing shall be void and of no effect. NVIDIA may assign, delegate or transfer this license and its rights and obligations, and if to a non-affiliate you will be notified.\n \n15. EXPORT. The SOFTWARE is subject to United States export laws and regulations. You agree to comply with all applicable U.S. and international export laws, including the Export Administration Regulations (EAR) administered by the U.S. Department of Commerce and economic sanctions administered by the U.S. Department of Treasury’s Office of Foreign Assets Control (OFAC). These laws include restrictions on destinations, end-users and end-use. By accepting this license, you confirm that you are not currently residing in a country or region currently embargoed by the U.S. and that you are not otherwise prohibited from receiving the SOFTWARE.\n\n16. GOVERNMENT USE. The SOFTWARE is, and shall be treated as being, “Commercial Items” as that term is defined at 48 CFR § 2.101, consisting of “commercial computer software” and “commercial computer software documentation”, respectively, as such terms are used in, respectively, 48 CFR § 12.212 and 48 CFR §§ 227.7202 & 252.227-7014(a)(1). Use, duplication or disclosure by the U.S. Government or a U.S. Government subcontractor is subject to the restrictions in this license pursuant to 48 CFR § 12.212 or 48 CFR § 227.7202. In no event shall the US Government user acquire rights in the SOFTWARE beyond those specified in 48 C.F.R. 52.227-19(b)(1)-(2). \n\n17. NOTICES. Please direct your legal notices or other correspondence to NVIDIA Corporation, 2788 San Tomas Expressway, Santa Clara, California 95051, United States of America, Attention: Legal Department.\n\n18. ENTIRE AGREEMENT. This license is the final, complete and exclusive agreement between the parties relating to the subject matter of this license and supersedes all prior or contemporaneous understandings and agreements relating to this subject matter, whether oral or written. If any court of competent jurisdiction determines that any provision of this license is illegal, invalid or unenforceable, the remaining provisions will remain in full force and effect. Any amendment or waiver under this license shall be in writing and signed by representatives of both parties.\n\n(v. November 17, 2021)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 \n"
  },
  {
    "path": "README.md",
    "content": "# Isaac ROS NITROS\n\nNVIDIA Isaac Transport for ROS package for hardware-acceleration friendly movement of messages.\n\n<div align=\"center\"><a class=\"reference internal image-reference\" href=\"https://media.githubusercontent.com/media/NVIDIA-ISAAC-ROS/.github/release-4.4/resources/isaac_ros_docs/repositories_and_packages/isaac_ros_nitros/image5-1.gif/\"><img alt=\"image\" src=\"https://media.githubusercontent.com/media/NVIDIA-ISAAC-ROS/.github/release-4.4/resources/isaac_ros_docs/repositories_and_packages/isaac_ros_nitros/image5-1.gif/\" width=\"600px\"/></a></div>\n\n## Overview\n\nIsaac ROS NITROS contains NVIDIA’s implementation\nof type adaptation and negotiation in ROS 2. To learn more about NITROS, see [here](https://nvidia-isaac-ros.github.io/concepts/nitros/index.html).\n\nIsaac ROS NITROS is composed of a number of individual packages, each with either a functional or structural purpose:\n\n`isaac_ros_gxf`:\n: This package serves as a container for precompiled GXF extensions used by other Isaac ROS packages.\n  While a number of GXF extensions used by Isaac ROS are provided with source, the extensions contained in `isaac_ros_gxf` are license constrained and are thus shipped as `.so` binaries.\n\n`isaac_ros_managed_nitros`:\n: This package contains the wrapper classes that enable developers to add NITROS-compatible publishers and subscribers to third-party CUDA-based ROS nodes.\n  For more information about CUDA with NITROS, see [here](https://nvidia-isaac-ros.github.io/concepts/nitros/cuda_with_nitros.html).\n\n`isaac_ros_nitros`:\n: This package contains the base `NitrosNode` class and associated core utilities that serve as the foundation for all NITROS-based ROS nodes.\n\n`isaac_ros_nitros_bridge`:\n: This folder contains the implementation of the NITROS Bridge for inter-process communication.\n\n`isaac_ros_nitros_interfaces`:\n: This package contains the definitions of the custom ROS 2 interfaces that facilitate type negotiation between NITROS nodes.\n\n`isaac_ros_nitros_topic_tools`:\n: This folder contains a NITROS based implementation of some of the nodes in the [topic_tools package](https://github.com/ros-tooling/topic_tools).\n\n`isaac_ros_nitros_type`:\n: This folder contains a number of packages, each defining a specific NITROS type and the associated type adaptation logic to convert to and from a standard ROS type.\n\n`isaac_ros_pynitros`:\n: This folder contains the implementation of Python NITROS.\n\n---\n\n## Documentation\n\nPlease visit the [Isaac ROS Documentation](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/index.html) to learn how to use this repository.\n\n---\n\n## Packages\n\n* [`isaac_ros_gxf`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_gxf/index.html)\n* [`isaac_ros_managed_nitros`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_managed_nitros/index.html)\n* [`isaac_ros_nitros`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros/index.html)\n  * [Quickstart](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros/index.html#quickstart)\n* [Isaac ROS NITROS Bridge](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_bridge/index.html)\n  * [Overview](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_bridge/index.html#overview)\n  * [Performance](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_bridge/index.html#performance)\n  * [Packages](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_bridge/index.html#packages)\n  * [Quickstart](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_bridge/index.html#quickstart)\n  * [Try Another Example](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_bridge/index.html#try-another-example)\n  * [Troubleshooting](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_bridge/index.html#troubleshooting)\n  * [Updates](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_bridge/index.html#updates)\n* [`isaac_ros_nitros_bridge_ros2`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/index.html)\n  * [API](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/index.html#api)\n* [`isaac_ros_nitros_topic_tools`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_topic_tools/index.html)\n  * [NitrosCameraDrop Node](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_topic_tools/index.html#nitroscameradrop-node)\n* [`isaac_ros_nitros_type`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_nitros_type/index.html)\n* [`isaac_ros_pynitros`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_pynitros/index.html)\n  * [Creating PyNITROS-Accelerated Nodes](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_pynitros/index.html#creating-pynitros-accelerated-nodes)\n  * [Quickstart](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_pynitros/index.html#quickstart)\n  * [Try More Examples](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_pynitros/index.html#try-more-examples)\n  * [API Reference](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nitros/isaac_ros_pynitros/index.html#api-reference)\n\n## Latest\n\nUpdate 2026-04-30: Compatibility and integration updates for the Isaac ROS 4.4.0 release\n"
  },
  {
    "path": "SECURITY.md",
    "content": "## Security\n\nNVIDIA is dedicated to the security and trust of our software products and services, including all source code repositories managed through our organization.\n\nIf you need to report a security issue, please use the appropriate contact points outlined below. **Please do not report security vulnerabilities through GitHub.** If a potential security issue is inadvertently reported via a public issue or pull request, NVIDIA maintainers may limit public discussion and redirect the reporter to the appropriate private disclosure channels.\n\n## Reporting Potential Security Vulnerability in an NVIDIA Product\n\nTo report a potential security vulnerability in any NVIDIA product:\n- Web: [Security Vulnerability Submission Form](https://www.nvidia.com/object/submit-security-vulnerability.html)\n- E-Mail: psirt@nvidia.com\n    - We encourage you to use the following PGP key for secure email communication: [NVIDIA public PGP Key for communication](https://www.nvidia.com/en-us/security/pgp-key)\n    - Please include the following information:\n   \t - Product/Driver name and version/branch that contains the vulnerability\n     - Type of vulnerability (code execution, denial of service, buffer overflow, etc.)\n   \t - Instructions to reproduce the vulnerability\n   \t - Proof-of-concept or exploit code\n   \t - Potential impact of the vulnerability, including how an attacker could exploit the vulnerability\n\nWhile NVIDIA currently does not have a bug bounty program, we do offer acknowledgement when an externally reported security issue is addressed under our coordinated vulnerability disclosure policy. Please visit our [Product Security Incident Response Team (PSIRT)](https://www.nvidia.com/en-us/security/psirt-policies/) policies page for more information.\n\n## NVIDIA Product Security\n\nFor all security-related concerns, please visit NVIDIA's Product Security portal at https://www.nvidia.com/en-us/security\n"
  },
  {
    "path": "isaac_ros_gxf/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_gxf)\n\n# Add option for overriding GXF core library directory\noption(GXF_CORE_LIB_DIR_OVERRIDE \"Override path for GXF core library directory\" \"\")\n\nexecute_process(COMMAND uname -m COMMAND tr -d '\\n' OUTPUT_VARIABLE ARCHITECTURE)\nmessage( STATUS \"Architecture: ${ARCHITECTURE}\" )\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\nadd_library(${PROJECT_NAME} STATIC src/isaac_ros_gxf.cpp)\n\n# GXF core headers\ninstall(\n  DIRECTORY gxf/core/include/\n  DESTINATION share/${PROJECT_NAME}/gxf/include/\n)\n\n# Install extensions directory\nif( ${ARCHITECTURE} STREQUAL \"x86_64\" )\n  install(DIRECTORY gxf/core/lib/gxf_x86_64_cuda_13_0/ DESTINATION share/${PROJECT_NAME}/gxf/lib)\n  install(FILES gxf/core/lib/gxf_x86_64_cuda_13_0/core/libgxf_core.so DESTINATION lib)\n  install(FILES gxf/core/lib/gxf_x86_64_cuda_13_0/logger/libgxf_logger.so DESTINATION lib)\n  install(FILES gxf/core/lib/gxf_x86_64_cuda_13_0/multimedia/libgxf_multimedia.so DESTINATION lib)\n  install(FILES gxf/core/lib/gxf_x86_64_cuda_13_0/cuda/libgxf_cuda.so DESTINATION lib)\nelseif( ${ARCHITECTURE} STREQUAL \"aarch64\" )\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_CORE_LIB_DIR gxf/core/lib/gxf_aarch64_cuda_13_0/)\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_CORE_LIB_DIR gxf/core/lib/gxf_jetpack70/)\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\n  install(DIRECTORY ${GXF_CORE_LIB_DIR} DESTINATION share/${PROJECT_NAME}/gxf/lib)\n  install(FILES ${GXF_CORE_LIB_DIR}/core/libgxf_core.so DESTINATION lib)\n  install(FILES ${GXF_CORE_LIB_DIR}/logger/libgxf_logger.so DESTINATION lib)\n  install(FILES ${GXF_CORE_LIB_DIR}/multimedia/libgxf_multimedia.so DESTINATION lib)\n  install(FILES ${GXF_CORE_LIB_DIR}/cuda/libgxf_cuda.so DESTINATION lib)\nendif()\n\n# Register cmake in install.\nament_index_register_resource(isaac_ros_gxf_cmake_path CONTENT\n    \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/cmake\")\nlist(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS cmake/isaac_ros_gxf-extras.cmake)\n\n# Core\nadd_library(Core INTERFACE)\nset_target_properties(Core PROPERTIES\n  INTERFACE_INCLUDE_DIRECTORIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\"\n)\nset_property(TARGET Core PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    magic_enum::magic_enum\n    \"$<INSTALL_PREFIX>/lib/libgxf_core.so\"\n)\n\n# Logger\nadd_library(Logger INTERFACE)\nset_target_properties(Logger PROPERTIES\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/logger/libgxf_logger.so\"\n  INTERFACE_INCLUDE_DIRECTORIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\"\n)\n\n# Std\nadd_library(Std INTERFACE)\nset_target_properties(Std PROPERTIES\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/std/libgxf_std.so\"\n  INTERFACE_INCLUDE_DIRECTORIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\"\n)\n\n# Multimedia\nadd_library(Multimedia INTERFACE)\nset_target_properties(Multimedia PROPERTIES\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/multimedia/libgxf_multimedia.so\"\n  INTERFACE_INCLUDE_DIRECTORIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\"\n)\n\n# Serialization\nadd_library(Serialization INTERFACE)\nset_target_properties(Serialization PROPERTIES\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/serialization/libgxf_serialization.so\"\n  INTERFACE_INCLUDE_DIRECTORIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\"\n)\n\n# Cuda\nadd_library(Cuda INTERFACE)\nset_target_properties(Cuda PROPERTIES\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/cuda/libgxf_cuda.so\"\n  INTERFACE_INCLUDE_DIRECTORIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\"\n)\n\ninstall(TARGETS Core Logger Std Multimedia Serialization Cuda\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\nif(BUILD_TESTING)\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE cmake)\n"
  },
  {
    "path": "isaac_ros_gxf/cmake/isaac_ros_gxf-extras.cmake",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n# Common flags and cmake commands for all Isaac ROS packages based on NITROS\n\nmessage(STATUS \"Loading isaac_ros_gxf extras\")\n\n# Append local cmake module path for CMAKE_MODULE_PATH\nament_index_get_resource(ISAAC_ROS_GXF_CMAKE_PATH isaac_ros_gxf_cmake_path isaac_ros_gxf)\nlist(APPEND CMAKE_MODULE_PATH \"${ISAAC_ROS_GXF_CMAKE_PATH}/modules\")\n\n# Versions\nset(ISAAC_ROS_GXF_VERSION 2.5.0)"
  },
  {
    "path": "isaac_ros_gxf/cmake/modules/FindGXF.cmake",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n# Create GXF imported cmake targets\n#\n# This module defines GXF_FOUND if all GXF libraries are found or\n# if the required libraries (COMPONENTS property in find_package)\n# are found.\n#\n# A new imported target is created for each component (library)\n# under the GXF namespace (GXF::${component_name})\n#\n# Note: this leverages the find-module paradigm [1]. The config-file paradigm [2]\n# is recommended instead in CMake.\n# [1] https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html#config-file-packages\n# [2] https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html#find-module-packages\n\n# Library names\nlist(APPEND _GXF_EXTENSIONS\n    cuda\n    multimedia\n    serialization\n    std\n)\n\nlist(APPEND GXF_BASE_PATHS \"${isaac_ros_gxf_DIR}/../../../share/isaac_ros_gxf/gxf\")\n\n# Common headers\nfind_path(GXF_common_INCLUDE_DIR\n    NAMES common/\n    PATHS ${GXF_BASE_PATHS}\n    PATH_SUFFIXES include/\n    REQUIRED\n)\n\nmark_as_advanced(GXF_common_INCLUDE_DIR)\nlist(APPEND GXF_INCLUDE_DIR_VARS GXF_common_INCLUDE_DIR)\n\n# Libraries and their headers\nlist(APPEND _GXF_LIBRARIES ${_GXF_EXTENSIONS} core)\n\nforeach(component IN LISTS _GXF_LIBRARIES)\n    # headers\n    find_path(GXF_${component}_INCLUDE_DIR\n        NAMES \"${component}/\"\n        PATHS ${GXF_BASE_PATHS}\n        PATH_SUFFIXES\n            include\n            include/gxf\n        NO_DEFAULT_PATH\n        REQUIRED\n    )\n    mark_as_advanced(GXF_${component}_INCLUDE_DIR)\n    list(APPEND GXF_INCLUDE_DIR_VARS GXF_${component}_INCLUDE_DIR)\n\n    # library\n    find_library(GXF_${component}_LIBRARY\n        NAMES \"gxf_${component}\" \"${component}\"\n        PATHS ${GXF_BASE_PATHS}\n        PATH_SUFFIXES\n            lib/${component}\n            lib/${GXF_PLATFORM_SUFFIX}\n            lib/${GXF_PLATFORM_SUFFIX}/${component}\n            lib/${GXF_PLATFORM_SUFFIX}/isaac\n            lib/${GXF_PLATFORM_SUFFIX}/isaac/${component}\n        REQUIRED\n    )\n    mark_as_advanced(GXF_${component}_LIBRARY)\n    list(APPEND GXF_LIBRARY_VARS GXF_${component}_LIBRARY)\n\n    # create imported target\n    if(GXF_${component}_LIBRARY AND GXF_${component}_INCLUDE_DIR)\n        set(gxf_component_location \"${GXF_${component}_LIBRARY}\")\n\n        if(NOT TARGET GXF::${component})\n            # Assume SHARED, though technically UNKNOWN since we don't enforce .so\n            add_library(GXF::${component} SHARED IMPORTED)\n        endif()\n\n        list(APPEND GXF_${component}_INCLUDE_DIRS\n            ${GXF_${component}_INCLUDE_DIR}\n            ${GXF_${component}_INCLUDE_DIR}/${component}\n            ${GXF_common_INCLUDE_DIR}\n        )\n\n        message(STATUS \"Found ${component} lib=${gxf_component_location}, include=${GXF_${component}_INCLUDE_DIR}\")\n        # Points to the copied location of GXF\n        set_target_properties(GXF::${component} PROPERTIES\n            IMPORTED_LOCATION \"${gxf_component_location}\"\n            IMPORTED_NO_SONAME ON\n            INTERFACE_INCLUDE_DIRECTORIES \"${GXF_${component}_INCLUDE_DIRS}\"\n        )\n\n        set(GXF_${component}_FOUND TRUE)\n    else()\n        message(FATAL_ERROR \"Could not find GXF::${component}: lib={GXF_${component}_LIBRARY} / include=${GXF_${component}_INCLUDE_DIR}\")\n        set(GXF_${component}_FOUND FALSE)\n    endif()\nendforeach()\n\nunset(_GXF_EXTENSIONS)\nunset(_GXF_LIBRARIES)\n\n# Find version\nif(GXF_core_INCLUDE_DIR)\n    # Note: \"kGxfCoreVersion \\\"(.*)\\\"$\" does not work with a simple string\n    # REGEX (doesn't stop and EOL, neither $ nor \\n), so we first extract\n    # the line with file(STRINGS), then the version with string(REGEX)\n    file(STRINGS \"${GXF_core_INCLUDE_DIR}/core/gxf.h\" _GXF_VERSION_LINE\n        REGEX \"kGxfCoreVersion\"\n    )\n    string(REGEX MATCH \"kGxfCoreVersion \\\"(.*)\\\"\" _ ${_GXF_VERSION_LINE})\n    set(GXF_VERSION ${CMAKE_MATCH_1})\n    unset(_GXF_VERSION_LINE)\nendif()\n\n# Generate GXF_FOUND\ninclude(FindPackageHandleStandardArgs)\nfind_package_handle_standard_args(GXF\n    FOUND_VAR GXF_FOUND\n    VERSION_VAR GXF_VERSION\n    HANDLE_COMPONENTS # Looks for GXF_${component}_FOUND\n)\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/AMENT_IGNORE",
    "content": ""
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/assert.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_ASSERT_HPP_\n#define NVIDIA_COMMON_ASSERT_HPP_\n\n#include <cstdlib>\n#include <cstring>\n#include <string>\n\n#include \"common/backtrace.hpp\"\n#include \"common/logger.hpp\"\n\n// Prints a panic message and aborts the program\n#define GXF_PANIC(...)                                                                             \\\n  {                                                                                                \\\n    GXF_LOG_PANIC(__VA_ARGS__);                                                                    \\\n    PrettyPrintBacktrace();                                                                        \\\n    std::exit(1);                                                                                  \\\n  }\n\n// Checks if an expression evaluates to true. If not prints a panic message and aborts the program.\n#define GXF_ASSERT(expr, ...)                                                                      \\\n  if (!(expr)) {                                                                                   \\\n    GXF_LOG_PANIC(__VA_ARGS__);                                                                    \\\n    PrettyPrintBacktrace();                                                                        \\\n    std::exit(1);                                                                                  \\\n  }\n\n#define GXF_ASSERT_SUCCESS(expr)                                                                   \\\n  {                                                                                                \\\n    const auto _result = (expr);                                                                   \\\n    if (_result != 0) {                                                                            \\\n      GXF_LOG_PANIC(\"GXF operation failed: %s\", GxfResultStr(_result));                            \\\n      PrettyPrintBacktrace();                                                                      \\\n      std::exit(1);                                                                                \\\n    }                                                                                              \\\n  }                                                                                                \\\n\n// Asserts that A == true. If not prints a panic message and aborts the program.\n#define GXF_ASSERT_TRUE(exp_a)                                                                     \\\n  {                                                                                                \\\n    const auto _va = exp_a;                                                                        \\\n    GXF_ASSERT(_va == true, \"Assert failed: %s == true.\", std::to_string(_va).c_str());            \\\n  }\n\n// Asserts that A == false. If not prints a panic message and aborts the program.\n#define GXF_ASSERT_FALSE(exp_a)                                                                    \\\n  {                                                                                                \\\n    const auto _va = exp_a;                                                                        \\\n    GXF_ASSERT(_va == false, \"Assert failed: %s == false.\", std::to_string(_va).c_str());          \\\n  }\n\n// Asserts that A == B. If not prints a panic message and aborts the program.\n#define GXF_ASSERT_EQ(exp_a, exp_b)                                                                \\\n  {                                                                                                \\\n    const auto _va = exp_a;                                                                        \\\n    const auto _vb = exp_b;                                                                        \\\n    GXF_ASSERT(_va == _vb, \"Assert failed: %s == %s.\", std::to_string(_va).c_str(),                \\\n               std::to_string(_vb).c_str());                                                       \\\n  }\n\n// Asserts that A == B for two strings. If not prints a panic message and aborts the program.\n#define GXF_ASSERT_STREQ(exp_a, exp_b)                                                             \\\n  {                                                                                                \\\n    const char* _va = (exp_a);                                                                     \\\n    const char* _vb = (exp_b);                                                                     \\\n    GXF_ASSERT(std::strcmp(_va, _vb) == 0, \"Assert failed: %s == %s.\", _va, _vb);                  \\\n  }\n\n// Asserts that A != B. If not prints a panic message and aborts the program.\n#define GXF_ASSERT_NE(exp_a, exp_b)                                                                \\\n  {                                                                                                \\\n    const auto _va = exp_a;                                                                        \\\n    const auto _vb = exp_b;                                                                        \\\n    GXF_ASSERT(_va != _vb, \"Assert failed: %s != %s.\", std::to_string(_va).c_str(),                \\\n               std::to_string(_vb).c_str());                                                       \\\n  }\n\n// Asserts that A > B. If not prints a panic message and aborts the program.\n#define GXF_ASSERT_GT(exp_a, exp_b)                                                                \\\n  {                                                                                                \\\n    const auto _va = exp_a;                                                                        \\\n    const auto _vb = exp_b;                                                                        \\\n    GXF_ASSERT(_va > _vb, \"Assert failed: %s > %s.\", std::to_string(_va).c_str(),                  \\\n               std::to_string(_vb).c_str());                                                       \\\n  }\n\n// Asserts that A >= B. If not prints a panic message and aborts the program.\n#define GXF_ASSERT_GE(exp_a, exp_b)                                                                \\\n  {                                                                                                \\\n    const auto _va = exp_a;                                                                        \\\n    const auto _vb = exp_b;                                                                        \\\n    GXF_ASSERT(_va >= _vb, \"Assert failed: %s >= %s.\", std::to_string(_va).c_str(),                \\\n               std::to_string(_vb).c_str());                                                       \\\n  }\n\n// Asserts that A > B. If not prints a panic message and aborts the program.\n#define GXF_ASSERT_LT(exp_a, exp_b)                                                                \\\n  {                                                                                                \\\n    const auto _va = exp_a;                                                                        \\\n    const auto _vb = exp_b;                                                                        \\\n    GXF_ASSERT(_va < _vb, \"Assert failed: %s > %s.\", std::to_string(_va).c_str(),                  \\\n               std::to_string(_vb).c_str());                                                       \\\n  }\n\n// Asserts that A <= B. If not prints a panic message and aborts the program.\n#define GXF_ASSERT_LE(exp_a, exp_b)                                                                \\\n  {                                                                                                \\\n    const auto _va = exp_a;                                                                        \\\n    const auto _vb = exp_b;                                                                        \\\n    GXF_ASSERT(_va <= _vb, \"Assert failed: %s <= %s.\", std::to_string(_va).c_str(),                \\\n               std::to_string(_vb).c_str());                                                       \\\n  }\n\n// Asserts that abs(A - B) <= abs_error. If not prints a panic message and aborts the program.\n#define GXF_ASSERT_NEAR(exp_a, exp_b, exp_abs_error)                                               \\\n  {                                                                                                \\\n    const auto _va = exp_a;                                                                        \\\n    const auto _vb = exp_b;                                                                        \\\n    const auto _verror = exp_abs_error;                                                            \\\n    GXF_ASSERT(std::abs(_va - _vb) <= _verror, \"Assert failed: abs(%s - %s) <= %s.\",               \\\n               std::to_string(_va).c_str(), std::to_string(_vb).c_str(),                           \\\n               std::to_string(_verror).c_str());                                                   \\\n  }\n\n#endif  // NVIDIA_COMMON_ASSERT_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/backtrace.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_BACKTRACE_HPP_\n#define NVIDIA_COMMON_BACKTRACE_HPP_\n\n// Print the stacktrace with demangled function names (if possible)\n// Function is disabled for QNX since execinfo.h is not available in the QNX toolchain\nvoid PrettyPrintBacktrace();\n\n#endif  // NVIDIA_COMMON_BACKTRACE_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/byte.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_BYTE_HPP_\n#define NVIDIA_COMMON_BYTE_HPP_\n\n# if __cplusplus < 201703L\nnamespace std {\nusing byte = unsigned char;\n}  // namespace std\nusing std::byte;\n# else\nnamespace nvidia {\n    using byte = unsigned char;\n}\nusing nvidia::byte;\n\n#endif\n\n#endif  // NVIDIA_COMMON_BYTE_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/endian.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_ENDIAN_HPP_\n#define NVIDIA_COMMON_ENDIAN_HPP_\n\n#ifdef _QNX_SOURCE\n#include <net/netbyte.h>\n#else\n#include <endian.h>\n#endif\n\n#include \"common/type_utils.hpp\"\n\nnamespace nvidia {\n\n/// Returns true if the machine stores multi-byte integers in little-endian format\n/// FIXME: Use std::memcpy if reinterpret_cast has to be removed\ninline bool IsLittleEndian() {\n  const uint16_t test = 0x0102;\n  return *reinterpret_cast<const uint8_t*>(&test) == 0x02;\n}\n\n/// Convert @a value to little-endian.\n/// This overload should only be used if @c T is a signed integer type.\ntemplate <typename T, typename = EnableIf_t<IsIntegral_v<T>>>\ninline T EncodeLittleEndian(T value) {\n  static_assert(IsSigned_v<T>);\n  using U = MakeUnsigned_t<T>;\n  return static_cast<T>(EncodeLittleEndian(static_cast<U>(value)));\n}\n\ntemplate <> inline uint64_t EncodeLittleEndian<uint64_t>(uint64_t value) { return htole64(value); }\ntemplate <> inline uint32_t EncodeLittleEndian<uint32_t>(uint32_t value) { return htole32(value); }\ntemplate <> inline uint16_t EncodeLittleEndian<uint16_t>(uint16_t value) { return htole16(value); }\ntemplate <> inline uint8_t  EncodeLittleEndian<uint8_t> (uint8_t  value) { return value; }\n\n/// Convert @a value from little-endian.\n/// This overload should only be used if @c T is a signed integer type.\ntemplate <typename T, typename = EnableIf_t<IsIntegral_v<T>>>\ninline T DecodeLittleEndian(T value) {\n  static_assert(IsSigned_v<T>);\n  using U = MakeUnsigned_t<T>;\n  return static_cast<T>(DecodeLittleEndian(static_cast<U>(value)));\n}\n\ntemplate <> inline uint64_t DecodeLittleEndian<uint64_t>(uint64_t value) { return le64toh(value); }\ntemplate <> inline uint32_t DecodeLittleEndian<uint32_t>(uint32_t value) { return le32toh(value); }\ntemplate <> inline uint16_t DecodeLittleEndian<uint16_t>(uint16_t value) { return le16toh(value); }\ntemplate <> inline uint8_t  DecodeLittleEndian<uint8_t> (uint8_t  value) { return value; }\n\n/// Returns true if the machine stores multi-byte integers in big-endian format\n/// FIXME: Use std::memcpy if reinterpret_cast has to be removed\ninline bool IsBigEndian() {\n  const uint16_t test = 0x0102;\n  return *reinterpret_cast<const uint8_t*>(&test) == 0x01;\n}\n\n/// Convert @a value to big-endian.\n/// This overload should only be used if @c T is a signed integer type.\ntemplate <typename T, typename = EnableIf_t<IsIntegral_v<T>>>\ninline T EncodeBigEndian(T value) {\n  static_assert(IsSigned_v<T>);\n  using U = MakeUnsigned_t<T>;\n  return static_cast<T>(EncodeBigEndian(static_cast<U>(value)));\n}\n\ntemplate <> inline uint64_t EncodeBigEndian<uint64_t>(uint64_t value) { return htobe64(value); }\ntemplate <> inline uint32_t EncodeBigEndian<uint32_t>(uint32_t value) { return htobe32(value); }\ntemplate <> inline uint16_t EncodeBigEndian<uint16_t>(uint16_t value) { return htobe16(value); }\ntemplate <> inline uint8_t  EncodeBigEndian<uint8_t> (uint8_t  value) { return value; }\n\n/// Convert @a value from big-endian.\n/// This overload should only be used if @c T is a signed integer type.\ntemplate <typename T, typename = EnableIf_t<IsIntegral_v<T>>>\ninline T DecodeBigEndian(T value) {\n  static_assert(IsSigned_v<T>);\n  using U = MakeUnsigned_t<T>;\n  return static_cast<T>(DecodeBigEndian(static_cast<U>(value)));\n}\n\ntemplate <> inline uint64_t DecodeBigEndian<uint64_t>(uint64_t value) { return be64toh(value); }\ntemplate <> inline uint32_t DecodeBigEndian<uint32_t>(uint32_t value) { return be32toh(value); }\ntemplate <> inline uint16_t DecodeBigEndian<uint16_t>(uint16_t value) { return be16toh(value); }\ntemplate <> inline uint8_t  DecodeBigEndian<uint8_t> (uint8_t  value) { return value; }\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_COMMON_ENDIAN_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/expected.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_EXPECTED_HPP_\n#define NVIDIA_COMMON_EXPECTED_HPP_\n\n#include <utility>\n\n#include \"common/assert.hpp\"\n#include \"common/memory_utils.hpp\"\n#include \"common/strong_type.hpp\"\n#include \"common/type_utils.hpp\"\n\nnamespace nvidia {\n\ntemplate <class E> using Unexpected = StrongType<struct Unexpected_t, E>;\ntemplate <class, class> class Expected;\n\n///-------------------------------------------------------------------------------------------------\n///  Local Helper Functions\n///-------------------------------------------------------------------------------------------------\nnamespace detail {\n\ntemplate <class>\nstruct IsExpectedHelper : FalseType {};\n\ntemplate <class T, class E>\nstruct IsExpectedHelper<Expected<T, E>> : TrueType {};\n\ntemplate <class T>\nusing IsExpected = IsExpectedHelper<RemoveCVRef_t<T>>;\n\ntemplate <class T>\nconstexpr bool IsExpected_v = IsExpected<T>::value;\n\n// Extracts the error type from a pack of Expecteds if they all contain the same Error .\ntemplate <class...>\nstruct ErrorTypeHelper : TypeIdentity<void> {};\n\ntemplate <class... Ts, class E>\nstruct ErrorTypeHelper<Expected<Ts, E>...> : TypeIdentity<E> {};\n\ntemplate <class... Ts>\nusing ErrorType = typename ErrorTypeHelper<RemoveCVRef_t<Ts>...>::type;\n\ntemplate <class E, class... Ts>\nusing ErrorTypeOr = Conditional_t<sizeof...(Ts) == 0, E, ErrorType<Ts...>>;\n\n// Extracts the error type from a pack of Expecteds if they all contain the same Error .\ntemplate <class...>\nstruct ValueTypeHelper {};\n\ntemplate <class T, class E>\nstruct ValueTypeHelper<Expected<T, E>> { using type = T; };\n\ntemplate <class T>\nusing ValueType = typename ValueTypeHelper<RemoveCVRef_t<T>>::type;\n\n// Extracts the type from a Unexpected if provided. Otherwise, returns the type T.\ntemplate <class T>\nstruct UnexpectedTypeHelper : TypeIdentity<T> {};\n\ntemplate <class T>\nstruct UnexpectedTypeHelper<Unexpected<T>> : TypeIdentity<T> {};\n\ntemplate <class T>\nusing UnexpectedType = typename UnexpectedTypeHelper<RemoveCVRef_t<T>>::type;\n\n// Finds the result type of invoking a function with the wrapped types of a pack of Expecteds.\ntemplate <class F, class... Es>\nusing ResultType = decltype(Declval<F>()(Declval<Es>().value()...));\n\ntemplate <class F, class... Es>\nusing ErrorResultType = UnexpectedType<decltype(Declval<F>()(Declval<Es>().error()...))>;\n\n// Empty struct for representing an object with only one possible value (itself). This can be used\n// for speciallizing on void types where void would not compile because it is not a valid type.\n// NOTE: GCC9 has a bug where it cannot evaluate the destructor of an empty struct, so put a dummy\n//   byte in to pass the compiler warnings. All empty structs are always considered non-zero in size\n//   in C++ anyway, and these are functionally equivalent for use with Expected<void>.\n//   https://www.stroustrup.com/bs_faq2.html#sizeof-empty\nstruct Unit { byte dummy; };\n\n// If T is already Expected, keep as Expected, otherwise wrap as Expected<T, E>\ntemplate <class T, class E>\nstruct FlattenExpectedHelper : TypeIdentity<Expected<T, E>> {};\n\ntemplate <class T, class E>\nstruct FlattenExpectedHelper<Expected<T, E>, E> : TypeIdentity<Expected<T, E>> {};\n\ntemplate <class T, class E>\nstruct FlattenExpectedHelper<Expected<T, E>&, E>\n: TypeIdentity<Expected<AddLvalueReference_t<T>, E>> {};\n\ntemplate <class T, class E>\nstruct FlattenExpectedHelper<const Expected<T, E>&, E>\n: TypeIdentity<Expected<const AddLvalueReference_t<T>, E>> {};\n\ntemplate <class T, class E>\nusing FlattenExpected = typename FlattenExpectedHelper<T, E>::type;\n\n// Helper for specializing on variadic parameter packs\ntemplate <class...> struct Pack {};\n\n// Checks if a functor is callable with an Expected value.\ntemplate <class, class, class = void>\nstruct IsCallableHelper : FalseType {};\n\ntemplate <class F, class... Es>\nstruct IsCallableHelper<F, Pack<Es...>, void_t<ResultType<F, Es...>>> : TrueType {};\n\ntemplate <class F, class... Es>\nstruct IsCallable : IsCallableHelper<F, Pack<Es...>> {};\n\ntemplate <class F, class... Es>\nconstexpr bool IsCallable_v = IsCallable<F, Es...>::value;\n\n// Checks if a functor is callable with an Expected value.\ntemplate <class, class, class = void>\nstruct IsErrorCallableHelper : FalseType {};\n\ntemplate <class F, class... Es>\nstruct IsErrorCallableHelper<F, Pack<Es...>, void_t<ErrorResultType<F, Es...>>> : TrueType {};\n\ntemplate <class F, class... Es>\nstruct IsErrorCallable : IsErrorCallableHelper<F, Pack<Es...>> {};\n\ntemplate <class F, class... Es>\nconstexpr bool IsErrorCallable_v = IsErrorCallable<F, Es...>::value;\n\n// Maps the values with the given functor. If F returns an Expected, it returns the Expected type\n// directly, otherwise it constructs a new Expected.\ntemplate <class E = void, class F, class... Args>\nauto FunctorMap(F&& func, Args&&... expected) ->\n    EnableIf_t<!IsVoid_v<ResultType<F, Args...>>,\n               FlattenExpected<ResultType<F, Args...>, ErrorTypeOr<E, Args...>>> {\n  return std::forward<F>(func)(std::forward<Args>(expected).value()...);\n}\n\n// Maps the values with the given void functor. Returns Expected<void, Es...>.\ntemplate <class E = void, class F, class... Args>\nauto FunctorMap(F&& func, Args&&... expected) ->\n    EnableIf_t<IsVoid_v<ResultType<F, Args...>>, Expected<void, ErrorTypeOr<E, Args...>>> {\n  std::forward<F>(func)(std::forward<Args>(expected).value()...);\n  return {};\n}\n\n// Maps the error values with the given function. If F returns an Unexpected, it returns the\n// Unexpected type directly, otherwise it constructs a new Unexpected.\ntemplate <class F, class E>\nExpected<ValueType<E>, ErrorResultType<F, E>> FunctorMapError(F&& func, E&& expected) {\n  using R = Unexpected<ErrorResultType<F, E>>;\n  return R{std::forward<F>(func)(std::forward<E>(expected).error())};\n}\n\ntemplate <class T, class F>\nExpected<T, ErrorResultType<F>> FunctorMapError(F&& func) {\n  using R = Unexpected<ErrorResultType<F>>;\n  return R{std::forward<F>(func)()};\n}\n\n// Base class for all operations of Expected, not directly related to setting or creating the value\n// type T into the underlying byte buffer.\ntemplate <class T, class E, class Derived>\nclass ExpectedBase {\n private:\n  static_assert(!IsVoid_v<T>, \"ExpectedBase cannot wrap void types\");\n  static_assert(!IsVoid_v<E>, \"ExpectedBase cannot wrap void types\");\n  static_assert(!IsReference_v<T>, \"ExpectedBase cannot wrap reference types\");\n  static_assert(!IsReference_v<E>, \"ExpectedBase cannot wrap reference types\");\n\n  template <class F, class U>\n  using EnableIfMappable_t =\n      EnableIf_t<IsCallable_v<F, U>, decltype(FunctorMap(Declval<F>(), Declval<U>()))>;\n\n  template <class F, class U>\n  using EnableIfNonaryMappable_t =\n      EnableIf_t<IsCallable_v<F> && !IsCallable_v<F, U>, FlattenExpected<ResultType<F>, E>>;\n\n  template <class F, class U>\n  using EnableIfMappableError_t =\n      EnableIf_t<IsErrorCallable_v<F, U>, decltype(FunctorMapError(Declval<F>(), Declval<U>()))>;\n\n  template <class F, class U>\n  using EnableIfNonaryMappableError_t =\n      EnableIf_t<IsErrorCallable_v<F> && !IsErrorCallable_v<F, U>,\n                 Expected<T, ErrorResultType<F>>>;\n\n public:\n  ///-----------------------------------------------------------------------------------------------\n  /// Constructors\n  ///-----------------------------------------------------------------------------------------------\n  constexpr ExpectedBase(const ExpectedBase& other) { constructFrom(other); }\n  constexpr ExpectedBase(ExpectedBase&& other) { constructFrom(std::move(other)); }\n\n  // Construction from convertible error types\n  template <class G, class C>\n  explicit constexpr ExpectedBase(const ExpectedBase<T, G, C>& other) {\n    static_assert(IsConvertible_v<G, E>,\n        \"Cannot construct Expected from type with unconvertible error type.\");\n    constructFrom(other);\n  }\n\n  template <class G, class C>\n  explicit constexpr ExpectedBase(ExpectedBase<T, G, C>&& other) {\n    static_assert(IsConvertible_v<G, E>,\n        \"Cannot construct Expected from type with unconvertible error type.\");\n    constructFrom(std::move(other));\n  }\n\n  template <class G>\n  constexpr ExpectedBase(const Unexpected<G>& error, const char* error_message = \"\")\n    : is_error_{true}, error_message_{error_message} {\n    static_assert(IsConvertible_v<G, E>,\n        \"Cannot construct Unexpected from type with unconvertible error type.\");\n    InplaceConstruct<Unexpected<E>>(buffer_, error.value());\n  }\n\n  template <class G>\n  constexpr ExpectedBase(Unexpected<G>&& error, const char* error_message = \"\")\n    : is_error_{true}, error_message_{error_message} {\n    static_assert(IsConvertible_v<G, E>,\n        \"Cannot construct Unexpected from type with unconvertible error type.\");\n    InplaceConstruct<Unexpected<E>>(buffer_, std::move(error.value()));\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Destructor\n  ///-----------------------------------------------------------------------------------------------\n  ~ExpectedBase() { destruct(); }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Assignment\n  ///-----------------------------------------------------------------------------------------------\n  ExpectedBase& operator=(const ExpectedBase& other) {\n    destruct();\n    constructFrom(other);\n    return *this;\n  }\n\n  ExpectedBase& operator=(ExpectedBase&& other) {\n    destruct();\n    constructFrom(std::move(other));\n    return *this;\n  }\n\n  template <class G>\n  ExpectedBase& operator=(const Unexpected<G>& error) {\n    destruct();\n    is_error_ = true;\n    InplaceConstruct<Unexpected<E>>(buffer_, error);\n    return *this;\n  }\n\n  template <class G>\n  ExpectedBase& operator=(Unexpected<G>&& error) {\n    destruct();\n    is_error_ = true;\n    InplaceConstruct<Unexpected<E>>(buffer_, std::move(error));\n    return *this;\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Observers\n  ///-----------------------------------------------------------------------------------------------\n  template <class V = ValueType<Derived>, class D = const Derived>\n  constexpr EnableIf_t<!IsVoid_v<V>, RemoveReference_t<decltype(Declval<D*>()->value())>*>\n  operator->() const  { return &(derived()->value()); }\n\n  template <class V = ValueType<Derived>, class D = Derived>\n  constexpr EnableIf_t<!IsVoid_v<V>, RemoveReference_t<decltype(Declval<D*>()->value())>*>\n  operator->()        { return &(derived()->value()); }\n\n  template <class V = ValueType<Derived>>\n  constexpr EnableIf_t<!IsVoid_v<V>, const V&>\n  operator*() const&  { return derived()->value(); }\n\n  template <class V = ValueType<Derived>>\n  constexpr EnableIf_t<!IsVoid_v<V>, V&>\n  operator*() &       { return derived()->value(); }\n\n  template <class V = ValueType<Derived>, EnableIf_t<!IsVoid_v<V>, void*> = nullptr>\n  constexpr decltype(auto) operator*() const&& { return std::move(derived())->value(); }\n\n  template <class V = ValueType<Derived>, EnableIf_t<!IsVoid_v<V>, void*> = nullptr>\n  constexpr decltype(auto) operator*() && { return std::move(derived())->value(); }\n\n  template <class U, class V = ValueType<Derived>, EnableIf_t<!IsVoid_v<V>, void*> = nullptr>\n  constexpr V value_or(U&& default_value) const& {\n    return has_value() ? derived()->value() : std::forward<U>(default_value);\n  }\n\n  template <class U, class V = ValueType<Derived>, EnableIf_t<!IsVoid_v<V>, void*> = nullptr>\n  constexpr V value_or(U&& default_value) && {\n    return has_value() ? std::move(derived())->value() : std::forward<U>(default_value);\n  }\n\n  constexpr          bool     has_value() const { return !is_error_; }\n  constexpr explicit operator bool()      const { return !is_error_; }\n\n  constexpr const E&  error() const& { return unexpected().value(); }\n  constexpr       E&  error() &      { return unexpected().value(); }\n  constexpr       E&& error() &&     { return std::move(unexpected()).value(); }\n\n  constexpr const char* get_error_message() {\n    GXF_ASSERT(is_error_, \"Expected does not have an error. Check before accessing.\");\n    return error_message_;\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Logging\n  ///-----------------------------------------------------------------------------------------------\n  // Logging helper functions if this contains an error value.\n  template <class... Args>\n  constexpr const Derived& log_error(Args&&... args) const& {\n    if (is_error_) { GXF_LOG_ERROR(std::forward<Args>(args)...); }\n    return static_cast<const Derived&>(*this);\n  }\n  template <class... Args>\n  constexpr Derived& log_error(Args&&... args) & {\n    if (is_error_) { GXF_LOG_ERROR(std::forward<Args>(args)...); }\n    return static_cast<Derived&>(*this);\n  }\n  template <class... Args>\n  constexpr Derived&& log_error(Args&&... args) && {\n  // GCC is not able to do format security validation when the string is coming from a\n  // variadic template, even if the string is originally a char*\n  // ignore this warning until a more recent GCC version fixes this behavior\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wformat-security\"\n    if (is_error_) { GXF_LOG_ERROR(std::forward<Args>(args)...); }\n#pragma GCC diagnostic pop\n    return static_cast<Derived&&>(std::move(*this));\n  }\n\n  template <class... Args>\n  constexpr const Derived& log_warning(Args&&... args) const& {\n    if (is_error_) { GXF_LOG_WARNING(std::forward<Args>(args)...); }\n    return static_cast<const Derived&>(*this);\n  }\n  template <class... Args>\n  constexpr Derived& log_warning(Args&&... args) & {\n    if (is_error_) { GXF_LOG_WARNING(std::forward<Args>(args)...); }\n    return static_cast<Derived&>(*this);\n  }\n  template <class... Args>\n  constexpr Derived&& log_warning(Args&&... args) && {\n    if (is_error_) { GXF_LOG_WARNING(std::forward<Args>(args)...); }\n    return static_cast<Derived&&>(std::move(*this));\n  }\n\n  template <class... Args>\n  constexpr const Derived& log_info(Args&&... args) const& {\n    if (is_error_) { GXF_LOG_INFO(std::forward<Args>(args)...); }\n    return static_cast<const Derived&>(*this);\n  }\n  template <class... Args>\n  constexpr Derived& log_info(Args&&... args) & {\n    if (is_error_) { GXF_LOG_INFO(std::forward<Args>(args)...); }\n    return static_cast<Derived&>(*this);\n  }\n  template <class... Args>\n  constexpr Derived&& log_info(Args&&... args) && {\n    if (is_error_) { GXF_LOG_INFO(std::forward<Args>(args)...); }\n    return static_cast<Derived&&>(std::move(*this));\n  }\n\n  template <class... Args>\n  constexpr const Derived& log_debug(Args&&... args) const& {\n    if (is_error_) { GXF_LOG_DEBUG(std::forward<Args>(args)...); }\n    return static_cast<const Derived&>(*this);\n  }\n  template <class... Args>\n  constexpr Derived& log_debug(Args&&... args) & {\n    if (is_error_) { GXF_LOG_DEBUG(std::forward<Args>(args)...); }\n    return static_cast<Derived&>(*this);\n  }\n  template <class... Args>\n  constexpr Derived&& log_debug(Args&&... args) && {\n    if (is_error_) { GXF_LOG_DEBUG(std::forward<Args>(args)...); }\n    return static_cast<Derived&&>(std::move(*this));\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Substitute\n  ///-----------------------------------------------------------------------------------------------\n  // When this expected does not contain an error an expected with the given value 'next' is\n  // created and returned, otherwise an unexpected with the error is returned.\n  template <class U>\n  constexpr FlattenExpected<U, E> substitute(U next) const& {\n    return has_value() ? FlattenExpected<U, E>{std::forward<U>(next)} : unexpected();\n  }\n\n  template <class U>\n  constexpr FlattenExpected<U, E> substitute(U next) && {\n    return has_value() ? FlattenExpected<U, E>{std::forward<U>(next)} : std::move(unexpected());\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Substitute Error\n  ///-----------------------------------------------------------------------------------------------\n  template <class G>\n  constexpr Expected<ValueType<Derived>, G> substitute_error(G new_error) const& {\n    using R = Expected<ValueType<Derived>, G>;\n    return is_error_ ? R{Unexpected<G>{new_error}} : R{*ValuePointer<T>(buffer_)};\n  }\n\n  template <class G>\n  constexpr Expected<ValueType<Derived>, G> substitute_error(G new_error) && {\n    using R = Expected<ValueType<Derived>, G>;\n    return is_error_ ? R{Unexpected<G>{new_error}} : R{std::move(*ValuePointer<T>(buffer_))};\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Unary Free Functor Map\n  ///-----------------------------------------------------------------------------------------------\n  // If in error the error is returned, otherwise the value is mapped with the given functor and\n  // the result is returned.\n  template <class F>\n  constexpr EnableIfMappable_t<F, Derived&> map(F&& func) & {\n    return has_value() ? FunctorMap(std::forward<F>(func), *derived()) : unexpected();\n  }\n\n  template <class F>\n  constexpr EnableIfMappable_t<F, const Derived&> map(F&& func) const& {\n    return has_value() ? FunctorMap(std::forward<F>(func), *derived()) : unexpected();\n  }\n\n  template <class F>\n  constexpr EnableIfMappable_t<F, Derived&&> map(F&& func) && {\n    return has_value() ? FunctorMap(std::forward<F>(func), std::move(*derived()))\n                       : std::move(unexpected());\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Unary Free Functor Map Error\n  ///-----------------------------------------------------------------------------------------------\n  // If has value the value is returned, otherwise the error is mapped with the given functor and\n  // the result is returned.\n  template <class F>\n  constexpr EnableIfMappableError_t<F, Derived&> map_error(F&& func) & {\n    using R = Expected<ValueType<Derived>, ErrorResultType<F, Derived&>>;\n    return is_error_ ? FunctorMapError(std::forward<F>(func), *derived())\n                     : R{*ValuePointer<T>(buffer_)};\n  }\n\n  template <class F>\n  constexpr EnableIfMappableError_t<F, const Derived&> map_error(F&& func) const& {\n    using R = Expected<ValueType<Derived>, ErrorResultType<F, const Derived&>>;\n    return is_error_ ? FunctorMapError(std::forward<F>(func), *derived())\n                     : R{*ValuePointer<T>(buffer_)};\n  }\n\n  template <class F>\n  constexpr EnableIfMappableError_t<F, Derived&&> map_error(F&& func) && {\n    using R = Expected<ValueType<Derived>, ErrorResultType<F, Derived&&>>;\n    return is_error_ ? FunctorMapError(std::forward<F>(func), std::move(*derived()))\n                     : R{std::move(*ValuePointer<T>(buffer_))};\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Nonary Free Functor Map\n  ///-----------------------------------------------------------------------------------------------\n  // If in error the error is returned, otherwise the given functor is called with no arguments and\n  // the result is returned.\n  template <class F>\n  constexpr EnableIfNonaryMappable_t<F, const Derived&> map(F&& func) const& {\n    return has_value() ? FunctorMap<E>(std::forward<F>(func)) : unexpected();\n  }\n\n  template <class F>\n  constexpr EnableIfNonaryMappable_t<F, const Derived&&> map(F&& func) && {\n    return has_value() ? FunctorMap<E>(std::forward<F>(func)) : std::move(unexpected());\n  }\n\n  template <class F>\n  constexpr FlattenExpected<ResultType<F>, E> and_then(F&& func) const& {\n    return has_value() ? FunctorMap<E>(std::forward<F>(func)) : unexpected();\n  }\n  template <class F>\n  constexpr FlattenExpected<ResultType<F>, E> and_then(F&& func) && {\n    return has_value() ? FunctorMap<E>(std::forward<F>(func)) : std::move(unexpected());\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Nonary Free Functor Map Error\n  ///-----------------------------------------------------------------------------------------------\n  // If has value the value is returned, otherwise the given functor is called with no arguments and\n  // the result is returned.\n  template <class F>\n  constexpr EnableIfNonaryMappableError_t<F, const Derived&> map_error(F&& func) const& {\n    static_assert(IsErrorCallable_v<F>);\n    static_assert(!IsErrorCallable_v<F, const Derived&>);\n    using R = Expected<ValueType<Derived>, ErrorResultType<F>>;\n    return is_error_ ? FunctorMapError<T>(std::forward<F>(func)) : R{*ValuePointer<T>(buffer_)};\n  }\n\n  template <class F>\n  constexpr EnableIfNonaryMappableError_t<F, const Derived&&> map_error(F&& func) && {\n    using R = Expected<ValueType<Derived>, ErrorResultType<F>>;\n    return is_error_ ? FunctorMapError<T>(std::forward<F>(func))\n                     : R{std::move(*ValuePointer<T>(buffer_))};\n  }\n\n  template <class F>\n  constexpr Expected<ValueType<Derived>, ErrorResultType<F>> and_then_error(F&& func) const& {\n    using R = Expected<ValueType<Derived>, ErrorResultType<F>>;\n    return is_error_ ? FunctorMapError<T>(std::forward<F>(func)) : R{*ValuePointer<T>(buffer_)};\n  }\n\n  template <class F>\n  constexpr Expected<ValueType<Derived>, ErrorResultType<F>> and_then_error(F&& func) && {\n    using R = Expected<ValueType<Derived>, ErrorResultType<F>>;\n    return is_error_ ? FunctorMapError<T>(std::forward<F>(func))\n                     : R{std::move(*ValuePointer<T>(buffer_))};\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Member Variable Map\n  ///-----------------------------------------------------------------------------------------------\n  template <class R, class U>\n  constexpr FlattenExpected<R&, E> map(R U::* ptr) & {\n    static_assert(IsSame_v<U, RemoveCVRef_t<ValueType<Derived>>>,\n        \"received pointer-to-member that is not of type T.\");\n    // NOTE: We explicitly declare the return type of the lambda to allow capturing of member by\n    // mutable reference, otherwise a lambda will always return a copy.\n    auto func = [&](ValueType<Derived>& a) -> decltype(auto) { return a.*ptr; };\n    return has_value() ? FunctorMap(func, *derived()) : unexpected();\n  }\n\n  template <class R, class U>\n  constexpr FlattenExpected<const R&, E> map(R U::* ptr) const& {\n    static_assert(IsSame_v<U, RemoveCVRef_t<ValueType<Derived>>>,\n        \"received pointer-to-member that is not of type T.\");\n    auto func = [&](const ValueType<Derived>& a) -> decltype(auto) { return a.*ptr; };\n    return has_value() ? FunctorMap(func, *derived()) : unexpected();\n  }\n\n  template <class R, class U>\n  constexpr FlattenExpected<Decay_t<R>, E> map(R U::* ptr) && {\n    static_assert(IsSame_v<U, RemoveCVRef_t<ValueType<Derived>>>,\n        \"received pointer-to-member that is not of type T.\");\n    // NOTE: there is a subtle distinction here. If we want to get access to a member variable\n    // from an rvalue context, we MUST make a copy, since we cannot guarantee the reference will\n    // still be valid after destruction of the temporary.\n    auto func = [&](ValueType<Derived>&& a) { return a.*ptr; };\n    return has_value() ? FunctorMap(func, std::move(*derived())) : unexpected();\n  }\n\n  template <class R, class U>\n  constexpr FlattenExpected<Decay_t<R>, E> map(R U::* ptr) const&& {\n    static_assert(IsSame_v<U, RemoveCVRef_t<ValueType<Derived>>>,\n        \"received pointer-to-member that is not of type T.\");\n    auto func = [&](const ValueType<Derived>&& a) { return a.*ptr; };\n    return has_value() ? FunctorMap(func, std::move(*derived())) : unexpected();\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Member Function Map\n  ///-----------------------------------------------------------------------------------------------\n  template <class R, class U, class... Args>\n  constexpr FlattenExpected<R, E> map(R (U::* ptr)(Args...), Args&&... args) {\n    static_assert(IsSame_v<U, RemoveCVRef_t<ValueType<Derived>>>,\n        \"received pointer-to-member that is not of type T.\");\n    auto func = [&](U& a) { return (a.*ptr)(std::forward<Args>(args)...); };\n    return has_value() ? FunctorMap(func, *derived()) : unexpected();\n  }\n\n  template <class R, class U, class... Args>\n  constexpr FlattenExpected<R, E> map(R (U::* ptr)(Args...) & , Args&&... args) & {\n    static_assert(IsSame_v<U, RemoveCVRef_t<ValueType<Derived>>>,\n        \"received pointer-to-member that is not of type T.\");\n    auto func = [&](U& a) { return (a.*ptr)(std::forward<Args>(args)...); };\n    return has_value() ? FunctorMap(func, *derived()) : unexpected();\n  }\n\n  template <class R, class U, class... Args>\n  constexpr FlattenExpected<R, E> map(R (U::* ptr)(Args...) &&, Args&&... args) && {\n    static_assert(IsSame_v<U, RemoveCVRef_t<ValueType<Derived>>>,\n        \"received pointer-to-member that is not of type T.\");\n    auto func = [&](U&& a) { return (std::move(a).*ptr)(std::forward<Args>(args)...); };\n    return has_value() ? FunctorMap(func, std::move(*derived())) : unexpected();\n  }\n\n  template <class R, class U, class... Args>\n  constexpr FlattenExpected<R, E> map(R (U::* ptr)(Args...) const, Args&&... args) const {\n    static_assert(IsSame_v<U, RemoveCVRef_t<ValueType<Derived>>>,\n        \"received pointer-to-member that is not of type T.\");\n    auto func = [&](const U& a) { return (a.*ptr)(std::forward<Args>(args)...); };\n    return has_value() ? FunctorMap(func, *derived()) : unexpected();\n  }\n\n  template <class R, class U, class... Args>\n  constexpr FlattenExpected<R, E> map(R (U::* ptr)(Args...) const&, Args&&... args) const& {\n    static_assert(IsSame_v<U, RemoveCVRef_t<ValueType<Derived>>>,\n        \"received pointer-to-member that is not of type T.\");\n    auto func = [&](const U& a) { return (a.*ptr)(std::forward<Args>(args)...); };\n    return has_value() ? FunctorMap(func, *derived()) : unexpected();\n  }\n\n  template <class R, class U, class... Args>\n  constexpr FlattenExpected<R, E> map(R (U::* ptr)(Args...) const&&, Args&&... args) const&& {\n    static_assert(IsSame_v<U, RemoveCVRef_t<ValueType<Derived>>>,\n        \"received pointer-to-member that is not of type T.\");\n    auto func = [&](const U&& a) { return (std::move(a).*ptr)(std::forward<Args>(args)...); };\n    return has_value() ? FunctorMap(func, std::move(*derived())) : unexpected();\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Assign Value\n  ///-----------------------------------------------------------------------------------------------\n  // Assigns the value of an expected directly to a concrete instance with perfect forwarding.\n  template <class U>\n  constexpr Expected<void, E> assign_to(U& value) const& {\n    static_assert(IsAssignable_v<U&, const ValueType<Derived>&>,\n                  \"Argument Type is not assignable with Expected Type (const T&)\");\n    auto assign = [&](const ValueType<Derived>& derived) { value = derived; };\n    return has_value() ? FunctorMap(assign, *derived()) : unexpected();\n  }\n\n  template <class U>\n  constexpr Expected<void, E> assign_to(U& value) && {\n    static_assert(IsAssignable_v<U&, ValueType<Derived>&&>,\n                  \"Argument Type is not assignable with Expected Type (T&&)\");\n    auto move_assign = [&](ValueType<Derived>&& derived) { value = std::move(derived); };\n    return has_value() ? FunctorMap(move_assign, std::move(*derived())) : std::move(unexpected());\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  ///  Guard\n  ///-----------------------------------------------------------------------------------------------\n\n  template <class P>\n  constexpr Expected<ValueType<Derived>, E> guard(P&& pred, E&& new_error) const& {\n    if (is_error_) { return unexpected(); }\n    return pred(derived()->value()) ? *derived() : Unexpected<E>{new_error};\n  }\n\n  template <class P>\n  constexpr Expected<ValueType<Derived>, E> guard(P&& pred, E&& new_error) && {\n    if (is_error_) { return std::move(unexpected()); }\n    return pred(derived()->value()) ? std::move(*derived()) : Unexpected<E>{new_error};\n  }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Ignore Error\n  ///-----------------------------------------------------------------------------------------------\n\n  template <class U>\n  constexpr Expected<ValueType<Derived>, E> ignore_error(U&& next) const& {\n    return has_value() ? *derived() : std::forward<U>(next);\n  }\n\n  template <class U>\n  constexpr Expected<ValueType<Derived>, E> ignore_error(U&& next) && {\n    return has_value() ? std::move(*derived()) : std::forward<U>(next);\n  }\n\n  constexpr Expected<void, E> ignore_error() const { return {}; }\n\n protected:\n  // Allow other Expected classes access to `unexpected` getters.\n  template <class, class, class>\n  friend class ExpectedBase;\n\n  // Protected default constructor so derived types can write custom initializers.\n  ExpectedBase() = default;\n\n  // Protected Value constructor so Derived types can convert to stored types\n  template <class... Args, EnableIf_t<IsConstructible_v<T, Args...>, void*> = nullptr>\n  constexpr ExpectedBase(Args&&... args) {\n    constructValueFrom(std::forward<Args>(args)...);\n  }\n\n  // Unexpected accessors to skip extra construction calls when not needed.\n  constexpr const Unexpected<E>& unexpected() const& {\n    GXF_ASSERT(is_error_, \"Expected does not have an error. Check before accessing.\");\n    return *ValuePointer<Unexpected<E>>(buffer_);\n  }\n\n  constexpr Unexpected<E>& unexpected() & {\n    GXF_ASSERT(is_error_, \"Expected does not have an error. Check before accessing.\");\n    return *ValuePointer<Unexpected<E>>(buffer_);\n  }\n\n  constexpr Unexpected<E>&& unexpected() && {\n    GXF_ASSERT(is_error_, \"Expected does not have an error. Check before accessing.\");\n    return std::move(*ValuePointer<Unexpected<E>>(buffer_));\n  }\n\n  // Returns a pointer to the derived type for accessing value result in supported specializations.\n  constexpr const Derived* derived() const { return static_cast<const Derived*>(this); }\n  constexpr       Derived* derived()       { return static_cast<Derived*>(this); }\n\n  ///-----------------------------------------------------------------------------------------------\n  /// Construction Helpers\n  ///-----------------------------------------------------------------------------------------------\n  // Construct a new object in allocated buffer.\n  template <class U, class G, class D>\n  constexpr void constructFrom(const ExpectedBase<U, G, D>& other) {\n    other.has_value() ? constructValueFrom(other) : constructErrorFrom(other);\n  }\n\n  // Move construct a new object in allocated buffer.\n  template <class U, class G, class D>\n  constexpr void constructFrom(ExpectedBase<U, G, D>&& other) {\n    other.has_value() ? constructValueFrom(std::move(other)) : constructErrorFrom(std::move(other));\n  }\n\n  template <class U, class G, class D>\n  constexpr void constructErrorFrom(const ExpectedBase<U, G, D>& other) {\n    is_error_ = true;\n    InplaceCopyConstruct(buffer_, *ValuePointer<Unexpected<G>>(other.buffer_));\n  }\n\n  template <class U, class G, class D>\n  constexpr void constructErrorFrom(ExpectedBase<U, G, D>&& other) {\n    is_error_ = true;\n    InplaceMoveConstruct(buffer_, std::move(*ValuePointer<Unexpected<G>>(other.buffer_)));\n  }\n\n  template <class U, class G, class D>\n  constexpr void constructValueFrom(const ExpectedBase<U, G, D>& other) {\n    is_error_ = false;\n    InplaceCopyConstruct<T>(buffer_, *ValuePointer<U>(other.buffer_));\n  }\n\n  template <class U, class G, class D>\n  constexpr void constructValueFrom(ExpectedBase<U, G, D>&& other) {\n    is_error_ = false;\n    InplaceConstruct<T>(buffer_, std::move(*ValuePointer<U>(other.buffer_)));\n  }\n\n  template <class... Args>\n  void constructValueFrom(Args&&... args) {\n    is_error_ = false;\n    InplaceConstruct<T>(buffer_, std::forward<Args>(args)...);\n  }\n\n  // Call the destructor for the current object explicitly\n  constexpr void destruct() {\n    has_value() ? Destruct<T>(buffer_) : Destruct<Unexpected<E>>(buffer_);\n  }\n\n  bool is_error_ = true;\n  const char* error_message_{};\n  static constexpr uint32_t kAlign =\n      (alignof(Unexpected<E>) < alignof(T)) ? alignof(T) : alignof(Unexpected<E>);\n  static constexpr uint32_t kSize =\n      (sizeof(Unexpected<E>) < sizeof(T)) ? sizeof(T) : sizeof(Unexpected<E>);\n  alignas(kAlign) byte buffer_[kSize] = {0};\n};\n}  // namespace detail\n\n///-------------------------------------------------------------------------------------------------\n//  Expected Implementation\n///-------------------------------------------------------------------------------------------------\n// Default instance for Expected types.\ntemplate <class T, class E>\nclass Expected : public detail::ExpectedBase<T, E, Expected<T, E>> {\n public:\n  using detail::ExpectedBase<T, E, Expected>::ExpectedBase;\n\n  // Enable implicit construction of Move-Only types\n  constexpr Expected(T&& value) : detail::ExpectedBase<T, E, Expected>(std::move(value)) {}\n\n  template <class U, EnableIf_t<IsConstructible_v<T, U> && IsConvertible_v<U, T>>* = nullptr>\n  constexpr Expected(U&& value) : detail::ExpectedBase<T, E, Expected>(std::forward<U>(value)) {}\n\n  template <class U, EnableIf_t<IsConstructible_v<T, U> && !IsConvertible_v<U, T>>* = nullptr>\n  explicit constexpr Expected(U&& value)\n      : detail::ExpectedBase<T, E, Expected>(std::forward<U>(value)) {}\n\n  template <class... Args, EnableIf_t<IsConstructible_v<T, Args...>, void*> = nullptr>\n  explicit constexpr Expected(Args&&... args)\n      : detail::ExpectedBase<T, E, Expected>(std::forward<Args>(args)...) {}\n\n  template <class U, EnableIf_t<IsConstructible_v<T, U> && IsConvertible_v<U, T>>* = nullptr>\n  constexpr Expected(const Expected<U, E>& other) {\n    other.has_value() ? this->constructValueFrom(other.value()) : this->constructErrorFrom(other);\n  }\n\n  template <class U, EnableIf_t<IsConstructible_v<T, U> && !IsConvertible_v<U, T>>* = nullptr>\n  explicit constexpr Expected(const Expected<U, E>& other) {\n    other.has_value() ? this->constructValueFrom(other.value()) : this->constructErrorFrom(other);\n  }\n\n  template <class U, EnableIf_t<IsConstructible_v<T, U> && IsConvertible_v<U, T>>* = nullptr>\n  constexpr Expected(Expected<U, E>&& other) {\n    other.has_value() ? this->constructValueFrom(std::move(other.value()))\n                      : this->constructErrorFrom(std::move(other));\n  }\n\n  template <class U, EnableIf_t<IsConstructible_v<T, U> && !IsConvertible_v<U, T>>* = nullptr>\n  explicit constexpr Expected(Expected<U, E>&& other) {\n    other.has_value() ? this->constructValueFrom(std::move(other.value()))\n                      : this->constructErrorFrom(std::move(other));\n  }\n\n  template <class U = T, EnableIf_t<IsConstructible_v<T, U> && IsConvertible_v<U, T>>* = nullptr>\n  constexpr Expected& operator=(U&& value) {\n    this->destruct();\n    this->constructValueFrom(std::forward<U>(value));\n    return *this;\n  }\n\n  template <class... Args>\n  T& replace(Args&&... args) {\n    static_assert(IsConstructible_v<T, Args...>,\n        \"T cannot be constructed with the provide argument types.\");\n    this->destruct();\n    this->constructValueFrom(std::forward<Args>(args)...);\n    return value();\n  }\n\n  constexpr const T& value() const& {\n    GXF_ASSERT(this->has_value(), \"Expected does not have a value. Check before accessing.\");\n    return *ValuePointer<T>(this->buffer_);\n  }\n\n  constexpr T& value() & {\n    GXF_ASSERT(this->has_value(), \"Expected does not have a value. Check before accessing.\");\n    return *ValuePointer<T>(this->buffer_);\n  }\n\n  constexpr const T&& value() const&& {\n    GXF_ASSERT(this->has_value(), \"Expected does not have a value. Check before accessing.\");\n    return std::move(*ValuePointer<T>(this->buffer_));\n  }\n\n  constexpr T&& value() && {\n    GXF_ASSERT(this->has_value(), \"Expected does not have a value. Check before accessing.\");\n    return std::move(*ValuePointer<T>(this->buffer_));\n  }\n};\n\n///-------------------------------------------------------------------------------------------------\n///  Expected of Reference Implementation\n///-------------------------------------------------------------------------------------------------\n// Specialization of Expected for references.\ntemplate <class T, class E>\nclass Expected<T&, E> : public detail::ExpectedBase<T*, E, Expected<T&, E>> {\n private:\n  template <class, class = void>\n  struct IsExpectedConvertible : FalseType {};\n\n  template <class U>\n  struct IsExpectedConvertible<U, void_t<decltype(Declval<U>().value())>>\n      : IsConvertible<decltype(Declval<U>().value()), T&> {};\n\n  template <class U>\n  static constexpr bool IsExpectedConvertible_v =\n      detail::IsExpected_v<U> && IsExpectedConvertible<U>::value;\n\n public:\n  using detail::ExpectedBase<T*, E, Expected>::ExpectedBase;\n\n  template <class U, EnableIf_t<IsConvertible_v<U, T&>, void*> = nullptr>\n  constexpr Expected(U&& value) : detail::ExpectedBase<T*, E, Expected>(&std::forward<U>(value)) {}\n\n  template <class U, EnableIf_t<IsExpectedConvertible_v<U>, void*> = nullptr>\n  constexpr Expected(U&& other) {\n    other.has_value() ? this->constructValueFrom(&std::forward<U>(other).value())\n                      : this->constructErrorFrom(std::forward<U>(other));\n  }\n\n  constexpr T& value() const {\n    GXF_ASSERT(this->has_value(), \"Expected does not have a value. Check before accessing.\");\n    return **ValuePointer<T*>(this->buffer_);\n  }\n};\n///-------------------------------------------------------------------------------------------------\n///  Expected of Void Implementation\n///-------------------------------------------------------------------------------------------------\n// Specialization of Expected for void. This is essentially an std::optional<E>.\ntemplate <class E>\nclass Expected<void, E> : public detail::ExpectedBase<detail::Unit, E, Expected<void, E>> {\n public:\n  using detail::ExpectedBase<detail::Unit, E, Expected>::ExpectedBase;\n\n  constexpr Expected() : detail::ExpectedBase<detail::Unit, E, Expected>(detail::Unit{}) {}\n\n  // Constructor that allows assigning an Expected with the same error type to an Expected<void>\n  template <class U>\n  explicit constexpr Expected(const Expected<U, E>& other) {\n    other.has_value() ? this->constructValueFrom() : this->constructErrorFrom(other);\n  }\n\n  // Constructor that allows assigning an Expected with the same error type to an Expected<void>\n  template <class U>\n  explicit constexpr Expected(Expected<U, E>&& other) {\n    other.has_value() ? this->constructValueFrom() : this->constructErrorFrom(std::move(other));\n  }\n\n  // Combines two expected value taking the first error.\n  template <class U>\n  Expected& operator&=(const Expected<U, E>& other) {\n    *this = *this & other;\n    return *this;\n  }\n\n  // Combines two expected value taking the last success.\n  template <class U>\n  Expected& operator|=(const Expected<U, E>& other) {\n    *this = *this | other;\n    return *this;\n  }\n};\n\n///-------------------------------------------------------------------------------------------------\n///  Utility Functions\n///-------------------------------------------------------------------------------------------------\n// Checks if all Expected values are not in error. If in error, return the first error found.\ntemplate <class E, class T>\nExpected<void, E> AllOf(const Expected<T, E>& expected) {\n  return Expected<void, E>{expected};\n}\n\n// Checks if all Expected values are not in error. If in error, return the first error found.\ntemplate <class E, class T, class... Ts>\nExpected<void, E> AllOf(const Expected<T, E>& expected, const Expected<Ts, E>&... others) {\n  return expected & AllOf(others...);\n}\n\n// Checks if all the Arguments are of type Expected<T, E>. If so, unwraps them and calls\n// the function object F with the unwrapped arguments. Otherwise, return an Unexpected with\n// the first error value found in the argument list.\ntemplate <class F, class... Args>\nauto Apply(F&& func, Args&&... args) -> EnableIf_t<\n    Conjunction_v<detail::IsExpected<Args>...>, decltype(detail::FunctorMap(func, args...)) > {\n  const auto all_valid = AllOf(std::forward<Args>(args)...);\n  if (!all_valid) { return Unexpected<detail::ErrorType<Args...>>(all_valid.error()); }\n  return detail::FunctorMap(std::forward<F>(func), std::forward<Args>(args)...);\n}\n\n///-------------------------------------------------------------------------------------------------\n///  Comparison Operators\n///-------------------------------------------------------------------------------------------------\ntemplate <class T, class E, class U, class G>\nconstexpr bool operator==(const Expected<T, E>& lhs, const Expected<U, G>& rhs) {\n  return ( lhs &&  rhs && (lhs.value() == rhs.value())) ||\n         (!lhs && !rhs && (lhs.error() == rhs.error()));\n}\ntemplate <class T, class E, class U>\nconstexpr bool operator==(const Expected<T, E>& lhs, const U& rhs) {\n  return lhs && (*lhs == rhs);\n}\ntemplate <class T, class E, class U>\nconstexpr bool operator==(const U& lhs, const Expected<T, E>& rhs) {\n  return rhs && (lhs == *rhs);\n}\ntemplate <class T, class E>\nconstexpr bool operator==(const Expected<T, E>& lhs, const Unexpected<E>& rhs) {\n  return !lhs && (lhs.error() == rhs.value());\n}\ntemplate <class T, class E>\nconstexpr bool operator==(const Unexpected<E>& lhs, const Expected<T, E>& rhs) {\n  return !rhs && (lhs.value() == rhs.error());\n}\ntemplate <class E>\nconstexpr bool operator==(const Unexpected<E>& lhs, const Unexpected<E>& rhs) {\n  return lhs.value() == rhs.value();\n}\n\n// All inequality operators are defined in terms of negated equality\ntemplate <class T, class E, class U, class G>\nconstexpr bool operator!=(const Expected<T, E>& lhs, const Expected<U, G>& rhs) {\n  return !(lhs == rhs);\n}\ntemplate <class T, class E, class U>\nconstexpr bool operator!=(const Expected<T, E>& lhs, const U& rhs) {\n  return !(lhs == rhs);\n}\ntemplate <class T, class E, class U>\nconstexpr bool operator!=(const U& lhs, const Expected<T, E>& rhs) {\n  return !(lhs == rhs);\n}\ntemplate <class E>\nconstexpr bool operator!=(const Unexpected<E>& lhs, const Unexpected<E>& rhs) {\n  return !(lhs == rhs);\n}\n\n///-------------------------------------------------------------------------------------------------\n///  Binary Logical Operators\n///-------------------------------------------------------------------------------------------------\n// Combines two expected value taking the first error.\ntemplate <class E, class T, class U>\nExpected<void, E> operator&(const Expected<T, E>& lhs, const Expected<U, E>& rhs) {\n  return !lhs ? Expected<void, E>{lhs} : Expected<void, E>{rhs};\n}\n\n// Combines two expected value taking the last success.\ntemplate <class E, class T, class U>\nExpected<void, E> operator|(const Expected<T, E>& lhs, const Expected<U, E>& rhs) {\n  return rhs ? Expected<void, E>{rhs} : Expected<void, E>{lhs};\n}\n\n///-------------------------------------------------------------------------------------------------\n///  Binary Arithmetic Operators\n///-------------------------------------------------------------------------------------------------\n// Arithmetic operator+ for values stored in expected values. Returns either the computed value\n// or the first unexpected among lhs and rhs.\ntemplate <class V, class E>\nExpected<V, E> operator+(const Expected<V, E>& lhs, const Expected<V, E>& rhs) {\n  return Apply([](const V& lhs, const V& rhs) { return lhs + rhs; }, lhs, rhs);\n}\n\ntemplate <class V, class E>\nExpected<V, E> operator+(const Expected<V, E>& lhs, const V& rhs) {\n  return lhs.map([&](const V& value) { return value + rhs; });\n}\n\ntemplate <class V, class E>\nExpected<V, E> operator+(const V& lhs, const Expected<V, E>& rhs) {\n  return rhs.map([&](const V& value) { return lhs + value; });\n}\n\n// Arithmetic operator- for values stored in expected values. Returns either the computed value\n// or the first unexpected among lhs and rhs.\ntemplate <class V, class E>\nExpected<V, E> operator-(const Expected<V, E>& lhs, const Expected<V, E>& rhs) {\n  return Apply([](const V& lhs, const V& rhs) { return lhs - rhs; }, lhs, rhs);\n}\n\ntemplate <class V, class E>\nExpected<V, E> operator-(const Expected<V, E>& lhs, const V& rhs) {\n  return lhs.map([&](const V& value) { return value - rhs; });\n}\n\ntemplate <class V, class E>\nExpected<V, E> operator-(const V& lhs, const Expected<V, E>& rhs) {\n  return rhs.map([&](const V& value) { return lhs - value; });\n}\n\n// Arithmetic operator* for values stored in expected values. Returns either the computed value\n// or the first unexpected among lhs and rhs.\ntemplate <class V, class E>\nExpected<V, E> operator*(const Expected<V, E>& lhs, const Expected<V, E>& rhs) {\n  return Apply([](const V& lhs, const V& rhs) { return lhs * rhs; }, lhs, rhs);\n}\n\ntemplate <class V, class E>\nExpected<V, E> operator*(const Expected<V, E>& lhs, const V& rhs) {\n  return lhs.map([&](const V& value) { return value * rhs; });\n}\n\ntemplate <class V, class E>\nExpected<V, E> operator*(const V& lhs, const Expected<V, E>& rhs) {\n  return rhs.map([&](const V& value) { return lhs * value; });\n}\n\n// Arithmetic operator/ for values stored in expected values. Returns either the computed value\n// or the first unexpected among lhs and rhs.\ntemplate <class V, class E>\nExpected<V, E> operator/(const Expected<V, E>& lhs, const Expected<V, E>& rhs) {\n  return Apply([](const V& lhs, const V& rhs) { return lhs / rhs; }, lhs, rhs);\n}\n\ntemplate <class V, class E>\nExpected<V, E> operator/(const Expected<V, E>& lhs, const V& rhs) {\n  return lhs.map([&](const V& value) { return value / rhs; });\n}\n\ntemplate <class V, class E>\nExpected<V, E> operator/(const V& lhs, const Expected<V, E>& rhs) {\n  return rhs.map([&](const V& value) { return lhs / value; });\n}\n\n// Arithmetic operator% for values stored in expected values. Returns either the computed value\n// or the first unexpected among lhs and rhs.\ntemplate <class V, class E>\nExpected<V, E> operator%(const Expected<V, E>& lhs, const Expected<V, E>& rhs) {\n  return Apply([](const V& lhs, const V& rhs) { return lhs % rhs; }, lhs, rhs);\n}\n\ntemplate <class V, class E>\nExpected<V, E> operator%(const Expected<V, E>& lhs, const V& rhs) {\n  return lhs.map([&](const V& value) { return value % rhs; });\n}\n\ntemplate <class V, class E>\nExpected<V, E> operator%(const V& lhs, const Expected<V, E>& rhs) {\n  return rhs.map([&](const V& value) { return lhs % value; });\n}\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_COMMON_EXPECTED_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/fixed_map.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_COMMON_FIXED_MAP_HPP_\n#define NVIDIA_GXF_COMMON_FIXED_MAP_HPP_\n\n#include <iterator>\n#include <utility>\n\n#include \"common/expected.hpp\"\n#include \"common/memory_utils.hpp\"\n#include \"common/type_utils.hpp\"\n\nnamespace nvidia {\n\n// Data structure that provides similar functionality to std::unordered_map but does not dynamically\n// reallocate memory and uses Expected type for error handling instead of exceptions.\n// This container is not thread-safe.\ntemplate <typename Key, typename T, typename Hash = std::hash<Key>>\nclass FixedMap {\n public:\n  template <typename TContainer, typename TValue = typename TContainer::value_type>\n  class Iterator;\n  template <typename TIterator>\n  class ReverseIterator;\n  template <typename TContainer>\n  using ConstIterator = Iterator<const TContainer, const typename TContainer::value_type>;\n\n  // Use STL naming convention for compatibility with STL algorithms\n  using key_type               = Key;\n  using mapped_type            = T;\n  using value_type             = std::pair<const key_type, mapped_type>;\n  using size_type              = size_t;\n  using iterator               = Iterator<FixedMap>;\n  using reverse_iterator       = ReverseIterator<iterator>;\n  using const_iterator         = ConstIterator<FixedMap>;\n  using const_reverse_iterator = ReverseIterator<const_iterator>;\n\n  // Custom error codes for map\n  enum struct Error {\n    kOutOfMemory,      // Memory allocation failed\n    kInvalidKey,       // Key is not in container\n    kDuplicateKey,     // Key is already in container\n    kContainerFull,    // Container is full\n    kInvalidIterator,  // Iterator is invalid\n  };\n\n  // Expected type which uses class specific errors\n  template <typename U>\n  using Expected = Expected<U, Error>;\n\n  template <typename TContainer, typename TValue>\n  class Iterator {\n   public:\n    static_assert(IsSame_v<RemoveConst_t<TContainer>, FixedMap>);\n\n    // iterator traits\n    using iterator_category = std::bidirectional_iterator_tag;\n    using value_type = TValue;\n    using difference_type = std::ptrdiff_t;\n    using pointer = TValue*;\n    using reference = TValue&;\n\n    constexpr Iterator() : container_{nullptr}, index_{-1} {}\n    constexpr Iterator(TContainer& container, size_t start) : container_{&container}, index_{0} {\n      *this += start;\n    }\n\n    constexpr Iterator(const Iterator& other) = default;\n    constexpr Iterator(Iterator&& other) = default;\n    constexpr Iterator& operator=(const Iterator& other) = default;\n    constexpr Iterator& operator=(Iterator&& other) = default;\n\n    constexpr Expected<TValue&> operator*() const {\n      if (container_ == nullptr || container_->data_ == nullptr ||\n          index_ < 0 || index_ >= static_cast<difference_type>(container_->capacity_)) {\n        return Unexpected<Error>{Error::kInvalidIterator};\n      }\n      Bucket& bucket = container_->data_[index_];\n      if (!bucket.occupied) {\n        return Unexpected<Error>{Error::kInvalidIterator};\n      }\n      return bucket.value;\n    }\n\n    constexpr Iterator& operator+=(difference_type offset) {\n      if (container_ == nullptr || container_->data_ == nullptr) {\n        return *this;\n      }\n\n      while (true) {\n        const Bucket& bucket = container_->data_[index_];\n        if (bucket.occupied) {\n          break;\n        }\n        index_++;\n        if (index_ >= static_cast<difference_type>(container_->capacity_)) {\n          break;\n        }\n      }\n\n      if ((offset > 0 && index_ >= static_cast<difference_type>(container_->capacity_)) ||\n          (offset < 0 && index_ <= 0)) {\n        return *this;\n      }\n\n      while (offset > 0) {\n        index_++;\n        if (index_ >= static_cast<difference_type>(container_->capacity_)) {\n          break;\n        }\n        const Bucket& bucket = container_->data_[index_];\n        if (bucket.occupied) {\n          offset--;\n        }\n      }\n\n      while (offset < 0) {\n        index_--;\n        if (index_ <= 0) {\n          break;\n        }\n        const Bucket& bucket = container_->data_[index_];\n        if (bucket.occupied) {\n          offset++;\n        }\n      }\n\n      return *this;\n    }\n    constexpr Iterator& operator++() {\n      *this += 1;\n      return *this;\n    }\n    constexpr Iterator operator++(int) {\n      Iterator iter = *this;\n      ++(*this);\n      return iter;\n    }\n    constexpr Iterator& operator-=(difference_type offset) {\n      *this += -offset;\n      return *this;\n    }\n    constexpr Iterator& operator--() {\n      *this -= 1;\n      return *this;\n    }\n    constexpr Iterator operator--(int) {\n      Iterator iter = *this;\n      --(*this);\n      return iter;\n    }\n\n    friend constexpr Iterator operator+(Iterator a, difference_type n) {\n      a += n;\n      return a;\n    }\n    friend constexpr Iterator operator+(difference_type n, Iterator a) {\n      return a + n;\n    }\n    friend constexpr Iterator operator-(Iterator a, difference_type n) {\n      a -= n;\n      return a;\n    }\n    friend constexpr difference_type operator-(const Iterator& a, const Iterator& b) {\n      return a.index_ - b.index_;\n    }\n    friend constexpr bool operator==(const Iterator& a, const Iterator& b) {\n      return a.container_ == b.container_ && a.index_ == b.index_;\n    }\n    friend constexpr bool operator!=(const Iterator& a, const Iterator& b) {\n      return !(a == b);\n    }\n\n   private:\n    // Container pointer\n    TContainer* container_;\n    // Iterator index\n    difference_type index_;\n  };\n\n  template <typename TIterator>\n  class ReverseIterator {\n   public:\n    static_assert(IsSame_v<TIterator, iterator> || IsSame_v<TIterator, const_iterator>);\n\n    using TValue = typename TIterator::value_type;\n    // iterator traits\n    using iterator_category = std::bidirectional_iterator_tag;\n    using value_type = TValue;\n    using difference_type = std::ptrdiff_t;\n    using pointer = TValue*;\n    using reference = TValue&;\n\n    constexpr explicit ReverseIterator() : iter_{} {}\n    constexpr explicit ReverseIterator(TIterator iter) : iter_{iter} {}\n\n    constexpr ReverseIterator(const ReverseIterator& other) = default;\n    constexpr ReverseIterator(ReverseIterator&& other) = default;\n    constexpr ReverseIterator& operator=(const ReverseIterator& other) = default;\n    constexpr ReverseIterator& operator=(ReverseIterator&& other) = default;\n\n    constexpr TIterator base() const { return iter_; }\n\n    constexpr Expected<TValue&> operator*() const { return *std::prev(iter_); }\n\n    constexpr ReverseIterator& operator+=(difference_type offset) {\n      iter_ -= offset;\n      return *this;\n    }\n    constexpr ReverseIterator& operator++() {\n      iter_ -= 1;\n      return *this;\n    }\n    constexpr ReverseIterator operator++(int) {\n      ReverseIterator iter = *this;\n      ++(*this);\n      return iter;\n    }\n    constexpr ReverseIterator& operator-=(difference_type offset) {\n      iter_ += offset;\n      return *this;\n    }\n    constexpr ReverseIterator& operator--() {\n      iter_ += 1;\n      return *this;\n    }\n    constexpr ReverseIterator operator--(int) {\n      ReverseIterator iter = *this;\n      --(*this);\n      return iter;\n    }\n\n    friend constexpr ReverseIterator operator+(ReverseIterator a, difference_type n) {\n      return ReverseIterator(a.iter_ - n);\n    }\n    friend constexpr ReverseIterator operator+(difference_type n, ReverseIterator a) {\n      return ReverseIterator(a.iter_ - n);\n    }\n    friend constexpr ReverseIterator operator-(ReverseIterator a, difference_type n) {\n      return ReverseIterator(a.iter_ + n);\n    }\n    friend constexpr difference_type operator-(const ReverseIterator& a, const ReverseIterator& b) {\n      return b.iter_ - a.iter_;\n    }\n    friend constexpr bool operator==(const ReverseIterator& a, const ReverseIterator& b) {\n      return a.iter_ == b.iter_;\n    }\n    friend constexpr bool operator!=(const ReverseIterator& a, const ReverseIterator& b) {\n      return !(a == b);\n    }\n\n   private:\n    /// Random-access iterator\n    TIterator iter_;\n  };\n\n  FixedMap() : data_{nullptr}, capacity_{0}, size_{0} {}\n  FixedMap(const FixedMap& other) = delete;\n  FixedMap(FixedMap&& other) : data_{nullptr}, capacity_{0}, size_{0} { *this = std::move(other); }\n  FixedMap& operator=(const FixedMap& other) = delete;\n  FixedMap& operator=(FixedMap&& other) {\n    if (this != &other) {\n      std::swap(data_, other.data_);\n      std::swap(capacity_, other.capacity_);\n      std::swap(size_, other.size_);\n    }\n    return *this;\n  }\n  ~FixedMap() {\n    clear();\n    DeallocateArray<Bucket>(data_);\n  }\n\n  bool operator==(const FixedMap& other) const {\n    if ((capacity_ != other.capacity_) || (size_ != other.size_)) {\n      return false;\n    }\n    for (size_type i = 0; i < capacity_; i++) {\n      if (data_[i].occupied) {\n        if (!other.data_[i].occupied || (data_[i].value != other.data_[i].value)) {\n          return false;\n        }\n      }\n    }\n    return true;\n  }\n  bool operator!=(const FixedMap& other) const { return !(*this == other); }\n\n  constexpr iterator         begin()  { return iterator(*this, 0); }\n  constexpr iterator         end()    { return iterator(*this, size_); }\n  constexpr reverse_iterator rbegin() { return reverse_iterator(end()); }\n  constexpr reverse_iterator rend()   { return reverse_iterator(begin()); }\n\n  constexpr const_iterator         begin()  const { return cbegin(); }\n  constexpr const_iterator         end()    const { return cend(); }\n  constexpr const_reverse_iterator rbegin() const { return crbegin(); }\n  constexpr const_reverse_iterator rend()   const { return crend(); }\n\n  constexpr const_iterator         cbegin()  const { return const_iterator(*this, 0); }\n  constexpr const_iterator         cend()    const { return const_iterator(*this, size_); }\n  constexpr const_reverse_iterator crbegin() const { return const_reverse_iterator(cend()); }\n  constexpr const_reverse_iterator crend()   const { return const_reverse_iterator(cbegin()); }\n\n  Expected<mapped_type&> operator[](const key_type& key) {\n    Bucket* bucket = findMatchingBucket(key);\n    if (!bucket) {\n      auto result = emplace(std::make_pair(key, mapped_type()));\n      if (!result) {\n        return Unexpected<Error>{result.error()};\n      }\n      bucket = findMatchingBucket(key);\n      if (!bucket) {\n        return Unexpected<Error>{Error::kInvalidKey};\n      }\n    }\n    return bucket->value.second;\n  }\n\n  // Returns the number of elements the container can currently hold\n  size_type capacity() const { return capacity_; }\n  // Returns the number of elements in the container\n  size_type size() const { return size_; }\n  // Returns true if the container has no elements\n  bool empty() const { return size_ == 0; }\n  // Returns true if the container has reached capacity\n  bool full() const { return size_ == capacity_; }\n  // Returns the load factor of the container\n  double load_factor() const {\n    return capacity_ > 0 ? static_cast<double>(size_) / static_cast<double>(capacity_) : 0.0;\n  }\n\n  // Checks if the container has a value for the given key\n  bool contains(const key_type& key) const { return findMatchingBucket(key) != nullptr; }\n\n  // Returns a reference to the value at the given key\n  Expected<mapped_type&> at(const key_type& key) {\n    Bucket* bucket = findMatchingBucket(key);\n    if (!bucket) {\n      return Unexpected<Error>{Error::kInvalidKey};\n    }\n    return bucket->value.second;\n  }\n\n  // Returns a read-only reference to the value with the given key\n  Expected<const mapped_type&> at(const key_type& key) const {\n    Bucket* bucket = findMatchingBucket(key);\n    if (!bucket) {\n      return Unexpected<Error>{Error::kInvalidKey};\n    }\n    return bucket->value.second;\n  }\n\n  // Update a key that is already in the map\n  Expected<void> update(const key_type& key, const mapped_type& value) {\n    Bucket* bucket = findMatchingBucket(key);\n    if (!bucket) { return Unexpected<Error>{Error::kInvalidKey}; }\n    bucket->value.second = value;\n    return kSuccess;\n  }\n\n  // Assigns an the value to the given key if it exists, else creates a new object\n  template <typename... Args>\n  Expected<void> insert_or_assign(Args&&... args) {\n    value_type value(std::forward<Args>(args)...);\n    Bucket* bucket = findMatchingBucket(value.first);\n    if (bucket) {\n      bucket->value.second = value.second;\n      return kSuccess;\n    } else {\n      return emplace(std::move(value));\n    }\n  }\n\n  // Creates a new object with the provided arguments and adds it using the given key\n  template <typename... Args>\n  Expected<void> emplace(Args&&... args) {\n    value_type value(std::forward<Args>(args)...);\n    if (findMatchingBucket(value.first)) {\n      return Unexpected<Error>{Error::kDuplicateKey};\n    }\n    Bucket* bucket = findEmptyBucket(value.first);\n    if (!bucket) {\n      return Unexpected<Error>{Error::kContainerFull};\n    }\n    InplaceMoveConstruct<value_type>(BytePointer(&bucket->value), std::move(value));\n    bucket->occupied = true;\n    size_++;\n    return kSuccess;\n  }\n\n  // Copies the object with the given key\n  Expected<void> insert(const value_type& value) { return emplace(value); }\n  // Moves the object with the given key\n  Expected<void> insert(value_type&& value) {\n    return emplace(std::forward<value_type>(value));\n  }\n\n  // Removes the object at the given key and destroys it\n  Expected<void> erase(const key_type& key) {\n    Bucket* bucket = findMatchingBucket(key);\n    if (!bucket) {\n      return Unexpected<Error>{Error::kInvalidKey};\n    }\n    Destruct<value_type>(BytePointer(&bucket->value));\n    bucket->occupied = false;\n    size_--;\n    return kSuccess;\n  }\n\n  // Removes all objects from the vector\n  void clear() {\n    for (size_type i = 0; i < capacity_; i++) {\n      Bucket* bucket = &data_[i];\n      if (bucket->occupied) {\n        Destruct<value_type>(BytePointer(&bucket->value));\n        bucket->occupied = false;\n      }\n    }\n    size_ = 0;\n  }\n\n  // Allocates memory to hold the specified number of elements\n  Expected<void> reserve(size_type capacity) {\n    if (capacity > capacity_) {\n      Bucket* const temp_data = data_;\n      const size_type temp_capacity = capacity_;\n      const size_type temp_size = size_;\n\n      data_ = AllocateArray<Bucket>(capacity);\n      capacity_ = capacity;\n      size_ = 0;\n\n      if (data_ == nullptr) {\n        data_ = temp_data;\n        capacity_ = temp_capacity;\n        size_ = temp_size;\n        return Unexpected<Error>{Error::kOutOfMemory};\n      }\n\n      for (size_type i = 0; i < capacity_; i++) {\n        data_[i].occupied = false;\n      }\n\n      for (size_type i = 0; i < temp_capacity; i++) {\n        Bucket* bucket = &temp_data[i];\n        if (bucket->occupied) {\n          auto result = emplace(std::move(bucket->value));\n          if (!result) {\n            return Unexpected<Error>{result.error()};\n          }\n        }\n      }\n\n      DeallocateArray<Bucket>(temp_data);\n    }\n\n    return kSuccess;\n  }\n\n  // Copies the contents of the given map\n  // Current contents are discarded\n  // Fails if given map is larger than current capacity\n  Expected<void> copy_from(const FixedMap& other) {\n    if (other.size_ > capacity_) {\n      return Unexpected<Error>{Error::kContainerFull};\n    }\n    clear();\n    for (size_type i = 0; i < other.capacity_; i++) {\n      Bucket* bucket = &other.data_[i];\n      if (bucket->occupied) {\n        auto result = emplace(bucket->value.first, bucket->value.second);\n        if (!result) {\n          return Unexpected<Error>{result.error()};\n        }\n      }\n    }\n    return kSuccess;\n  }\n\n private:\n  // Container for a key-value pair\n  struct Bucket {\n    value_type value;\n    bool occupied;\n  };\n\n  // Searches for a matching bucket with the given key\n  Bucket* findMatchingBucket(const key_type& key) const {\n    if (capacity_ == 0) {\n      return nullptr;\n    }\n\n    size_type index = hash(key);\n    const size_type start = index;\n    Bucket* bucket = &data_[index];\n\n    // Look for a matching bucket\n    while (bucket->occupied) {\n      if (bucket->value.first == key) {\n        return bucket;\n      }\n\n      index = (index + 1) % capacity_;\n      bucket = &data_[index];\n\n      if (index == start) {\n        return nullptr;\n      }\n    }\n\n    return nullptr;\n  }\n\n  // Searches for an empty bucket with the given key\n  Bucket* findEmptyBucket(const key_type& key) const {\n    if (capacity_ == 0) {\n      return nullptr;\n    }\n\n    size_type index = hash(key);\n    const size_type start = index;\n    Bucket* bucket = &data_[index];\n\n    // Look for an empty bucket\n    while (bucket->occupied) {\n      index = (index + 1) % capacity_;\n      bucket = &data_[index];\n\n      if (index == start) {\n        return nullptr;\n      }\n    }\n\n    return bucket;\n  }\n\n  // Computes a hash of the given key that can be used as a table index\n  size_type hash(const key_type& key) const { return Hash{}(key) % capacity_; }\n\n  // Special value for returning a success\n  const Expected<void> kSuccess{};\n\n  // Pointer to an array of entries\n  Bucket* data_;\n  // Maximum number of entries the container can hold\n  size_type capacity_;\n  // Number of entries stored\n  size_type size_;\n};\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_COMMON_FIXED_MAP_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/fixed_string.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_COMMON_FIXED_STRING_HPP_\n#define NVIDIA_GXF_COMMON_FIXED_STRING_HPP_\n\n#include <algorithm>\n#include <cstring>\n\n#include \"common/expected.hpp\"\n#include \"common/iterator.hpp\"\n#include \"common/type_utils.hpp\"\n\nnamespace nvidia {\n\n// Safety container that substitutes std::string and uses Expected for error handling.\n// Uses a fixed-size buffer allocated on the stack for storage.\ntemplate <size_t N>\nclass FixedString {\n public:\n  // Use STL naming convention for compatibility with STL algorithms\n  using value_type             = char;\n  using size_type              = size_t;\n  using const_iterator         = ConstRandomAccessIterator<FixedString>;\n  using const_reverse_iterator = ReverseIterator<const_iterator>;\n\n  // Hash support\n  struct Hash {\n    template <size_t M>\n    size_t operator()(const FixedString<M>& str) const {\n      return std::_Hash_impl::hash(str.data(), str.size());\n    }\n  };\n\n  // Custom error codes for container\n  enum struct Error {\n    kArgumentNull,\n    kExceedingPreallocatedSize,\n  };\n\n  // Expected type which uses class specific errors\n  template <typename U>\n  using Expected = Expected<U, Error>;\n\n  // Constructors\n  constexpr FixedString() { clear(); }\n  template <size_t M>\n  constexpr FixedString(const char (&str)[M]) { copy(str); }\n  template <size_t M>\n  constexpr FixedString(const FixedString<M>& str) { copy(str); }\n\n  // Assignment operators\n  template <size_t M>\n  constexpr FixedString& operator=(const char (&other)[M]) {\n    copy(other);\n    return *this;\n  }\n  template <size_t M>\n  constexpr FixedString& operator=(const FixedString<M>& other) {\n    copy(other);\n    return *this;\n  }\n  // ISO standards state the assignment operator for M = N cannot be a template\n  constexpr FixedString& operator=(const FixedString& other) {\n    copy(other);\n    return *this;\n  }\n\n  constexpr const_iterator         begin()  const { return cbegin(); }\n  constexpr const_iterator         end()    const { return cend(); }\n  constexpr const_reverse_iterator rbegin() const { return crbegin(); }\n  constexpr const_reverse_iterator rend()   const { return crend(); }\n\n  constexpr const_iterator         cbegin()  const { return const_iterator(*this, 0); }\n  constexpr const_iterator         cend()    const { return const_iterator(*this, size_); }\n  constexpr const_reverse_iterator crbegin() const { return const_reverse_iterator(cend()); }\n  constexpr const_reverse_iterator crend()   const { return const_reverse_iterator(cbegin()); }\n\n  // Comparison operators\n  template <size_t M>\n  constexpr bool operator==(const char (&other)[M]) const { return compare(other) == 0; }\n  template <size_t M>\n  constexpr bool operator==(const FixedString<M>& other) const { return compare(other) == 0; }\n  template <size_t M>\n  constexpr bool operator!=(const char (&other)[M]) const { return !(*this == other); }\n  template <size_t M>\n  constexpr bool operator!=(const FixedString<M>& other) const { return !(*this == other); }\n  template <size_t M>\n  constexpr bool operator<(const char (&other)[M]) const { return compare(other) < 0; }\n  template <size_t M>\n  constexpr bool operator<(const FixedString<M>& other) const { return compare(other) < 0; }\n  template <size_t M>\n  constexpr bool operator<=(const char (&other)[M]) const { return compare(other) <= 0; }\n  template <size_t M>\n  constexpr bool operator<=(const FixedString<M>& other) const { return compare(other) <= 0; }\n  template <size_t M>\n  constexpr bool operator>(const char (&other)[M]) const { return !(*this <= other); }\n  template <size_t M>\n  constexpr bool operator>(const FixedString<M> other) const { return !(*this <= other); }\n  template <size_t M>\n  constexpr bool operator>=(const char (&other)[M]) const { return !(*this < other); }\n  template <size_t M>\n  constexpr bool operator>=(const FixedString<M> other) const { return !(*this < other); }\n\n  // Returns the length of the string excluding the null terminator\n  constexpr size_t   size() const { return size_; }\n  constexpr size_t length() const { return size_; }\n\n  // Returns the number of characters the string can store excluding the null terminator\n  constexpr size_t max_size() const { return kMaxSize; }\n\n  // Returns the number of characters the string can store including the null terminator\n  constexpr size_t capacity() const { return kCapacity; }\n\n  // Returns true if the string is empty\n  constexpr bool empty() const { return size_ == 0; }\n\n  // Returns true if the string is full\n  constexpr bool full() const { return size_ == kMaxSize; }\n\n  // Returns a pointer to a null-terminated array of characters\n  constexpr AddLvalueReference_t<const char[N + 1]>  data() const { return data_; }\n  constexpr AddLvalueReference_t<const char[N + 1]> c_str() const { return data_; }\n\n  // Clears the string\n  constexpr void clear() {\n    size_ = 0;\n    data_[size_] = '\\0';\n  }\n\n  // Appends a C-string to the end of the string\n  constexpr Expected<void> append(const char* str, size_t size) {\n    if (str == nullptr) {\n      return Unexpected<Error>{Error::kArgumentNull};\n    }\n    const size_t length = strnlen(str, size);\n    if (size_ + length > kMaxSize) {\n      return Unexpected<Error>{Error::kExceedingPreallocatedSize};\n    }\n    std::memcpy(&data_[size_], str, length);\n    size_ += length;\n    data_[size_] = '\\0';\n    return kSuccess;\n  }\n\n  // Appends a character array to the end of the string\n  template <size_t M>\n  constexpr Expected<void> append(const char (&str)[M]) { return append(str, M); }\n\n  // Appends a fixed string to the end of the string\n  template <size_t M>\n  constexpr Expected<void> append(const FixedString<M>& str) {\n    return append(str.data(), str.size());\n  }\n\n  // Appends a character to the end of the string\n  constexpr Expected<void> append(char c) { return append(&c, 1); }\n\n  // Copies a C-string to the string\n  constexpr Expected<void> copy(const char* str, size_t size) {\n    clear();\n    return append(str, size);\n  }\n\n  // Copies a character array to the string\n  template <size_t M>\n  constexpr void copy(const char (&str)[M]) {\n    static_assert(M <= kMaxSize, \"Exceeding container capacity\");\n    copy(str, M);\n  }\n\n  // Copies a fixed string to the string\n  template <size_t M>\n  constexpr void copy(const FixedString<M>& str) {\n    static_assert(M <= kMaxSize, \"Exceeding container capacity\");\n    copy(str.data(), str.size());\n  }\n\n  // Compares string to a character array\n  template <size_t M>\n  constexpr int compare(const char (&str)[M]) const {\n    const size_t length = strnlen(str, M);\n    const int result = std::strncmp(data_, str, std::min(size_, length));\n    if (result != 0) {\n      return result;\n    }\n    if (length > size_) {\n      return -1;\n    }\n    if (length < size_) {\n      return 1;\n    }\n    return 0;\n  }\n\n  // Compares string to a fixed string\n  template <size_t M>\n  constexpr int compare(const FixedString<M>& str) const { return compare(str.data()); }\n\n private:\n  // Storage capacity\n  static constexpr size_t kCapacity = N + 1;\n  // Maximum length of the string\n  static constexpr size_t kMaxSize = N;\n\n  // Special value for returning a success\n  const Expected<void> kSuccess{};\n\n  // String length\n  size_t size_;\n  // Data buffer\n  char data_[kCapacity];\n};\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_COMMON_FIXED_STRING_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/fixed_vector.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_COMMON_FIXED_VECTOR_HPP_\n#define NVIDIA_GXF_COMMON_FIXED_VECTOR_HPP_\n\n#include <cstring>\n#include <utility>\n\n#include \"common/byte.hpp\"\n#include \"common/expected.hpp\"\n#include \"common/iterator.hpp\"\n#include \"common/memory_utils.hpp\"\n\nnamespace nvidia {\n\n// Data structure that provides similar functionality to std::vector but does not dynamically\n// reallocate memory and uses Expected type for error handling instead of exceptions.\n// This container is not thread-safe.\n//\n// This container supports allocating memory on the stack or on the heap. If the template argument\n// N is specified and greater than 0, a container with a capacity of N will be created on the stack.\n// The stack allocated container cannot be resized after it is initialized. The heap allocated\n// container must use the reserve function to allocate memory before use. It does not support\n// copy assignment or construction.\n\n// Base implementation\ntemplate <typename T>\nclass FixedVectorBase {\n public:\n  // Use STL naming convention for compatibility with STL algorithms\n  using value_type             = T;\n  using size_type              = size_t;\n  using iterator               = RandomAccessIterator<FixedVectorBase>;\n  using reverse_iterator       = ReverseIterator<iterator>;\n  using const_iterator         = ConstRandomAccessIterator<FixedVectorBase>;\n  using const_reverse_iterator = ReverseIterator<const_iterator>;\n\n  virtual ~FixedVectorBase() = default;\n\n  // Custom error codes for vector\n  enum struct Error {\n    kOutOfMemory,         // Memory allocation failed\n    kArgumentOutOfRange,  // Argument is out of valid range\n    kContainerEmpty,      // Container is empty\n    kContainerFull,       // Container is fixed and reached max capacity\n    kInvalidIterator,     // Iterator is invalid\n  };\n\n  // Expected type which uses class specific errors\n  template <typename U>\n  using Expected = Expected<U, Error>;\n\n  constexpr bool operator==(const FixedVectorBase& other) const {\n    if (size_ != other.size_) {\n      return false;\n    }\n    if (data_ == other.data_) {\n      return true;\n    }\n    for (size_t i = 0; i < size_; i ++) {\n      if (data_[i] != other.data_[i]) {\n        return false;\n      }\n    }\n    return true;\n  }\n  constexpr bool operator!=(const FixedVectorBase& other) const { return !(*this == other); }\n\n  constexpr iterator         begin()  { return iterator(*this, 0); }\n  constexpr iterator         end()    { return iterator(*this, size_); }\n  constexpr reverse_iterator rbegin() { return reverse_iterator(end()); }\n  constexpr reverse_iterator rend()   { return reverse_iterator(begin()); }\n\n  constexpr const_iterator         begin()  const { return cbegin(); }\n  constexpr const_iterator         end()    const { return cend(); }\n  constexpr const_reverse_iterator rbegin() const { return crbegin(); }\n  constexpr const_reverse_iterator rend()   const { return crend(); }\n\n  constexpr const_iterator         cbegin()  const { return const_iterator(*this, 0); }\n  constexpr const_iterator         cend()    const { return const_iterator(*this, size_); }\n  constexpr const_reverse_iterator crbegin() const { return const_reverse_iterator(cend()); }\n  constexpr const_reverse_iterator crend()   const { return const_reverse_iterator(cbegin()); }\n\n  constexpr Expected<T&> operator[](size_t index) { return at(index); }\n  constexpr Expected<const T&> operator[](size_t index) const { return at(index); }\n\n  // Returns a pointer to data\n  constexpr T* data() { return data_; }\n  // Returns a read-only pointer to data\n  constexpr const T* data() const { return data_; }\n  // Returns the number of elements the vector can currently hold\n  constexpr size_t capacity() const { return capacity_; }\n  // Returns the number of elements in the vector\n  constexpr size_t size() const { return size_; }\n  // Returns true if the vector contains no elements\n  constexpr bool empty() const { return size_ == 0; }\n  // Returns true if the vector reached capacity\n  constexpr bool full() const { return size_ == capacity_; }\n\n  // Returns a reference to the element at the given index\n  constexpr Expected<T&> at(size_t index) {\n    if (index >= size_) {\n      return Unexpected<Error>{Error::kArgumentOutOfRange};\n    }\n    return data_[index];\n  }\n\n  // Returns a read-only reference to the element at the given index\n  constexpr Expected<const T&> at(size_t index) const {\n    if (index >= size_) {\n      return Unexpected<Error>{Error::kArgumentOutOfRange};\n    }\n    return data_[index];\n  }\n\n  // Returns a reference to the first element\n  constexpr Expected<T&> front() {\n    if (empty()) {\n      return Unexpected<Error>{Error::kContainerEmpty};\n    }\n    return data_[0];\n  }\n\n  // Returns a read-only reference to the first element\n  constexpr Expected<const T&> front() const {\n    if (empty()) {\n      return Unexpected<Error>{Error::kContainerEmpty};\n    }\n    return data_[0];\n  }\n\n  // Returns a reference to the last element\n  constexpr Expected<T&> back() {\n    if (empty()) {\n      return Unexpected<Error>{Error::kContainerEmpty};\n    }\n    return data_[size_ - 1];\n  }\n\n  // Returns a read-only reference to the last element\n  constexpr Expected<const T&> back() const {\n    if (empty()) {\n      return Unexpected<Error>{Error::kContainerEmpty};\n    }\n    return data_[size_ - 1];\n  }\n\n  // Creates a new object with the provided arguments and adds it at the specified index\n  template <typename... Args>\n  constexpr Expected<void> emplace(size_t index, Args&&... args) {\n    if (index > size_) {\n      return Unexpected<Error>{Error::kArgumentOutOfRange};\n    }\n    if (full()) {\n      return Unexpected<Error>{Error::kContainerFull};\n    }\n    if (index < size_) {\n      ArrayMoveConstruct(BytePointer(&data_[index + 1]), &data_[index], size_ - index);\n    }\n    InplaceConstruct<T>(BytePointer(&data_[index]), std::forward<Args>(args)...);\n    size_++;\n    return kSuccess;\n  }\n\n  // Creates a new object with the provided arguments and adds it to the end of the vector\n  template <typename... Args>\n  constexpr Expected<void> emplace_back(Args&&... args) {\n    return emplace(size_, std::forward<Args>(args)...);\n  }\n\n  // Copies the object to the specified index\n  constexpr Expected<void> insert(size_t index, const T& obj) { return emplace(index, obj); }\n  // Moves the object to the specified index\n  constexpr Expected<void> insert(size_t index, T&& obj) {\n    return emplace(index, std::forward<T>(obj));\n  }\n\n  // Insert elements from another fixed vector object to the specified index\n  constexpr Expected<void> insert(iterator index, const_iterator start, const_iterator end) {\n        ssize_t count = static_cast<ssize_t>(std::distance(start, end));\n        size_t pos = static_cast<size_t>(std::distance(begin(), index));\n        if ((pos > size_) || (size_ + count > capacity_) || (count < 0)) {\n            return Unexpected<Error>{Error::kArgumentOutOfRange};\n        }\n\n        auto maybe_value = *start;\n        if (maybe_value) {\n          const T* ptr = &(*maybe_value);\n          if (pos < size_) {\n            ArrayCopyConstruct(BytePointer(data_ + pos + count), &data_[pos], size_ - pos);\n          }\n          ArrayCopyConstruct(BytePointer(data_ + pos), ptr, count);\n          size_ += count;\n        } else {\n          return Unexpected<Error>{Error::kInvalidIterator};\n        }\n\n        return kSuccess;\n  }\n\n  // Copies the object to the end of the vector\n  constexpr Expected<void> push_back(const T& obj) { return emplace_back(obj); }\n  // Moves the object to the end of the vector\n  constexpr Expected<void> push_back(T&& obj) { return emplace_back(std::forward<T>(obj)); }\n\n  // Removes the object at the specified index and destroys it\n  constexpr Expected<void> erase(size_t index) {\n    if (index >= size_) {\n      return Unexpected<Error>{Error::kArgumentOutOfRange};\n    }\n    if (empty()) {\n      return Unexpected<Error>{Error::kContainerEmpty};\n    }\n    Destruct<T>(BytePointer(&data_[index]));\n    size_--;\n    if (index < size_) {\n      ArrayMoveConstruct(BytePointer(&data_[index]), &data_[index + 1], size_ - index);\n    }\n    return kSuccess;\n  }\n\n  // Removes the object at the end of the vector and destroys it\n  constexpr Expected<void> pop_back() { return erase(size_ - 1); }\n\n  // Removes all objects from the vector\n  constexpr void clear() {\n    while (size_ > 0) {\n      Destruct<T>(BytePointer(&data_[--size_]));\n    }\n  }\n\n  // Resizes the vector by removing objects from the end if shrinking\n  // or by adding default objects to the end if expanding\n  constexpr Expected<void> resize(size_t count) {\n    if (count > capacity_) {\n      return Unexpected<Error>{Error::kArgumentOutOfRange};\n    }\n    while (count > size_) {\n      push_back(T());\n    }\n    while (count < size_) {\n      pop_back();\n    }\n    return kSuccess;\n  }\n\n  // Resizes the vector by removing objects from the end if shrinking\n  // or by adding copies of the given object to the end if expanding\n  constexpr Expected<void> resize(size_t count, const T& obj) {\n    if (count > capacity_) {\n      return Unexpected<Error>{Error::kArgumentOutOfRange};\n    }\n    while (count > size_) {\n      push_back(obj);\n    }\n    while (count < size_) {\n      pop_back();\n    }\n    return kSuccess;\n  }\n\n protected:\n  // Special value for returning a success\n  const Expected<void> kSuccess{};\n\n  FixedVectorBase() : data_{nullptr}, capacity_{0}, size_{0} {};\n\n  // Pointer to an array of objects\n  T* data_;\n  // Maximum number of objects the container can hold\n  size_t capacity_;\n  // Number of objects stored\n  size_t size_;\n};\n\n// Special value used to instantiate a FixedVector with heap memory allocation\nconstexpr ssize_t kFixedVectorHeap = -1;\n\n// Vector with stack memory allocation\ntemplate <typename T, ssize_t N = kFixedVectorHeap>\nclass FixedVector : public FixedVectorBase<T> {\n public:\n  static_assert(N >= 0, \"N must be non-negative\");\n\n  constexpr FixedVector() {\n    data_ = ValuePointer<T>(pool_);\n    capacity_ = N;\n  }\n  constexpr FixedVector(const FixedVector& other) { *this = other; }\n  constexpr FixedVector(FixedVector&& other) { *this = std::move(other); }\n  constexpr FixedVector& operator=(const FixedVector& other) {\n    if (this != &other) {\n      data_ = ValuePointer<T>(pool_);\n      capacity_ = N;\n      ArrayCopyConstruct(BytePointer(data_), other.data_, other.size_);\n      size_ = other.size_;\n    }\n    return *this;\n  }\n  constexpr FixedVector& operator=(FixedVector&& other) {\n    if (this != &other) {\n      data_ = ValuePointer<T>(pool_);\n      capacity_ = N;\n      ArrayMoveConstruct(BytePointer(data_), other.data_, other.size_);\n      std::swap(size_, other.size_);\n    }\n    return *this;\n  }\n  ~FixedVector() { FixedVectorBase<T>::clear(); }\n\n private:\n  using FixedVectorBase<T>::data_;\n  using FixedVectorBase<T>::capacity_;\n  using FixedVectorBase<T>::size_;\n\n  // Size of memory pool for stack allocation\n  static constexpr size_t kPoolSize = N * sizeof(T);\n  // Memory pool for storing objects on the stack\n  byte pool_[kPoolSize];\n};\n\n// Vector with heap memory allocation\ntemplate <typename T>\nclass FixedVector<T, kFixedVectorHeap> : public FixedVectorBase<T> {\n public:\n  template<typename U>\n  using Expected = typename FixedVectorBase<T>::template Expected<U>;\n\n  constexpr FixedVector() = default;\n  constexpr FixedVector(const FixedVector& other) = delete;\n  constexpr FixedVector(FixedVector&& other) { *this = std::move(other); }\n  constexpr FixedVector& operator=(const FixedVector& other) = delete;\n  constexpr FixedVector& operator=(FixedVector&& other) {\n    if (this != &other) {\n      std::swap(data_, other.data_);\n      std::swap(capacity_, other.capacity_);\n      std::swap(size_, other.size_);\n    }\n    return *this;\n  }\n  ~FixedVector() {\n    FixedVectorBase<T>::clear();\n    DeallocateArray<T>(data_);\n  }\n\n  // Copies the contents of the given vector\n  // Current contents are discarded\n  // Fails if given vector is larger than current capacity\n  constexpr Expected<void> copy_from(const FixedVector& other) {\n    if (other.size_ > capacity_) {\n      return Unexpected<Error>{Error::kArgumentOutOfRange};\n    }\n    FixedVectorBase<T>::clear();\n    size_ = other.size_;\n    ArrayCopyConstruct(BytePointer(data_), other.data_, size_);\n    return kSuccess;\n  }\n\n  // Allocates memory to hold the specified number of elements\n  constexpr Expected<void> reserve(size_t capacity) {\n    if (capacity > capacity_) {\n      T* data = AllocateArray<T>(capacity);\n      if (!data) {\n        return Unexpected<Error>{Error::kOutOfMemory};\n      }\n      ArrayMoveConstruct(BytePointer(data), data_, size_);\n      DeallocateArray<T>(data_);\n      data_ = data;\n      capacity_ = capacity;\n    }\n    return kSuccess;\n  }\n\n  // Shrinks memory allocation to fit current number of elements\n  constexpr Expected<void> shrink_to_fit() {\n    if (size_ < capacity_) {\n      T* data = AllocateArray<T>(size_);\n      if (!data) {\n        return Unexpected<Error>(Error::kOutOfMemory);\n      }\n      ArrayMoveConstruct(BytePointer(data), data_, size_);\n      DeallocateArray<T>(data_);\n      data_ = data;\n      capacity_ = size_;\n    }\n    return kSuccess;\n  }\n\n private:\n  using Error = typename FixedVectorBase<T>::Error;\n  using FixedVectorBase<T>::kSuccess;\n  using FixedVectorBase<T>::data_;\n  using FixedVectorBase<T>::capacity_;\n  using FixedVectorBase<T>::size_;\n};\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_COMMON_FIXED_VECTOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/iterator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_COMMON_ITERATOR_HPP_\n#define NVIDIA_GXF_COMMON_ITERATOR_HPP_\n\n#include <iterator>\n\n#include \"common/expected.hpp\"\n#include \"common/type_utils.hpp\"\n\nnamespace nvidia {\n\nnamespace detail {\n\n/// Type traits to determine if a type has `data()`\ntemplate <class, class = void> struct HasData                               : FalseType {};\ntemplate <class T> struct HasData<T, void_t<decltype(Declval<T>().data())>> : TrueType  {};\ntemplate <class T> constexpr bool HasData_v = HasData<T>::value;\n\n/// Type traits to determine if a type has `size()`\ntemplate <class, class = void> struct HasSize                               : FalseType {};\ntemplate <class T> struct HasSize<T, void_t<decltype(Declval<T>().size())>> : TrueType  {};\ntemplate <class T> constexpr bool HasSize_v = HasSize<T>::value;\n\n}  // namespace detail\n\n/// Random-access iterator for containers with elements in contiguous memory.\n/// A pointer to the container is stored so the iterator does not get invalidated if elements are\n/// added and/or removed from the container. Incrementing/decrementing operations will saturate\n/// if the iterator reaches the end/beginning of the container. Element access is wrapped in\n/// an `Expected` to prevent dereferencing invalid memory.\ntemplate <typename TContainer, typename TValue = typename TContainer::value_type>\nclass RandomAccessIterator {\n  static_assert(detail::HasData_v<TContainer>, \"TContainer must have data()\");\n  static_assert(detail::HasSize_v<TContainer>, \"TContainer must have size()\");\n  static_assert(IsIntegral_v<decltype(Declval<TContainer>().size())>,\n                \"size() must return an integral type\");\n\n public:\n  enum struct Error {\n    kArgumentOutOfRange,\n    kInvalidIterator,\n  };\n\n  template <typename U>\n  using Expected = Expected<U, Error>;\n\n    // iterator traits\n    using iterator_category = std::random_access_iterator_tag;\n    using value_type = TValue;\n    using difference_type = std::ptrdiff_t;\n    using pointer = TValue*;\n    using reference = TValue&;\n  constexpr RandomAccessIterator() : container_{nullptr}, index_{-1} {}\n  constexpr RandomAccessIterator(TContainer& container, size_t start)\n      : container_{&container}, index_{0} {\n    *this += start;\n  }\n\n  template <class OContainer,\n            typename std::enable_if<\n                std::is_same<TContainer, const OContainer>::value &&\n                std::is_same<TValue, const typename OContainer::value_type>::value, int>::type = 0>\n  constexpr RandomAccessIterator(const RandomAccessIterator<OContainer>& other)\n      : container_{other.container_}, index_{0} {\n    *this += other.index_;\n  }\n\n  constexpr RandomAccessIterator(const RandomAccessIterator& other) = default;\n  constexpr RandomAccessIterator(RandomAccessIterator&& other) = default;\n  constexpr RandomAccessIterator& operator=(const RandomAccessIterator& other) = default;\n  constexpr RandomAccessIterator& operator=(RandomAccessIterator&& other) = default;\n\n  constexpr Expected<TValue&> operator*() const {\n    return 0 <= index_ && index_ < static_cast<difference_type>(container_->size())\n        ? Expected<TValue&>{container_->data()[index_]}\n        : Unexpected<Error>{Error::kInvalidIterator};\n  }\n  constexpr Expected<TValue&> operator[](difference_type offset) const {\n    if (container_ == nullptr) {\n      return Unexpected<Error>{Error::kInvalidIterator};\n    }\n    difference_type index = index_ + offset;\n    return 0 <= index && index < static_cast<difference_type>(container_->size())\n        ? Expected<TValue&>{container_->data()[index]}\n        : Unexpected<Error>{Error::kArgumentOutOfRange};\n  }\n\n  constexpr RandomAccessIterator& operator+=(difference_type offset) {\n    if (container_ == nullptr) {\n      return *this;\n    }\n    difference_type index = index_ + offset;\n    if (index < 0) {\n      index = 0;\n    } else if (index > static_cast<difference_type>(container_->size())) {\n      index = container_->size();\n    }\n    index_ = index;\n    return *this;\n  }\n  constexpr RandomAccessIterator& operator++() {\n    *this += 1;\n    return *this;\n  }\n  constexpr RandomAccessIterator operator++(int) {\n    RandomAccessIterator iter = *this;\n    ++(*this);\n    return iter;\n  }\n  constexpr RandomAccessIterator& operator-=(difference_type offset) {\n    *this += -offset;\n    return *this;\n  }\n  constexpr RandomAccessIterator& operator--() {\n    *this -= 1;\n    return *this;\n  }\n  constexpr RandomAccessIterator operator--(int) {\n    RandomAccessIterator iter = *this;\n    --(*this);\n    return iter;\n  }\n\n  friend constexpr RandomAccessIterator operator+(RandomAccessIterator a, difference_type n) {\n    a += n;\n    return a;\n  }\n  friend constexpr RandomAccessIterator operator+(difference_type n, RandomAccessIterator a) {\n    return a + n;\n  }\n  friend constexpr RandomAccessIterator operator-(RandomAccessIterator a, difference_type n) {\n    a -= n;\n    return a;\n  }\n  friend constexpr difference_type operator-(const RandomAccessIterator& a,\n                                             const RandomAccessIterator& b) {\n    return a.index_ - b.index_;\n  }\n  friend constexpr bool operator==(const RandomAccessIterator& a, const RandomAccessIterator& b) {\n    return a.container_ == b.container_ && a.index_ == b.index_;\n  }\n  friend constexpr bool operator!=(const RandomAccessIterator& a, const RandomAccessIterator& b) {\n    return !(a == b);\n  }\n\n private:\n  friend class RandomAccessIterator<const TContainer, const typename TContainer::value_type>;\n  /// Container pointer\n  TContainer* container_;\n  /// Iterator index\n  difference_type index_;\n};\n\n/// Constant Random-access iterator\ntemplate <typename TContainer>\nusing ConstRandomAccessIterator = RandomAccessIterator<const TContainer,\n                                                       const typename TContainer::value_type>;\n\n/// Reverse iterator\ntemplate <typename TIterator>\nclass ReverseIterator {\n public:\n  using TValue = typename TIterator::value_type;\n  using Error = typename TIterator::Error;\n\n  template <typename U>\n  using Expected = Expected<U, Error>;\n\n  // iterator traits\n  using iterator_category = std::random_access_iterator_tag;\n  using value_type = TValue;\n  using difference_type = std::ptrdiff_t;\n  using pointer = TValue*;\n  using reference = TValue&;\n\n  constexpr explicit ReverseIterator() : iter_{} {}\n  constexpr explicit ReverseIterator(TIterator iter) : iter_{iter} {}\n\n  constexpr ReverseIterator(const ReverseIterator& other) = default;\n  constexpr ReverseIterator(ReverseIterator&& other) = default;\n  constexpr ReverseIterator& operator=(const ReverseIterator& other) = default;\n  constexpr ReverseIterator& operator=(ReverseIterator&& other) = default;\n\n  constexpr TIterator base() const { return iter_; }\n\n  constexpr Expected<TValue&> operator*() const { return *std::prev(iter_); }\n  constexpr Expected<TValue&> operator[](difference_type offset) const {\n    return std::prev(iter_)[-offset];\n  }\n\n  constexpr ReverseIterator& operator+=(difference_type offset) {\n    iter_ -= offset;\n    return *this;\n  }\n  constexpr ReverseIterator& operator++() {\n    iter_ -= 1;\n    return *this;\n  }\n  constexpr ReverseIterator operator++(int) {\n    ReverseIterator iter = *this;\n    ++(*this);\n    return iter;\n  }\n  constexpr ReverseIterator& operator-=(difference_type offset) {\n    iter_ += offset;\n    return *this;\n  }\n  constexpr ReverseIterator& operator--() {\n    iter_ += 1;\n    return *this;\n  }\n  constexpr ReverseIterator operator--(int) {\n    ReverseIterator iter = *this;\n    --(*this);\n    return iter;\n  }\n\n  friend constexpr ReverseIterator operator+(ReverseIterator a, difference_type n) {\n    return ReverseIterator(a.iter_ - n);\n  }\n  friend constexpr ReverseIterator operator+(difference_type n, ReverseIterator a) {\n    return ReverseIterator(a.iter_ - n);\n  }\n  friend constexpr ReverseIterator operator-(ReverseIterator a, difference_type n) {\n    return ReverseIterator(a.iter_ + n);\n  }\n  friend constexpr difference_type operator-(const ReverseIterator& a, const ReverseIterator& b) {\n    return b.iter_ - a.iter_;\n  }\n  friend constexpr bool operator==(const ReverseIterator& a, const ReverseIterator& b) {\n    return a.iter_ == b.iter_;\n  }\n  friend constexpr bool operator!=(const ReverseIterator& a, const ReverseIterator& b) {\n    return !(a == b);\n  }\n\n private:\n  /// Random-access iterator\n  TIterator iter_;\n};\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_COMMON_ITERATOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/logger.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_LOGGER_HPP_\n#define NVIDIA_COMMON_LOGGER_HPP_\n\n#include <cstdarg>\n#include <cstdio>\n#include <vector>\n\n#include \"gxf/logger/gxf_logger.hpp\"\n\n#define GXF_LOG_LEVEL_PANIC 0\n#define GXF_LOG_LEVEL_ERROR 1\n#define GXF_LOG_LEVEL_WARNING 2\n#define GXF_LOG_LEVEL_INFO 3\n#define GXF_LOG_LEVEL_DEBUG 4\n#define GXF_LOG_LEVEL_VERBOSE 5\n\n// clang-format off\n\n// Set the default active logging level to VERBOSE(5) if not defined\n#if !defined(GXF_LOG_ACTIVE_LEVEL)\n#  define GXF_LOG_ACTIVE_LEVEL 5  // GXF_LOG_LEVEL_VERBOSE\n#endif\n\n// Define GXF_LOG_ACTIVE_LEVEL before including `common/logger.hpp` to control the logging level\n// at compile time. This allows you to skip logging at certain levels.\n//\n// Example:\n//     #define GXF_LOG_ACTIVE_LEVEL 2\n//     #include \"common/logger.hpp\"\n//     ...\n//\n// With this setting, logging will occur only at the WARNING(2), ERROR(1), and PANIC(0) levels.\n//\n// You can define GXF_LOG_ACTIVE_LEVEL in your build system. For instance, in CMake, use:\n//\n//     target_compile_definitions(my_target PRIVATE GXF_LOG_ACTIVE_LEVEL=2)\n//\n// This sets the active logging level to WARNING(2) for the target `my_target`.\n//\n// Alternatively, define GXF_LOG_ACTIVE_LEVEL at compile time by passing `-DGXF_LOG_ACTIVE_LEVEL=2`\n// directly to the compiler.\n//\n// In the Bazel build system, set this in your build configuration as follows:\n//\n//     cc_binary(\n//         name = \"my_binary\",\n//         srcs = [\"my_binary.cc\"],\n//         copts = [\"-DGXF_LOG_ACTIVE_LEVEL=2\"],\n//     )\n//\n// This sets the active logging level to WARNING(2) for the target `my_binary`.\n//\n// Or, when using a Bazel build command:\n//     bazel build --copt=-DGXF_LOG_ACTIVE_LEVEL=3 //path:to_your_target\n//\n// This sets the active logging level to INFO(3) for the target `//path:to_your_target`.\n\n// Logs a verbose message\n#if GXF_LOG_ACTIVE_LEVEL >= GXF_LOG_LEVEL_VERBOSE\n#  define GXF_LOG_VERBOSE(...) \\\n      ::nvidia::Log(__FILE__, __LINE__, ::nvidia::Severity::VERBOSE, __VA_ARGS__)\n#else\n#  define GXF_LOG_VERBOSE(...) (void)0\n#endif\n\n// Logs a debug message\n#if GXF_LOG_ACTIVE_LEVEL >= GXF_LOG_LEVEL_DEBUG\n#  define GXF_LOG_DEBUG(...) \\\n      ::nvidia::Log(__FILE__, __LINE__, ::nvidia::Severity::DEBUG, __VA_ARGS__)\n#else\n#  define GXF_LOG_DEBUG(...) (void)0\n#endif\n\n// Logs an informational message\n#if GXF_LOG_ACTIVE_LEVEL >= GXF_LOG_LEVEL_INFO\n#  define GXF_LOG_INFO(...) \\\n      ::nvidia::Log(__FILE__, __LINE__, ::nvidia::Severity::INFO, __VA_ARGS__)\n#else\n#  define GXF_LOG_INFO(...) (void)0\n#endif\n\n// Logs a warning\n#if GXF_LOG_ACTIVE_LEVEL >= GXF_LOG_LEVEL_WARNING\n#  define GXF_LOG_WARNING(...) \\\n      ::nvidia::Log(__FILE__, __LINE__, ::nvidia::Severity::WARNING, __VA_ARGS__)\n#else\n#  define GXF_LOG_WARNING(...) (void)0\n#endif\n\n// Logs an error\n#if GXF_LOG_ACTIVE_LEVEL >= GXF_LOG_LEVEL_ERROR\n#  define GXF_LOG_ERROR(...) \\\n      ::nvidia::Log(__FILE__, __LINE__, ::nvidia::Severity::ERROR, __VA_ARGS__)\n#else\n#  define GXF_LOG_ERROR(...) (void)0\n#endif\n\n// Logs a panic\n#if GXF_LOG_ACTIVE_LEVEL >= GXF_LOG_LEVEL_PANIC\n#  define GXF_LOG_PANIC(...) \\\n      ::nvidia::Log(__FILE__, __LINE__, ::nvidia::Severity::PANIC, __VA_ARGS__)\n#else\n#  define GXF_LOG_PANIC(...) (void)0\n#endif\n\n// clang-format on\n\nnamespace nvidia {\n\n// Function which is used for logging. It can be changed to intercept the logged messages.\n// Additional arguments can be supplied via LoggingFunctionArg.\nextern void (*LoggingFunction)(const char* file, int line, Severity severity,\n                               const char* log, void* arg);\nextern void* LoggingFunctionArg;\n\n// Default implementation of the logging function which prints to console\nvoid DefaultConsoleLogging(const char* file, int line, Severity severity,\n                           const char* log, void* arg);\n\n// Redirects the output for a given log severity.\nvoid Redirect(std::FILE* file, Severity severity = Severity::ALL);\n\n// Sets the log severity from the environment variable.\n// If the environment variable is not set or invalid, the severity is not changed.\n// Returns true if the environment variable is accessible and the severity was updated from it.\nbool SetSeverityFromEnv(const char* env_name = kGxfLogEnvName);\n\n// Returns the log severity from the environment variable.\n// If the environment variable is not set or invalid, returns Severity::COUNT.\n// If 'error_code' is not null, it will be set to 1 if the environment variable is set but invalid,\n// or to 0 otherwise.\nSeverity GetSeverityFromEnv(const char* env_name = kGxfLogEnvName, int* error_code = nullptr);\n\n// Sets global log severity thus effectively disabling all logging with lower severity\nvoid SetSeverity(Severity severity);\n\n// Returns global log severity\nSeverity GetSeverity();\n\n// Converts the message and argument into a string and pass it to LoggingFunction.\ntemplate<typename... Args>\nvoid Log(const char* file, int line, Severity severity, const char* txt, ...) __attribute__((format(printf, 4, 5))); // NOLINT\n\ntemplate<typename... Args>\nvoid Log(const char* file, int line, Severity severity, const char* txt, ...) {\n  va_list args1;\n  va_start(args1, txt);\n  va_list args2;\n  va_copy(args2, args1);\n  std::vector<char> buf(1 + std::vsnprintf(NULL, 0, txt, args1));\n  va_end(args1);\n  auto buf_data = buf.data();\n  auto buf_size = buf.size();\n  std::vsnprintf(buf_data, buf_size, txt, args2);\n  va_end(args2);\n\n  logger::GxfLogger& logger = logger::GlobalGxfLogger::instance();\n  logger.log(file, line, nullptr, static_cast<int>(severity), buf_data);\n}\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_COMMON_LOGGER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/memory_utils.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_MEMORY_UTILS_HPP_\n#define NVIDIA_COMMON_MEMORY_UTILS_HPP_\n\n#include <cstring>\n#include <memory>\n#include <new>\n#include <type_traits>\n#include <utility>\n\n#include \"common/byte.hpp\"\n\nnamespace nvidia {\n\n// Convert a raw byte pointer `src` to a concrete type T\ntemplate <typename T>\nT* ValuePointer(byte* src) {\n  return reinterpret_cast<T*>(src);\n}\n\n// Convert a const raw byte pointer `src` to a concrete type T\ntemplate <typename T>\nconst T* ValuePointer(const byte* src) {\n  return reinterpret_cast<const T*>(src);\n}\n\n// Convert a type T pointer `src` to a raw byte pointer\ntemplate <typename T>\nbyte* BytePointer(T* src) {\n  return reinterpret_cast<byte*>(src);\n}\n\n// Convert a const type T pointer `src` to a raw byte pointer\ntemplate <typename T>\nconst byte* BytePointer(const T* src) {\n  return reinterpret_cast<const byte*>(src);\n}\n\n// Call the destructor for type T on the object located at `src`.\ntemplate <typename T>\nvoid Destruct(byte* src) {\n  reinterpret_cast<T*>(src)->~T();\n}\n\n// Construct an object of type T with the provided arguments into the memory at `dst` with\n// placement new operator.\ntemplate <typename T, typename... Args>\nT* InplaceConstruct(byte* dst, Args&&... args) {\n  return new (dst) T{std::forward<Args>(args)...};\n}\n\n// Move construct an object of type T into the memory at `dst` with placement new operator.\ntemplate <typename T>\nT* InplaceMoveConstruct(byte* dst, T&& other) {\n  return new (dst) T{std::forward<T>(other)};\n}\n\n// Copy construct an object of type T into the memory at `dst` with placement new operator.\ntemplate <typename T>\nT* InplaceCopyConstruct(byte* dst, const T& other) {\n  return new (dst) T{other};\n}\n\n// Move constructs an array of objects of type T into the memory at `dst`\n// Used if object is movable\ntemplate <typename T, std::enable_if_t<std::is_move_constructible<T>::value>* = nullptr>\nvoid ArrayMoveConstruct(byte* dst, T* src, size_t count) {\n  // Reverse move direction to handle overlapping memory segments\n  const bool reverse = dst > BytePointer(src) && dst < BytePointer(src + count);\n  for (size_t i = 0; i < count; i++) {\n    const size_t index = reverse ? count - 1 - i : i;\n    InplaceMoveConstruct<T>(dst + sizeof(T) * index, std::move(src[index]));\n  }\n}\n\n// Move constructs an array of objects of type T into the memory at `dst`\n// Used if object is not movable but trivially copyable\ntemplate <typename T, std::enable_if_t<!std::is_move_constructible<T>::value &&\n                                       std::is_trivially_copyable<T>::value>* = nullptr>\nvoid ArrayMoveConstruct(byte* dst, T* src, size_t count) {\n  std::memmove(dst, src, sizeof(T) * count);\n}\n\n// Move constructs an array of objects of type T into the memory at `dst`\n// Used if object is not movable and not trivially copyable\ntemplate <typename T, std::enable_if_t<!std::is_move_constructible<T>::value &&\n                                       !std::is_trivially_copyable<T>::value>* = nullptr>\nvoid ArrayMoveConstruct(byte* dst, T* src, size_t count) {\n  // Reverse move direction to handle overlapping memory segments\n  const bool reverse = dst > BytePointer(src) && dst < BytePointer(src + count);\n  for (size_t i = 0; i < count; i++) {\n    const size_t index = reverse ? count - 1 - i : i;\n    InplaceCopyConstruct<T>(dst + sizeof(T) * index, src[index]);\n  }\n}\n\n// Copy constructs an array of objects of type T into the memory at `dst`\n// Used if object is trivially copyable\ntemplate <typename T, std::enable_if_t<std::is_trivially_copyable<T>::value>* = nullptr>\nvoid ArrayCopyConstruct(byte* dst, T* src, size_t count) {\n  std::memmove(dst, src, sizeof(T) * count);\n}\n\n// Copy constructs an array of objects of type T into the memory at `dst`\n// Used if object is not trivially copyable\ntemplate <typename T, std::enable_if_t<!std::is_trivially_copyable<T>::value>* = nullptr>\nvoid ArrayCopyConstruct(byte* dst, T* src, size_t count) {\n  // Reverse move direction to handle overlapping memory segments\n  const bool reverse = dst > BytePointer(src) && dst < BytePointer(src + count);\n  for (size_t i = 0; i < count; i++) {\n    const size_t index = reverse ? count - 1 - i : i;\n    InplaceCopyConstruct<T>(dst + sizeof(T) * index, src[index]);\n  }\n}\n\n// Allocates an array with `size` number of objects of type T\n// Does not call object constructor\ntemplate <typename T>\nT* AllocateArray(size_t size) {\n  return static_cast<T*>(::operator new(size * sizeof(T), std::nothrow));\n}\n\n// Deallocates the array `data` of objects of type T\n// Does not call object destructor\ntemplate <typename T>\nvoid DeallocateArray(T* data) {\n  ::operator delete(data);\n}\n\n// Construct an object of type T with the provided arguments and returns a unique pointer to it\n// Null pointer is returned on error\ntemplate <typename T, typename... Args>\nstd::unique_ptr<T> MakeUniqueNoThrow(Args&&... args) {\n  return std::unique_ptr<T>(new(std::nothrow) T{std::forward<Args>(args)...});\n}\n\n// Construct an object of type T with the provided arguments and returns a shared pointer to it\n// Null pointer is returned on error\ntemplate <typename T, typename... Args>\nstd::shared_ptr<T> MakeSharedNoThrow(Args&&... args) {\n  return std::shared_ptr<T>(new(std::nothrow) T{std::forward<Args>(args)...});\n}\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_COMMON_MEMORY_UTILS_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/nvtx_helper.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_NVTX_HELPER_HPP_\n#define NVIDIA_COMMON_NVTX_HELPER_HPP_\n\n#include <string>\n\n#include \"nvtx3/nvToolsExt.h\"\n\n// Helper functions to create named color events for NVTX profiling\n\ninline nvtxEventAttributes_t CreateGreenEvent(const std::string& message, uint32_t category) {\n  nvtxEventAttributes_t eventAttrib = {0};\n  eventAttrib.version = NVTX_VERSION;\n  eventAttrib.size = NVTX_EVENT_ATTRIB_STRUCT_SIZE;\n  eventAttrib.colorType = NVTX_COLOR_ARGB;\n  eventAttrib.color = 0xFF76b900;  // NVIDIA GREEN\n  eventAttrib.messageType = NVTX_MESSAGE_TYPE_ASCII;\n  eventAttrib.message.ascii = message.c_str();\n  eventAttrib.category = category;\n\n  return eventAttrib;\n}\n\ninline nvtxEventAttributes_t CreateRedEvent(const std::string& message, uint32_t category) {\n  nvtxEventAttributes_t eventAttrib = {0};\n  eventAttrib.version = NVTX_VERSION;\n  eventAttrib.size = NVTX_EVENT_ATTRIB_STRUCT_SIZE;\n  eventAttrib.colorType = NVTX_COLOR_ARGB;\n  eventAttrib.color = 0xFFFe2712;  // Red\n  eventAttrib.messageType = NVTX_MESSAGE_TYPE_ASCII;\n  eventAttrib.message.ascii = message.c_str();\n  eventAttrib.category = category;\n\n  return eventAttrib;\n}\n\n\ninline nvtxEventAttributes_t CreateBlackEvent(const std::string& message, uint32_t category) {\n  nvtxEventAttributes_t eventAttrib = {0};\n  eventAttrib.version = NVTX_VERSION;\n  eventAttrib.size = NVTX_EVENT_ATTRIB_STRUCT_SIZE;\n  eventAttrib.colorType = NVTX_COLOR_ARGB;\n  eventAttrib.color = 0xFF010203;  // Rich black\n  eventAttrib.messageType = NVTX_MESSAGE_TYPE_ASCII;\n  eventAttrib.message.ascii = message.c_str();\n  eventAttrib.category = category;\n\n  return eventAttrib;\n}\n\ninline nvtxEventAttributes_t CreateBlueEvent(const std::string& message, uint32_t category) {\n  nvtxEventAttributes_t eventAttrib = {0};\n  eventAttrib.version = NVTX_VERSION;\n  eventAttrib.size = NVTX_EVENT_ATTRIB_STRUCT_SIZE;\n  eventAttrib.colorType = NVTX_COLOR_ARGB;\n  eventAttrib.color = 0xFF66a1fe;  // Light Blue\n  eventAttrib.messageType = NVTX_MESSAGE_TYPE_ASCII;\n  eventAttrib.message.ascii = message.c_str();\n  eventAttrib.category = category;\n\n  return eventAttrib;\n}\n\n#endif  // NVIDIA_COMMON_NVTX_HELPER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/singleton.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_SINGLETON_HPP_\n#define NVIDIA_COMMON_SINGLETON_HPP_\n\nnamespace gxf {\n\n// A singleton which supports creation at pre-execution time\n// Usage: For a class `MyType` with a member function `myMember` you can just write:\n//   Singleton<MyType>::Get().myMember();\n// This will automatically create a singleton, i.e. return the same instance each time you call Get.\n// The constructor of MyType will be called at pre-execution time, i.e. before entering main.\n// If you use multiple singletons they will all be created at pre-execution time, but you should\n// not rely on a particular instantiation order.\ntemplate <typename T>\nstruct Singleton {\n public:\n  Singleton& operator=(Singleton&) = delete;\n\n  // Get the singleton\n  static T& Get() {\n    static T singleton;\n    Use(dummy_);\n    return singleton;\n  }\n\n private:\n  // Helpers to force pre-execution time\n  static void Use(const T&) {}\n  static T& dummy_;\n};\n\n// Force instantiation of the singleton at pre-execution time\ntemplate <typename T>\nT& Singleton<T>::dummy_ = Singleton<T>::Get();\n\n}  // namespace gxf\n\n#endif  // NVIDIA_COMMON_SINGLETON_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/span.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_COMMON_SPAN_HPP_\n#define NVIDIA_GXF_COMMON_SPAN_HPP_\n\n#include \"common/byte.hpp\"\n#include \"common/expected.hpp\"\n#include \"common/iterator.hpp\"\n#include \"common/type_utils.hpp\"\n\nnamespace nvidia {\n\nnamespace detail {\n\n/// Type traits to enable comparing `Span<T>` with `Span<const T>`\ntemplate <class T, class U> using EnableIfComparable_t = EnableIf_t<\n  IsSame_v<RemoveConst_t<T>, RemoveConst_t<U>>\n>;\n\n/// Type traits to determine if type stores elements in contiguous memory\ntemplate <class T> using EnableIfContiguous_t = EnableIf_t<Conjunction_v<HasData<T>, HasSize<T>>>;\n\n}  // namespace detail\n\n/// Container to hold a pointer and size for contiguous memory.\n/// `Span` can be passed between functions and components and ensures that the size of the array is\n/// always available and can be checked for safe memory access. `Span` only wraps existing memory.\ntemplate <typename T>\nclass Span {\n public:\n  using element_type     = T;\n  using value_type       = RemoveCV_t<element_type>;\n  using size_type        = size_t;\n  using iterator         = RandomAccessIterator<const Span, element_type>;\n  using reverse_iterator = ReverseIterator<iterator>;\n\n  /// Custom error codes for `Span`\n  enum struct Error {\n    kArgumentOutOfRange,\n    kContainerEmpty,\n  };\n\n  /// `Expected` which uses `Span::Error`\n  template <typename U>\n  using Expected = Expected<U, Error>;\n\n  constexpr Span(T* data, size_t size) : data_{data}, size_{data != nullptr ? size : 0} {}\n  constexpr Span() : Span(nullptr, 0) {}\n  template <size_t N>\n  constexpr Span(T (&array)[N]) : Span(array, N) {}\n  /// Conversion from other types that have `data()` and `size()` functions\n  template <typename U, typename = detail::EnableIfContiguous_t<U>>\n  constexpr Span(U& other) : Span(other.data(), other.size()) {}\n\n  /// Implicit conversion from `Span<T>` to `Span<const T>`\n  operator Span<const T>() const { return Span<const T>(data_, size_); }\n\n  ~Span() = default;\n\n  constexpr Span(const Span& other)      = default;\n  constexpr Span(Span&& other)           = default;\n  constexpr Span& operator=(const Span&) = default;\n  constexpr Span& operator=(Span&&)      = default;\n\n  constexpr iterator         begin()  const { return iterator(*this, 0); }\n  constexpr iterator         end()    const { return iterator(*this, size_); }\n  constexpr reverse_iterator rbegin() const { return reverse_iterator(end()); }\n  constexpr reverse_iterator rend()   const { return reverse_iterator(begin()); }\n\n  constexpr Expected<T&> operator[](size_t index) const { return at(index); }\n\n  /// Returns a pointer to the underlying data\n  constexpr T* data() const { return data_; }\n  /// Returns the number of elements in the `Span`\n  constexpr size_t size() const { return size_; }\n  /// Returns the size of the `Span` in bytes\n  constexpr size_t size_bytes() const { return size_ * sizeof(T); }\n  /// Returns true if the `Span` has no elements\n  constexpr bool empty() const { return size_ == 0; }\n\n  /// Returns a reference to the element at `index`\n  constexpr Expected<T&> at(size_t index) const {\n    return index < size_ ? Expected<T&>{data_[index]}\n                         : Unexpected<Error>{Error::kArgumentOutOfRange};\n  }\n\n  /// Returns a reference to the first element\n  constexpr Expected<T&> front() const {\n    return !empty() ? Expected<T&>{data_[0]}\n                    : Unexpected<Error>{Error::kContainerEmpty};\n  }\n\n  /// Returns a reference to the last element\n  constexpr Expected<T&> back() const {\n    return !empty() ? Expected<T&>{data_[size_ - 1]}\n                    : Unexpected<Error>{Error::kContainerEmpty};\n  }\n\n  /// Returns a `Span` consisting of `count` elements starting from `offset`\n  constexpr Expected<Span> subspan(size_t offset, size_t count) const {\n    return offset <= size_ && count <= size_ && offset + count <= size_\n        ? Expected<Span>{Span(data_ + offset, count)}\n        : Unexpected<Error>{Error::kArgumentOutOfRange};\n  }\n\n  /// Returns a `Span` starting from `offset`\n  constexpr Expected<Span> subspan(size_t offset) const { return subspan(offset, size_ - offset); }\n  /// Returns a `Span` consisting of the first `count` elements\n  constexpr Expected<Span> first(size_t count) const { return subspan(0, count); }\n  /// Returns a `Span` consisting of the last `count` elements\n  constexpr Expected<Span> last(size_t count) const { return subspan(size_ - count, count); }\n\n private:\n  /// Pointer to data\n  T* data_;\n  /// Size of data\n  size_t size_;\n};\n\ntemplate <typename T, typename U, typename = detail::EnableIfComparable_t<T, U>>\nconstexpr bool operator==(const Span<T>& a, const Span<U>& b) {\n  if (a.size() != b.size()) {\n    return false;\n  }\n  if (a.data() == b.data()) {\n    return true;\n  }\n  for (size_t i = 0; i < a.size(); i++) {\n    if (a.data()[i] != b.data()[i]) {\n      return false;\n    }\n  }\n  return true;\n}\n\ntemplate <typename T, typename U, typename = detail::EnableIfComparable_t<T, U>>\nconstexpr bool operator!=(const Span<T>& a, const Span<U>& b) { return !(a == b); }\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_COMMON_SPAN_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/strong_type.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_COMMON_STRONG_TYPE_HPP_\n#define NVIDIA_GXF_COMMON_STRONG_TYPE_HPP_\n\n#include <utility>\n\n#include \"common/type_utils.hpp\"\n\nnamespace nvidia {\n\n/// The base type for a\n/// [strong type](https://www.fluentcpp.com/2016/12/08/strong-types-for-strong-interfaces).\n/// It is useful for making lightweight types which are distinct. Unlike type aliases\n/// (@c typedef or @c using), two @c StrongType types are not substitutable, even if the\n/// backing type is the same.\n///\n/// The primary use case for this type is numeric identifiers. For example, imagine a sensor\n/// system where the sensor is identified by a @c uint32_t. Beyond being a 32-bit integer, this\n/// value shares almost no properties with the C type. Arithmetic and bit shifts do not make a\n/// lot of sense for this value -- there is no meaning to the result of multiplication between\n/// two sensor IDs. Comparing a sensor ID to any non-sensor ID value is probably a bug. By\n/// wrapping ID values into a dedicated @c SensorId @c StrongType, this behavior is prevented.\n///\n/// @code\n/// // Create two types: Foo and Bar, both wrapping a uint64_t\n/// using Foo = StrongType<struct foo_t, uint64_t>;\n/// using Bar = StrongType<struct bar_t, uint64_t>;\n///\n/// void fooOnly(const Foo&);\n/// void barOnly(const Bar&);\n///\n/// int main() {\n///   // Values must be constructed explicitly\n///   auto foo = Foo(106);\n///   auto bar = Bar(314);\n///\n///   fooOnly(foo);         // <- legal\n///   fooOnly(bar);         // <- illegal: a Bar type is distinct from Foo\n///   fooOnly(foo.value()); // <- illegal: no implicit construction\n///   barOnly(foo);         // <- illegal: foo is not a Bar\n/// }\n/// @endcode\ntemplate <typename TName, typename TValue>\nclass StrongType {\n public:\n  /// The underlying type of this type.\n  using value_type = TValue;\n\n  /// Default-construct an instance. This overload is only enabled if\n  /// @ref value_type is default-constructible.\n  constexpr StrongType() = default;\n\n  /// Construct an instance from @a value.\n  ///\n  /// @param value The value to convert from.\n  template <typename UValue>\n  constexpr explicit\n  StrongType(UValue&& value) noexcept(IsNothrowConstructible_v<value_type, UValue>)\n      : value_(std::forward<UValue>(value)) {}\n\n  constexpr explicit operator value_type() const& noexcept { return value_; }\n  constexpr explicit operator value_type() &      noexcept { return value_; }\n  constexpr explicit operator value_type() &&     noexcept { return value_; }\n\n  constexpr value_type const& value() const &    noexcept { return value_; }\n  constexpr value_type&       value() &          noexcept { return value_; }\n  constexpr value_type&&      value() &&         noexcept { return std::move(value_); }\n  constexpr value_type const* operator->() const noexcept { return &value_; }\n  constexpr value_type*       operator->()       noexcept { return &value_; }\n\n  /// Comparison operatorions for the underlying type.\n  friend constexpr bool operator==(const StrongType& lhs, const StrongType& rhs) {\n    return lhs.value() == rhs.value();\n  }\n  friend constexpr bool operator!=(const StrongType& lhs, const StrongType& rhs) {\n    return lhs.value() != rhs.value();\n  }\n  friend constexpr bool operator<=(const StrongType& lhs, const StrongType& rhs) {\n    return lhs.value() <= rhs.value();\n  }\n  friend constexpr bool operator>=(const StrongType& lhs, const StrongType& rhs) {\n    return lhs.value() >= rhs.value();\n  }\n  friend constexpr bool operator<(const StrongType& lhs, const StrongType& rhs) {\n    return lhs.value() < rhs.value();\n  }\n  friend constexpr bool operator>(const StrongType& lhs, const StrongType& rhs) {\n    return lhs.value() > rhs.value();\n  }\n\n private:\n  /// The underlying value of this type.\n  value_type value_;\n};\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_COMMON_STRONG_TYPE_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/type_name.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_TYPE_NAME_HPP_\n#define NVIDIA_COMMON_TYPE_NAME_HPP_\n\n#include <cstdint>\n\n#if defined(__clang__)\ninline char* TypenameAsStringImpl(const char* begin, char* output, int32_t max_length) {\n  // Return nullptr since this is not implemented\n  return nullptr;\n}\n#elif defined(__GNUC__)\n#include \"type_name_gnuc.hpp\"\n#elif defined(_MSC_VER)\n// Not yet implemented\n#endif\n\nnamespace nvidia {\nnamespace helper {\n/**\n * Returns the pretty function name. It includes the complete function name along with the namespace\n * for template parameters.\n * Compiler using gcc-13 or later strips the namespace of the template parameter if\n * PRETTY_FUNCTION is called in the same namespace as that of the namespace of template parameter.\n * Compiler does not strip the namespace if the function that calls the PRETTY_FUNCTION macro is\n * called in a different namespace.\n *\n * @return the pretty function name as a const char*\n */\n  template <typename>\n  const char* PrettyFunctionName() {\n    return __PRETTY_FUNCTION__;\n  }\n\n/**\n * Returns the sizeof the pretty function name.\n */\n  template <typename>\n  constexpr int32_t PrettyFunctionSize() {\n    return sizeof(__PRETTY_FUNCTION__);\n  }\n}  // namespace helper\n}  // namespace nvidia\n\nnamespace nvidia {\n// Gives a string representation of the name of a C++ type.\n//\n// The function will compute the typename during the first invocation and store it in heap memory.\n// Computation of the typename is linear in the length of the typename and does not involve dynamic\n// memory allocations.\n//\n// For example:\n//   namespace foo {\n//     struct Bar {};\n//   }  // namespace\n//   TyenameAsString<foo::Bar>();  // returns \"foo::Bar\"\n//\n// Note: Only \"simple\" class types in global namespace or a \"normal\" namespaces are guaranteed to\n// work. Templates, lambdas and anonymous namespaces are not guaranteed to work as expected.\ntemplate <typename T>\nconst char* TypenameAsString() {\n  // Ideally the typename string would be computed at compile time, however this does not seem to\n  // be possible. Thus the typename string is computed the first time this function is evaluated\n  // and cached for future function evaluations.\n  const char* template_name = nvidia::helper::PrettyFunctionName<T>();\n  // Use upper bound to be safe.\n  constexpr int32_t kMaxNameLength = nvidia::helper::PrettyFunctionSize<T>();\n  static char s_name[kMaxNameLength] = {0};  // Initialize with 0 to get a null-terminated string.\n  static char* result = s_name;\n  if (s_name[0] == 0 && result != nullptr) {  // Check for first invocation of this function.\n    result = TypenameAsStringImpl(template_name, s_name, kMaxNameLength);\n  }\n  return result;\n}\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_COMMON_TYPE_NAME_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/type_name_gnuc.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_TYPE_NAME_GNUC_HPP_\n#define NVIDIA_COMMON_TYPE_NAME_GNUC_HPP_\n\n#include <cstdint>\n\nnamespace nvidia {\n\n// For the GNU compiler __PRETTY_FUNCTION__ is a null-terminated string with the following form:\n//   const char* nvidia::TypenameAsString() [with T = nvidia::gxf::Component]\n// We would like to extract the type name as \"nvidia::gxf::Component\".\n// The result is stored in the given string 'output'. If the type name is longer than max_length\n// or another error occurs the function returns nullptr; otherwise 'output' is returned.\nchar* TypenameAsStringGnuC(const char* begin, char* output, int32_t max_length);\n\n// Compiler-specific implementation of the TypenameAsString function.\ninline char* TypenameAsStringImpl(const char* begin, char* output, int32_t max_length) {\n  return TypenameAsStringGnuC(begin, output, max_length);\n}\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_COMMON_TYPE_NAME_GNUC_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/type_utils.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_COMMON_TYPE_UTILS_HPP_\n#define NVIDIA_COMMON_TYPE_UTILS_HPP_\n\n#include <cstdint>\n\n// GXF definitions for common <type_traits> class definitions. These should behave identically\n// ISO standard definitions. They are defined locally to minimize dependencies of core GXF code\n// on any stl implementations.\nnamespace nvidia {\n\n// https://en.cppreference.com/w/cpp/types/void_t\ntemplate <class...> using void_t = void;\n\n// https://en.cppreference.com/w/cpp/types/type_identity\ntemplate <class T> struct TypeIdentity { using type = T; };\n\ntemplate <class T> using TypeIdentity_t = typename TypeIdentity<T>::type;\n\n// https://en.cppreference.com/w/cpp/types/integral_constant\ntemplate <class T, T v> struct IntegralConstant { static constexpr T value = v; };\ntemplate <bool B>       struct BoolConstant : IntegralConstant<bool, B> {};\n\nstruct TrueType  : BoolConstant<true>  {};\nstruct FalseType : BoolConstant<false> {};\n\n// https://en.cppreference.com/w/cpp/types/conditional\ntemplate <bool B, class T, class F> struct Conditional              : TypeIdentity<T> {};\ntemplate<class T, class F>          struct Conditional<false, T, F> : TypeIdentity<F> {};\n\ntemplate <bool B, class T, class F> using Conditional_t = typename Conditional<B, T, F>::type;\n\n// https://en.cppreference.com/w/cpp/types/conjunction\ntemplate <class...>\nstruct Conjunction : TrueType {};\n\ntemplate <class B, class... Bs>\nstruct Conjunction<B, Bs...> : Conditional<B::value, Conjunction<Bs...>, B>::type {};\n\ntemplate <class... Bs> constexpr bool Conjunction_v = Conjunction<Bs...>::value;\n\n// https://en.cppreference.com/w/cpp/types/disjunction\ntemplate <class...>\nstruct Disjunction : FalseType {};\n\ntemplate <class B, class... Bs>\nstruct Disjunction<B, Bs...> : Conditional<B::value, B, Disjunction<Bs...>>::type {};\n\ntemplate <class... Bs> constexpr bool Disjunction_v = Disjunction<Bs...>::value;\n\n// https://en.cppreference.com/w/cpp/types/negation\ntemplate <class B> struct Negation : BoolConstant<!static_cast<bool>(B::value)> {};\n\ntemplate <class B> constexpr bool Negation_v = Negation<B>::value;\n\n// https://en.cppreference.com/w/cpp/types/remove_reference\ntemplate <class T> struct RemoveReference      : TypeIdentity<T> {};\ntemplate <class T> struct RemoveReference<T&>  : TypeIdentity<T> {};\ntemplate <class T> struct RemoveReference<T&&> : TypeIdentity<T> {};\n\ntemplate <class T> using RemoveReference_t = typename RemoveReference<T>::type;\n\n// https://en.cppreference.com/w/cpp/types/remove_cv\ntemplate <class T> struct RemoveCV                   : TypeIdentity<T> {};\ntemplate <class T> struct RemoveCV<const T>          : TypeIdentity<T> {};\ntemplate <class T> struct RemoveCV<volatile T>       : TypeIdentity<T> {};\ntemplate <class T> struct RemoveCV<const volatile T> : TypeIdentity<T> {};\n\ntemplate <class T> using RemoveCV_t = typename RemoveCV<T>::type;\n\ntemplate <class T> struct RemoveConst                : TypeIdentity<T> {};\ntemplate <class T> struct RemoveConst<const T>       : TypeIdentity<T> {};\n\ntemplate <class T> using RemoveConst_t = typename RemoveConst<T>::type;\n\ntemplate <class T> struct RemoveVolatile             : TypeIdentity<T> {};\ntemplate <class T> struct RemoveVolatile<volatile T> : TypeIdentity<T> {};\n\ntemplate <class T> using RemoveVolatile_t = typename RemoveVolatile<T>::type;\n\n// https://en.cppreference.com/w/cpp/types/remove_cvref\ntemplate <class T> struct RemoveCVRef : RemoveCV<typename RemoveReference<T>::type>{};\n\ntemplate <class T> using RemoveCVRef_t = typename RemoveCVRef<T>::type;\n\n// https://en.cppreference.com/w/cpp/types/add_reference\ntemplate <class T>\nstruct AddLvalueReference {\n private:\n  template <class U> static constexpr TypeIdentity<U&> Test(void*);\n  template <class U> static constexpr TypeIdentity<U>  Test(...);\n public:\n    using type = typename decltype(Test<T>(nullptr))::type;\n};\n\ntemplate <class T>\nstruct AddRvalueReference {\n private:\n  template <class U> static constexpr TypeIdentity<U&&> Test(void*);\n  template <class U> static constexpr TypeIdentity<U>   Test(...);\n public:\n    using type = typename decltype(Test<T>(nullptr))::type;\n};\n\ntemplate <class T> using AddLvalueReference_t = typename AddLvalueReference<T>::type;\ntemplate <class T> using AddRvalueReference_t = typename AddRvalueReference<T>::type;\n\n// https://en.cppreference.com/w/cpp/utility/declval\ntemplate <class T>\nAddRvalueReference_t<T> Declval() noexcept {\n  static_assert(Conditional_t<false, T, FalseType>::value,\n      \"Declval() cannot be used in an evaluated context.\");\n}\n\n// https://en.cppreference.com/w/cpp/types/decay\ntemplate <class T>\nstruct Decay {\n private:\n  template <class U> static constexpr auto Id(U u) noexcept { return u; }\n  template <class U> static constexpr decltype(Id(Declval<U>())) Test(void*);\n  template <class>   static constexpr void Test(...) {}\n public:\n    using type = decltype(Test<T>(nullptr));\n};\n\ntemplate <class T> using Decay_t = typename Decay<T>::type;\n\n// https://en.cppreference.com/w/cpp/types/enable_if\ntemplate <bool, class = void> struct EnableIf {};\ntemplate <class T>            struct EnableIf<true, T> : TypeIdentity<T> {};\n\ntemplate <bool B, class T = void> using EnableIf_t = typename EnableIf<B, T>::type;\n\n// https://en.cppreference.com/w/cpp/types/is_same\ntemplate <class, class> struct IsSame       : FalseType {};\ntemplate <class T>      struct IsSame<T, T> : TrueType  {};\n\ntemplate <class T, class U> constexpr bool IsSame_v = IsSame<T, U>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_void\ntemplate <class T> struct IsVoid : IsSame<void, typename RemoveCV<T>::type> {};\n\ntemplate <class T> constexpr bool IsVoid_v = IsVoid<T>::value;\n\n// // https://en.cppreference.com/w/cpp/types/is_const\ntemplate <class T> struct IsConst          : FalseType {};\ntemplate <class T> struct IsConst<const T> : TrueType {};\n\ntemplate <class T> constexpr bool IsConst_v = IsConst<T>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_reference\ntemplate <class>   struct IsReference      : FalseType {};\ntemplate <class T> struct IsReference<T&>  : TrueType  {};\ntemplate <class T> struct IsReference<T&&> : TrueType  {};\n\ntemplate <class T> constexpr bool IsReference_v = IsReference<T>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_constructible\ntemplate <class T, class... Args>\nstruct IsConstructible {\n private:\n  template<class V, class = decltype(static_cast<T>(Declval<V>()))>\n  static constexpr bool TestCast(void*) { return true; }\n\n  template<class...>\n  static constexpr bool TestCast(...) { return false; }\n\n  template <int, class, class = void>\n  struct Test : FalseType {};\n\n  template <class U>\n  struct Test<1, U, void> : BoolConstant<TestCast<Args...>(nullptr)> {};\n\n  template <int I, class U>\n  struct Test<I, U, void_t<decltype(new U(Declval<Args>()...))>> : TrueType {};\n\n public:\n  static constexpr bool value = Test<sizeof...(Args), T>::value;\n};\n\ntemplate <class T, class... Args>\nconstexpr bool IsConstructible_v = IsConstructible<T, Args...>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_default_constructible\ntemplate <class T>\nusing IsDefaultConstructible = IsConstructible<T>;\n\ntemplate <class T>\nconstexpr bool IsDefaultConstructible_v = IsDefaultConstructible<T>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_nothrow_constructible\ntemplate <class T, class... Args>\nstruct IsNothrowConstructible {\n private:\n  template<class V, bool Result = noexcept(static_cast<T>(Declval<V>()))>\n  static constexpr bool TestCast(void*) { return Result; }\n\n  template<class...>\n  static constexpr bool TestCast(...) { return false; }\n\n  template<int, class, class = void>\n  struct Test : FalseType {};\n\n  template<class U>\n  struct Test<0, U, void_t<decltype(U())>> : BoolConstant<noexcept(U())> {};\n\n  template<class U>\n  struct Test<1, U, void> : BoolConstant<TestCast<Args...>(nullptr)> {};\n\n  template<int I, class U>\n  struct Test<I, U, void_t<decltype(new U(Declval<Args>()...))>>\n      : BoolConstant<noexcept(new U(Declval<Args>()...))> {};\n\n public:\n  static constexpr bool value = Test<sizeof...(Args), T>::value;\n};\n\ntemplate <class T, class U>\nconstexpr bool IsNothrowConstructible_v = IsNothrowConstructible<T, U>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_nothrow_default_constructible\ntemplate <class T>\nusing IsNothrowDefaultConstructible = IsNothrowConstructible<T>;\n\ntemplate <class T>\nconstexpr bool IsNothrowDefaultConstructible_v = IsNothrowDefaultConstructible<T>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_convertible\ntemplate <class From, class To>\nstruct IsConvertible {\n private:\n  template <class F, class T, class = decltype(Declval<T(&)(T)>()(Declval<F>()))>\n  static constexpr bool Test(void*) { return true; }\n  template <class, class>\n  static constexpr bool Test(...) { return false; }\n public:\n  static constexpr bool value = (IsVoid_v<From> && IsVoid_v<To>) || Test<From, To>(nullptr);\n};\n\ntemplate <class From, class To>\nconstexpr bool IsConvertible_v = IsConvertible<From, To>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_convertible\ntemplate <class From, class To>\nstruct IsNothrowConvertible {\n private:\n  template <class T>\n  static constexpr T DecayFunc(T) noexcept;\n  template <class F, class T, bool Result = noexcept(DecayFunc<T>(Declval<F>()))>\n  static constexpr bool Test(void*) { return Result; }\n  template <class, class>\n  static constexpr bool Test(...) { return false; }\n public:\n  static constexpr bool value = (IsVoid_v<From> && IsVoid_v<To>) || Test<From, To>(nullptr);\n};\n\ntemplate <class From, class To>\nconstexpr bool IsNothrowConvertible_v = IsNothrowConvertible<From, To>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_assignable\ntemplate <class, class, class = void>\nstruct IsAssignable : FalseType {};\n\ntemplate <class T, class U>\nstruct IsAssignable<T, U, void_t<decltype(Declval<T>() = Declval<U>())>> : TrueType{};\n\ntemplate <class T, class U> constexpr bool IsAssignable_v = IsAssignable<T, U>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_integral\ntemplate <class T>\nstruct IsIntegral : BoolConstant<\n    IsSame_v<bool,     typename RemoveCV<T>::type>  ||\n    IsSame_v<int8_t,   typename RemoveCV<T>::type>  ||\n    IsSame_v<uint8_t,  typename RemoveCV<T>::type>  ||\n    IsSame_v<int16_t,  typename RemoveCV<T>::type>  ||\n    IsSame_v<uint16_t, typename RemoveCV<T>::type>  ||\n    IsSame_v<int32_t,  typename RemoveCV<T>::type>  ||\n    IsSame_v<uint32_t, typename RemoveCV<T>::type>  ||\n    IsSame_v<int64_t,  typename RemoveCV<T>::type>  ||\n    IsSame_v<uint64_t, typename RemoveCV<T>::type>> {};\n\ntemplate <class T> constexpr bool IsIntegral_v = IsIntegral<T>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_floating_point\ntemplate <class T>\nstruct IsFloatingPoint : BoolConstant<\n    IsSame_v<float,       typename RemoveCV<T>::type>  ||\n    IsSame_v<double,      typename RemoveCV<T>::type>  ||\n    IsSame_v<long double, typename RemoveCV<T>::type>> {};\n\ntemplate <class T> constexpr bool IsFloatingPoint_v = IsFloatingPoint<T>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_arithmetic\ntemplate <class T>\nstruct IsArithmetic : BoolConstant<IsIntegral_v<T> || IsFloatingPoint_v<T>> {};\n\ntemplate <class T> constexpr bool IsArithmetic_v = IsArithmetic<T>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_signed\ntemplate <class T, bool = IsArithmetic_v<T>> struct IsSigned : BoolConstant<(T(-1) < T(0))> {};\ntemplate <class T> struct IsSigned<T, false> : FalseType {};\ntemplate <class T> constexpr bool IsSigned_v = IsSigned<T>::value;\n\n// https://en.cppreference.com/w/cpp/types/is_unsigned\ntemplate <class T, bool = IsArithmetic_v<T>> struct IsUnsigned : BoolConstant<(T(0) < T(-1))> {};\ntemplate <class T> struct IsUnsigned<T, false> : FalseType {};\ntemplate <class T> constexpr bool IsUnsigned_v = IsUnsigned<T>::value;\n\n// https://en.cppreference.com/w/cpp/types/make_signed\ntemplate <class T, class = EnableIf_t<IsIntegral_v<T>>> struct MakeSigned : TypeIdentity<T> {};\ntemplate <> struct MakeSigned<uint8_t>  : TypeIdentity<int8_t>  {};\ntemplate <> struct MakeSigned<uint16_t> : TypeIdentity<int16_t> {};\ntemplate <> struct MakeSigned<uint32_t> : TypeIdentity<int32_t> {};\ntemplate <> struct MakeSigned<uint64_t> : TypeIdentity<int64_t> {};\ntemplate <class T> using MakeSigned_t = typename MakeSigned<T>::type;\n\n// https://en.cppreference.com/w/cpp/types/make_unsigned\ntemplate <class T, class = EnableIf_t<IsIntegral_v<T>>> struct MakeUnsigned : TypeIdentity<T> {};\ntemplate <> struct MakeUnsigned<int8_t>  : TypeIdentity<uint8_t>  {};\ntemplate <> struct MakeUnsigned<int16_t> : TypeIdentity<uint16_t> {};\ntemplate <> struct MakeUnsigned<int32_t> : TypeIdentity<uint32_t> {};\ntemplate <> struct MakeUnsigned<int64_t> : TypeIdentity<uint64_t> {};\ntemplate <class T> using MakeUnsigned_t = typename MakeUnsigned<T>::type;\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_COMMON_TYPE_UTILS_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/unique_index_map.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_COMMON_UNIQUE_INDEX_MAP_HPP_\n#define NVIDIA_GXF_COMMON_UNIQUE_INDEX_MAP_HPP_\n\n#include \"common/expected.hpp\"\n\nnamespace nvidia {\n\n// Provides a basic fixed capacity container for tracking objects and assigning a unique id to every\n// object inserted. It provides constant time insert/delete/access for all elements with the\n// generated uid. This container guarantees that uids are never reused, and will return an\n// Unexpected Error if it runs out of capacity.\ntemplate <typename T>\nclass UniqueIndexMap {\n private:\n  // number of bits to use for holding the version number\n  static constexpr uint64_t kVersionSize = 32;\n  // number of bits to use for holding the index into the underlying container\n  static constexpr uint64_t kIndexSize = 32;\n  // bitmask for Version number\n  static constexpr uint64_t kVersionMask = ((1ULL << kVersionSize) - 1ULL) << kIndexSize;\n  // bitmask for Index number\n  static constexpr uint64_t kIndexMask = (1ULL << kIndexSize) - 1ULL;\n\n  static_assert(kIndexSize + kVersionSize == 64, \"Invalid UID bit length\");\n  static_assert((kVersionMask ^ kIndexMask) == 0xFFFFFFFFFFFFFFFF,\n                \"bitmasks must cover full size of uint64_t\");\n\n public:\n  // Custom Error codes for construction and access of UniqueIndexMap map.\n  enum struct Error {\n    kArgumentInvalid,  // Invalid construction/initialization parameter\n    kOutOfMemory,      // Could not allocate memory during initialization\n    kContainerFull,    // UniqueIndexMap is full\n    kNotFound          // UID does not exits in UniqueIndexMap\n  };\n\n  // Expected type for UniqueIndexMap which uses class specific Error type.\n  template <typename U>\n  using Expected = Expected<U, Error>;\n\n  UniqueIndexMap() : uids_{nullptr}, data_{nullptr}, capacity_{0}, size_{0}, next_{0} {}\n  UniqueIndexMap(const UniqueIndexMap<T>&) = delete;\n  UniqueIndexMap(UniqueIndexMap<T>&& other)\n      : uids_{other.uids_},\n        data_{other.data_},\n        capacity_{other.capacity_},\n        size_{other.size_},\n        next_{other.next_} {\n    other.reset_values();\n  }\n\n  UniqueIndexMap<T>& operator=(const UniqueIndexMap<T>& other) = delete;\n  UniqueIndexMap<T>& operator=(UniqueIndexMap<T>&& other) {\n    deinitialize();\n    uids_ = other.uids_;\n    data_ = other.data_;\n    capacity_ = other.capacity_;\n    size_ = other.size_;\n    next_ = other.next_;\n\n    other.reset_values();\n    return *this;\n  }\n\n  ~UniqueIndexMap() {\n    deinitialize();\n  }\n\n  // Deallocates the memory allocated in initialize\n  void deinitialize() {\n    for (uint64_t i = 0; i < capacity_; ++i) {\n      if (GetIndex(uids_[i]) == i) {\n        data_[i].~T();\n      }\n    }\n\n    delete[] uids_;\n    ::operator delete(data_);\n    reset_values();\n  }\n\n  // Allocate memory up to the provided capacity and initialize values for implicit free list.\n  // Returns Unexpected<Error> if max_capacity is too large.\n  Expected<void> initialize(uint64_t max_capacity) {\n    if (max_capacity > (1ULL << kIndexSize)) {\n      return Unexpected<Error>{Error::kArgumentInvalid};\n    }\n\n    capacity_ = max_capacity;\n\n    uids_ = new (std::nothrow) uint64_t[max_capacity];\n    data_ = static_cast<T*>(::operator new(max_capacity * sizeof(T), std::nothrow));\n\n    if (uids_ == nullptr || data_ == nullptr) {\n      return Unexpected<Error>{Error::kOutOfMemory};\n    }\n\n    for (uint64_t i = 0; i < max_capacity; ++i) {\n      uids_[i] = i + 1;\n    }\n    return {};\n  }\n\n  // Check if there exists an object with the provided UID. Returns true if the uid exists in the\n  // allocated range and if the version numbers match.\n  bool valid(uint64_t uid) const {\n    const uint64_t idx = GetIndex(uid);\n    return (idx < capacity_) && (uids_[idx] == uid);\n  }\n\n  // Insert a new object into the container and return the generated UID via move constructor.\n  // Return Unexpected<Error> if the object cannot be inserted.\n  Expected<uint64_t> insert(T&& object) { return emplace(static_cast<T&&>(object)); }\n\n  // Insert a new object into the container and return the generated UID via copy constructor.\n  // Return Unexpected<Error> if the object cannot be inserted.\n  Expected<uint64_t> insert(const T& object) { return emplace(object); }\n\n  // Construct a new object with the provided arguments directly in the container, and return the\n  // generated UID via copy. Return Unexpected<Error> if the object cannot be inserted.\n  template <typename... Args>\n  Expected<uint64_t> emplace(Args&&... args) {\n    if (size_ == capacity_) {\n      return Unexpected<Error>{Error::kOutOfMemory};\n    }\n\n    if (GetVersion(next_) == (1ULL << kVersionSize) - 1ULL) {\n      return Unexpected<Error>{Error::kContainerFull};\n    }\n\n    const uint64_t idx = GetIndex(next_);\n    const uint64_t temp = next_;\n    next_ = uids_[idx];\n    uids_[idx] = temp + (1ULL << kIndexSize);\n    new (&data_[idx]) T{static_cast<Args&&>(args)...};\n    ++size_;\n    return uids_[idx];\n  }\n\n  // Erase the object tracked by UID. Return Unexpected<Error> if UID is not valid.\n  Expected<void> erase(uint64_t uid) {\n    if (!valid(uid)) {\n      return Unexpected<Error>{Error::kNotFound};\n    }\n\n    uint64_t idx = GetIndex(uid);\n    uids_[idx] = next_;\n    next_ = uid;\n    --size_;\n    data_[idx].~T();\n\n    return {};\n  }\n\n  // Get a pointer to the object tracked by UID. Returns an error if UID is not valid.\n  Expected<T*> try_get(uint64_t uid) {\n    if (!valid(uid)) {\n      return Unexpected<Error>{Error::kNotFound};\n    }\n    return &data_[GetIndex(uid)];\n  }\n\n  // Get a pointer to the object tracked by UID. Returns an error if UID is not valid.\n  Expected<const T*> try_get(uint64_t uid) const {\n    if (!valid(uid)) {\n      return Unexpected<Error>{Error::kNotFound};\n    }\n    return &data_[GetIndex(uid)];\n  }\n\n  // Performs a linear search and returns the UID of the object.\n  // Returns an error if object is not found\n  Expected<uint64_t> find(const T& object) const {\n    for (size_t i = 0; i < capacity_; i++) {\n      if (valid(uids_[i]) && data_[i] == object) {\n        return uids_[i];\n      }\n    }\n    return Unexpected<Error>{Error::kNotFound};\n  }\n\n  // Maximum capacity of the unique index map.\n  inline uint64_t capacity() const { return capacity_; }\n\n  // Current number of elements in the container.\n  inline uint64_t size() const { return size_; }\n\n private:\n  static uint64_t GetVersion(uint64_t uid) { return uid >> kIndexSize; }\n  static uint64_t GetIndex(uint64_t uid) { return uid & kIndexMask; }\n\n  // Resets all pointers and storage parameters to zero, without calling delete, primarily to be\n  // used with move operators.\n  void reset_values() {\n    uids_ = nullptr;\n    data_ = nullptr;\n    capacity_ = 0;\n    size_ = 0;\n    next_ = 0;\n  }\n\n  uint64_t* uids_;     // ids with implicit list of free elements\n  T* data_;            // objects storage\n  uint64_t capacity_;  // maximum number of concurrent objects\n  uint64_t size_;      // current number of objects\n  uint64_t next_;      // next \"free\" element in the container\n};\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_COMMON_UNIQUE_INDEX_MAP_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/common/yaml_parser.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_COMMON_YAML_PARSER_HPP_\n#define NVIDIA_GXF_COMMON_YAML_PARSER_HPP_\n\n#include <complex>\n#include <regex>\n#include <sstream>\n#include <string>\n\n#include \"common/logger.hpp\"\n#include \"yaml-cpp/yaml.h\"\n\n\ninline static bool is_space(unsigned char c) {\n  return c == ' ';\n}\n\n/**\n * Custom YAML parser for std::complex types\n *\n * Handles parsing of strings containing a complex floating point value.\n *\n * Examples of valid strings are:\n *    \"1.0 + 2.5j\"\n *    \"-1.0 - 3i\"\n *    \"1+3.3j\"\n *\n * There may be 0 or 1 space between a + or - sign and the digits.\n * Either \"i\" or \"j\" must appear immediately after the second number.\n */\ntemplate <typename T>\nstruct YAML::convert<std::complex<T>> {\n  static Node encode(const std::complex<T>& data) {\n    std::stringstream ss;\n    ss << data.real();\n    if (data.imag() >= 0) {\n      ss << '+';\n    }\n    ss << data.imag() << 'j';\n    Node node = YAML::Load(ss.str());\n    return node;\n  }\n\n  static bool decode(const Node& node, std::complex<T>& data) {\n    if (!node.IsScalar()) {\n      GXF_LOG_ERROR(\"complex<T> decode: expected a scalar\");\n      return false;\n    }\n    std::string value = node.as<std::string>();\n\n    std::regex complex_reg(\"\\\\s*([+-]?\\\\s?\\\\d*\\\\.?\\\\d+)\\\\s?([+-]{1}\\\\s?\\\\d*\\\\.?\\\\d+)[ij]{1}\\\\s*$\");\n    std::smatch m;\n    if (std::regex_search(value, m, complex_reg)) {\n      if (m.size() != 3) {\n        GXF_LOG_ERROR(\"unexpected match size: [%ld],  matched: [%s]\", m.size(), m.str(0).c_str());\n      }\n      // extract the real and imaginary components of the number\n      std::string real_str = m.str(1);\n      std::string imag_str = m.str(2);\n\n      // remove any white space around + or - (necessary for std::stod to work)\n      real_str.erase(std::remove_if(real_str.begin(), real_str.end(), is_space), real_str.end());\n      imag_str.erase(std::remove_if(imag_str.begin(), imag_str.end(), is_space), imag_str.end());\n\n      // format real and imaginary strings as floating point\n      double real = std::stod(real_str);\n      double imag = std::stod(imag_str);\n      data = std::complex<T>(real, imag);\n    } else {\n      GXF_LOG_ERROR(\"failed to match expected regex for complex<T>\");\n      return false;\n    }\n    return true;\n  }\n};\n\n\n// operator overload for std::complex<T>\ntemplate <typename T>\nYAML::Emitter& operator<<(YAML::Emitter& out, const std::complex<T>& c) {\n  std::stringstream ss;\n  ss << c.real();\n  if (c.imag() >= 0) ss << '+';\n  ss << c.imag() << 'j';\n  out << ss.str();\n  return out;\n}\n\n#endif  // NVIDIA_GXF_COMMON_YAML_PARSER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/dlpack/dlpack.h",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2017-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef DLPACK_DLPACK_H_\n#define DLPACK_DLPACK_H_\n\n/**\n * \\brief Compatibility with C++\n */\n#ifdef __cplusplus\n#define DLPACK_EXTERN_C extern \"C\"\n#else\n#define DLPACK_EXTERN_C\n#endif\n\n/*! \\brief The current version of dlpack */\n#define DLPACK_VERSION 80\n\n/*! \\brief The current ABI version of dlpack */\n#define DLPACK_ABI_VERSION 1\n\n/*! \\brief DLPACK_DLL prefix for windows */\n#ifdef _WIN32\n#ifdef DLPACK_EXPORTS\n#define DLPACK_DLL __declspec(dllexport)\n#else\n#define DLPACK_DLL __declspec(dllimport)\n#endif\n#else\n#define DLPACK_DLL\n#endif\n\n#include <stdint.h>\n#include <stddef.h>\n\n#ifdef __cplusplus\nextern \"C\" {\n#endif\n/*!\n * \\brief The device type in DLDevice.\n */\n#ifdef __cplusplus\ntypedef enum : int32_t {\n#else\ntypedef enum {\n#endif\n  /*! \\brief CPU device */\n  kDLCPU = 1,\n  /*! \\brief CUDA GPU device */\n  kDLCUDA = 2,\n  /*!\n   * \\brief Pinned CUDA CPU memory by cudaMallocHost\n   */\n  kDLCUDAHost = 3,\n  /*! \\brief OpenCL devices. */\n  kDLOpenCL = 4,\n  /*! \\brief Vulkan buffer for next generation graphics. */\n  kDLVulkan = 7,\n  /*! \\brief Metal for Apple GPU. */\n  kDLMetal = 8,\n  /*! \\brief Verilog simulator buffer */\n  kDLVPI = 9,\n  /*! \\brief ROCm GPUs for AMD GPUs */\n  kDLROCM = 10,\n  /*!\n   * \\brief Pinned ROCm CPU memory allocated by hipMallocHost\n   */\n  kDLROCMHost = 11,\n  /*!\n   * \\brief Reserved extension device type,\n   * used for quickly test extension device\n   * The semantics can differ depending on the implementation.\n   */\n  kDLExtDev = 12,\n  /*!\n   * \\brief CUDA managed/unified memory allocated by cudaMallocManaged\n   */\n  kDLCUDAManaged = 13,\n  /*!\n   * \\brief Unified shared memory allocated on a oneAPI non-partititioned\n   * device. Call to oneAPI runtime is required to determine the device\n   * type, the USM allocation type and the sycl context it is bound to.\n   *\n   */\n  kDLOneAPI = 14,\n  /*! \\brief GPU support for next generation WebGPU standard. */\n  kDLWebGPU = 15,\n  /*! \\brief Qualcomm Hexagon DSP */\n  kDLHexagon = 16,\n} DLDeviceType;\n\n/*!\n * \\brief A Device for Tensor and operator.\n */\ntypedef struct {\n  /*! \\brief The device type used in the device. */\n  DLDeviceType device_type;\n  /*!\n   * \\brief The device index.\n   * For vanilla CPU memory, pinned memory, or managed memory, this is set to 0.\n   */\n  int32_t device_id;\n} DLDevice;\n\n/*!\n * \\brief The type code options DLDataType.\n */\ntypedef enum {\n  /*! \\brief signed integer */\n  kDLInt = 0U,\n  /*! \\brief unsigned integer */\n  kDLUInt = 1U,\n  /*! \\brief IEEE floating point */\n  kDLFloat = 2U,\n  /*!\n   * \\brief Opaque handle type, reserved for testing purposes.\n   * Frameworks need to agree on the handle data type for the exchange to be well-defined.\n   */\n  kDLOpaqueHandle = 3U,\n  /*! \\brief bfloat16 */\n  kDLBfloat = 4U,\n  /*!\n   * \\brief complex number\n   * (C/C++/Python layout: compact struct per complex number)\n   */\n  kDLComplex = 5U,\n  /*! \\brief boolean */\n  kDLBool = 6U,\n} DLDataTypeCode;\n\n/*!\n * \\brief The data type the tensor can hold. The data type is assumed to follow the\n * native endian-ness. An explicit error message should be raised when attempting to\n * export an array with non-native endianness\n *\n *  Examples\n *   - float: type_code = 2, bits = 32, lanes = 1\n *   - float4(vectorized 4 float): type_code = 2, bits = 32, lanes = 4\n *   - int8: type_code = 0, bits = 8, lanes = 1\n *   - std::complex<float>: type_code = 5, bits = 64, lanes = 1\n *   - bool: type_code = 6, bits = 8, lanes = 1 (as per common array library convention, the underlying storage size of bool is 8 bits)\n */\ntypedef struct {\n  /*!\n   * \\brief Type code of base types.\n   * We keep it uint8_t instead of DLDataTypeCode for minimal memory\n   * footprint, but the value should be one of DLDataTypeCode enum values.\n   * */\n  uint8_t code;\n  /*!\n   * \\brief Number of bits, common choices are 8, 16, 32.\n   */\n  uint8_t bits;\n  /*! \\brief Number of lanes in the type, used for vector types. */\n  uint16_t lanes;\n} DLDataType;\n\n/*!\n * \\brief Plain C Tensor object, does not manage memory.\n */\ntypedef struct {\n  /*!\n   * \\brief The data pointer points to the allocated data. This will be CUDA\n   * device pointer or cl_mem handle in OpenCL. It may be opaque on some device\n   * types. This pointer is always aligned to 256 bytes as in CUDA. The\n   * `byte_offset` field should be used to point to the beginning of the data.\n   *\n   * Note that as of Nov 2021, multiply libraries (CuPy, PyTorch, TensorFlow,\n   * TVM, perhaps others) do not adhere to this 256 byte aligment requirement\n   * on CPU/CUDA/ROCm, and always use `byte_offset=0`.  This must be fixed\n   * (after which this note will be updated); at the moment it is recommended\n   * to not rely on the data pointer being correctly aligned.\n   *\n   * For given DLTensor, the size of memory required to store the contents of\n   * data is calculated as follows:\n   *\n   * \\code{.c}\n   * static inline size_t GetDataSize(const DLTensor* t) {\n   *   size_t size = 1;\n   *   for (tvm_index_t i = 0; i < t->ndim; ++i) {\n   *     size *= t->shape[i];\n   *   }\n   *   size *= (t->dtype.bits * t->dtype.lanes + 7) / 8;\n   *   return size;\n   * }\n   * \\endcode\n   */\n  void* data;\n  /*! \\brief The device of the tensor */\n  DLDevice device;\n  /*! \\brief Number of dimensions */\n  int32_t ndim;\n  /*! \\brief The data type of the pointer*/\n  DLDataType dtype;\n  /*! \\brief The shape of the tensor */\n  int64_t* shape;\n  /*!\n   * \\brief strides of the tensor (in number of elements, not bytes)\n   *  can be NULL, indicating tensor is compact and row-majored.\n   */\n  int64_t* strides;\n  /*! \\brief The offset in bytes to the beginning pointer to data */\n  uint64_t byte_offset;\n} DLTensor;\n\n/*!\n * \\brief C Tensor object, manage memory of DLTensor. This data structure is\n *  intended to facilitate the borrowing of DLTensor by another framework. It is\n *  not meant to transfer the tensor. When the borrowing framework doesn't need\n *  the tensor, it should call the deleter to notify the host that the resource\n *  is no longer needed.\n */\ntypedef struct DLManagedTensor {\n  /*! \\brief DLTensor which is being memory managed */\n  DLTensor dl_tensor;\n  /*! \\brief the context of the original host framework of DLManagedTensor in\n   *   which DLManagedTensor is used in the framework. It can also be NULL.\n   */\n  void * manager_ctx;\n  /*! \\brief Destructor signature void (*)(void*) - this should be called\n   *   to destruct manager_ctx which holds the DLManagedTensor. It can be NULL\n   *   if there is no way for the caller to provide a reasonable destructor.\n   *   The destructors deletes the argument self as well.\n   */\n  void (*deleter)(struct DLManagedTensor * self);\n} DLManagedTensor;\n#ifdef __cplusplus\n}  // DLPACK_EXTERN_C\n#endif\n#endif  // DLPACK_DLPACK_H_"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/application.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_APPLICATION_HPP_\n#define NVIDIA_GXF_APPLICATION_HPP_\n\n#include <filesystem>\n#include <map>\n#include <memory>\n#include <string>\n#include <unordered_set>\n#include <utility>\n#include <vector>\n\n#include \"common/assert.hpp\"\n#include \"gxf/app/arg.hpp\"\n#include \"gxf/app/config_parser.hpp\"\n#include \"gxf/app/driver.hpp\"\n#include \"gxf/app/extension_manager.hpp\"\n#include \"gxf/app/segment.hpp\"\n#include \"gxf/app/worker.hpp\"\n#include \"gxf/core/expected_macro.hpp\"\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\n\nnamespace gxf {\n\ntemplate <typename T, typename... Args>\nstd::shared_ptr<T> create_app(Args&&... args) {\n  auto app = std::make_shared<T>(std::forward<Args>(args)...);\n  return app;\n}\n\n/**\n * @brief Enum representing the application mode of execution.\n *  An application can either have segments or entities in its\n *  compose() api.\n */\nenum class ExecutionMode : int8_t {\n  kUnset = 0,\n  kSingleSegment,\n  kMultiSegment,\n  kDistributed,\n};\n\n/**\n * @brief Scaffolding layer to create applications imperatively. Users implement a virtual\n * compose() api where individual building blocks of an application is constructed, configured\n * and connected with each other.\n *\n */\nclass Application : public Segment {\n private:\n  friend class Worker;\n  friend class Driver;\n public:\n  Application();\n\n  ~Application();\n\n  Application(const Application&) = delete;\n\n  Application& operator=(const Application&) = delete;\n\n  Application(Application&&) = delete;\n\n  Application& operator=(Application&&) = delete;\n\n  virtual void compose() {}\n\n  /**\n   * @brief Set segment config file\n   *\n   * @param file_path Absolute path to the yaml config file for segments\n   * distributed execution\n   * @return Expected<void> Success or error code\n   */\n  Expected<void> setConfig(const std::string& file_path);\n\n  /**\n   * @brief Set segment config file\n   *\n   * @param argc CLI argument count\n   * @param argv CLI argument array, the second is config file path for segments\n   * distributed execution\n   * @return Expected<void> Success or error code\n   */\n  Expected<void> setConfig(int argc, char** argv);\n\n  /**\n   * @brief Creates a segment in plan with its own context. The graph for the segment\n   * will be composed() after creation\n   *\n   * @tparam SegmentT Type of segment\n   * @param name Name of the segment\n   * @return std::shared_ptr<SegmentT> Shared pointer of the segment\n   */\n  template <typename SegmentT,\n            typename = std::enable_if_t<!std::is_same_v<Segment, std::decay_t<SegmentT>>>>\n  std::shared_ptr<SegmentT> createSegment(const char* name) {\n    if (segments_plan_.find(name) != segments_plan_.end()) {\n      GXF_LOG_ERROR(\"ProxySegment with name [%s] already exists. \"\n        \"Segment names have to be unique\", name);\n      return nullptr;\n    }\n    std::shared_ptr<SegmentT> element = std::make_shared<SegmentT>();\n    element->setName(name);\n    segments_plan_.emplace(name, element);\n    return element;\n  }\n\n  /**\n   * @brief API to load extensions at runtime\n   *\n   * @param manifest path to manifest file with list of extensions\n   * @return Expected<void> Success or error code\n   */\n  Expected<void> loadExtensionManifest(const char* manifest);\n\n  /**\n   * @brief Adds a UCX connection between two entities with many : many tx and rx\n   *  Ucx Transmitter and Ucx Receiver components are added to the source and target entities in\n   *  both of the segments\n   * @param source Segment with the entity transmitting the message\n   * @param target Segment with the entity receiving entity. A message available term is added\n   *               along with the Ucx Receiver\n   * @param port_maps Segment port map with entity and queue name to be used for connection.\n   * @return Expected<void> Success if connection was added successfully, error code on failure\n   */\n  Expected<void> connect(SegmentPtr source, SegmentPtr target,\n                         std::vector<SegmentPortPair> port_maps);\n\n  /**\n   * @brief Sets the severity level of the logs (corresponding to GXF_LOG_* logging macros)\n   *  for a specific segment\n   *\n   *\n   * @param name Name of the segment\n   * @param severity a valid severity level as defined in `gxf_severity_t`. Logs corresponding to\n   *                any level <= severity will be logged.\n   * @return gxf_result_t On success the function returns GXF_SUCCESS.\n   */\n  gxf_result_t setSegmentSeverity(const char* name, gxf_severity_t severity);\n\n  /**\n   * @brief A blocking api to run the graph. If the application contains multiple segments,\n   * each segment is launched asynchronously and this thread is blocked until each one of\n   * the segments have finished execution. If the graph contains multiple entities,\n   * then this thread is blocked until the graph execution is complete.\n   * @return Expected<void> Success or error code\n   */\n  Expected<void> run();\n\n  /**\n   * @brief A non blocking api call to run an application. If the application contains multiple\n   * segments, each segment is launched asynchronously.\n   * @return Expected<void> Success or error code\n   */\n  Expected<void> runAsync();\n\n  /**\n   * @brief A non blocking api to stop all running running segments or entities.\n   *\n   * @return Expected<void> Success or error code\n   */\n  Expected<void> interrupt();\n\n  /**\n   * @brief A blocking API to waits until the graph execution has completed\n   *\n   * @return Expected<void> Success or error code\n   */\n  Expected<void> wait();\n\n  /**\n   * @brief In-place add a GraphWorker Component into Application's root context,\n   * in which case Application's context should only hold and run GraphWorker or GraphDriver\n  */\n  template <typename... Args>\n  Expected<void> setWorker(const std::string& name, Args... args) {\n    if (worker_ != nullptr) {\n      GXF_LOG_ERROR(\"Worker already created with name: %s\", worker_->name().c_str());\n      return Unexpected{GXF_FAILURE};\n    }\n    worker_ = std::make_shared<Worker>(this, name);\n\n    std::vector<Arg> arg_list;\n    if constexpr (sizeof...(args) > 0) { arg_list = parseArgsOfType<Arg>(args...); }\n    // 1. parse worker its own port\n    int32_t port = -1;\n    Expected<Arg> arg_port = findArg(arg_list, kPort, GXF_PARAMETER_TYPE_INT32);\n    if (!arg_port) {\n      GXF_LOG_ERROR(\"Failed to find arg: %s when set GraphWorker[%s]\", kPort, name.c_str());\n      return Unexpected{arg_port.error()};\n    }\n    port = arg_port.value().as<int32_t>();\n    // 2. parse driver_ip\n    std::string driver_ip;\n    Expected<Arg> arg_driver_ip = findArg(arg_list, kDriverIp, GXF_PARAMETER_TYPE_STRING);\n    if (!arg_driver_ip) {\n      GXF_LOG_ERROR(\"Arg 'driver_ip' is wrong when set GraphWorker[%s]\", name.c_str());\n      return Unexpected{arg_driver_ip.error()};\n    }\n    driver_ip = arg_driver_ip.value().as<std::string>();\n    // 3. parse driver_port\n    int32_t driver_port = -1;\n    Expected<Arg> arg_driver_port = findArg(arg_list, kDriverPort, GXF_PARAMETER_TYPE_INT32);\n    if (!arg_driver_port) {\n      GXF_LOG_ERROR(\"Arg 'driver_port' is wrong when set GraphWorker[%s]\", name.c_str());\n      return Unexpected{arg_driver_port.error()};\n    }\n    driver_port = arg_driver_port.value().as<int32_t>();\n\n    std::unordered_set<std::string> segment_names;\n    for (const auto& arg : arg_list) {\n      auto it = segments_.find(arg.key());\n      if (it != segments_.end()) {\n        GXF_LOG_DEBUG(\"Add Segment[name: %s] to GraphWorker[name: %s]\",\n          it->first.c_str(), name.c_str());\n        segment_names.emplace(it->first);\n      }\n    }\n    if (!segment_names.empty()) {\n      GXF_LOG_ERROR(\"No segments selected in setWorker() arg list\");\n      return Unexpected{GXF_ARGUMENT_INVALID};\n    }\n    RETURN_IF_ERROR(worker_->setPort(port));\n    RETURN_IF_ERROR(worker_->setDriverIp(driver_ip));\n    RETURN_IF_ERROR(worker_->setDriverPort(driver_port));\n    RETURN_IF_ERROR(worker_->setSegments(segment_names));\n    return Success;\n  }\n\n  /**\n   * @brief In-place add a GraphDriver Component into Application's root context,\n   * in which case Application's context should only hold and run GraphWorker or GraphDriver\n  */\n  template <typename... Args>\n  Expected<void> setDriver(const std::string& name, Args... args) {\n    if (driver_ != nullptr) {\n      GXF_LOG_ERROR(\"Driver already created with name: %s\", driver_->name().c_str());\n      return Unexpected{GXF_FAILURE};\n    }\n    driver_ = std::make_shared<Driver>(this, name);\n\n    std::vector<Arg> arg_list;\n    if constexpr (sizeof...(args) > 0) { arg_list = parseArgsOfType<Arg>(args...); }\n    // parse driver its own port\n    int32_t port = -1;\n    Expected<Arg> arg_port = findArg(arg_list, kPort, GXF_PARAMETER_TYPE_INT32);\n    if (!arg_port) {\n      GXF_LOG_ERROR(\"Arg 'port' is wrong when set GraphDriver[%s]\", name.c_str());\n      return Unexpected{arg_port.error()};\n    }\n    port = arg_port.value().as<int32_t>();\n\n    RETURN_IF_ERROR(driver_->setPort(port));\n    return Success;\n  }\n\n  using Segment::connect;\n\n private:\n  Expected<void> commitCompose();\n  Expected<void> commitSegment(SegmentPtr segment, const char* name);\n  Expected<void> commitConnect(SegmentPtr source, SegmentPtr target,\n                         std::vector<SegmentPortPair> port_maps);\n  Expected<void> checkConfiguration();\n  Expected<void> activate();\n  Expected<void> deactivate();\n  Expected<void> finalize();\n  Expected<void> setupCrashHandler();\n\n  std::map<std::string, std::shared_ptr<Segment>> segments_;\n  std::map<std::string, std::shared_ptr<Segment>> segments_plan_;\n  std::unordered_set<SegmentConnection, SegmentConnection::Hash> segment_connections_;\n  std::unordered_set<SegmentConnection, SegmentConnection::Hash> segment_connections_plan_;\n  std::unordered_set<std::string> enabled_segments_;\n  std::string name_;\n  ExecutionMode mode_{ExecutionMode::kUnset};\n  ExtensionManager extension_manager_;\n  uint64_t next_ucx_port_ = 13337U;  // will be set to DEFAULT_UCX_PORT by constructor\n  WorkerPtr worker_;\n  DriverPtr driver_;\n  std::unique_ptr<ConfigParser> config_parser_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_APPLICATION_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/arg.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_GRAPH_ARG_HPP_\n#define NVIDIA_GXF_GRAPH_ARG_HPP_\n\n#include <any>\n#include <optional>\n#include <string>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"yaml-cpp/yaml.h\"\n\n#include \"common/type_name.hpp\"\n#include \"common/type_utils.hpp\"\n#include \"gxf/core/parameter_parser.hpp\"\n#include \"gxf/core/parameter_parser_std.hpp\"\n#include \"gxf/core/parameter_registrar.hpp\"\n#include \"gxf/core/parameter_wrapper.hpp\"\n\nnamespace nvidia {\n\nnamespace gxf {\n\nclass ProxyComponent;\n\n/**\n * @brief Struct to hold type info of an Arg\n *\n */\ntypedef struct ArgInfo {\n  gxf_parameter_type_t type;\n  std::string type_name;\n  int32_t rank = 0;\n  std::array<int32_t, ParameterInfo<int32_t>::kMaxRank> shape = {0};\n} ArgInfo;\n\n/**\n * @brief ArgOverride for scalar parameter type\n *\n * @tparam T A valid argument type\n */\ntemplate <typename T>\nstruct ArgOverride {\n  static Expected<void> apply(ArgInfo& info) {\n    info.type = ParameterTypeTrait<T>::type;\n    info.type_name.assign(ParameterTypeTrait<T>::type_name);\n    info.rank = 0;\n    if (info.type == GXF_PARAMETER_TYPE_CUSTOM) { return Unexpected{GXF_ARGUMENT_INVALID}; }\n    return Success;\n  }\n\n  static Expected<YAML::Node> wrap(const T& value) { return YAML::Node(value); }\n};\n\n/**\n * @brief Specialized ArgOverride for args of type handle<T>\n *\n * @tparam T A valid argument type\n */\ntemplate <typename T>\nstruct ArgOverride<Handle<T>> {\n  static Expected<void> apply(ArgInfo& info) {\n    info.type = GXF_PARAMETER_TYPE_HANDLE;\n    info.type_name.assign(TypenameAsString<T>());\n    info.rank = 0;\n    return Success;\n  }\n\n  static Expected<YAML::Node> wrap(const Handle<T>& value) {\n    std::string c_name = value.name();\n    const char* entity_name;\n    gxf_uid_t eid = kNullUid;\n    auto result = GxfComponentEntity(value.context(), value.cid(), &eid);\n    if (result != GXF_SUCCESS) {\n      GXF_LOG_ERROR(\"Unable to find the entity for %s\", c_name.c_str());\n      return Unexpected{result};\n    }\n    result = GxfEntityGetName(value.context(), eid, &entity_name);\n    if (result != GXF_SUCCESS) {\n      GXF_LOG_ERROR(\"Unable to get the entity name\");\n      return Unexpected{result};\n    }\n\n    std::string full_name = std::string(entity_name) + \"/\" + c_name;\n    return YAML::Node(full_name);\n  }\n};\n\n/**\n * @brief Specialized ArgOverride for args of type std::vector<T>\n *\n * @tparam T A valid argument type\n */\ntemplate <typename T>\nstruct ArgOverride<std::vector<T>> {\n  static Expected<void> apply(ArgInfo& info) {\n    // Get the element info\n    ArgInfo element_info;\n    const auto result = ArgOverride<T>::apply(element_info);\n    if (!result) { return ForwardError(result); }\n\n    info.type = element_info.type;\n    info.type_name = element_info.type_name;\n\n    // Fetch the shape of <T> and update it to the current ArgInfo\n    for (int32_t i = 0; i < element_info.rank; ++i) { info.shape[i] = element_info.shape[i]; }\n\n    // A vector increases the rank by 1 and adds shape [-1] to <T>.\n    info.shape[element_info.rank] = -1;\n    info.rank = element_info.rank + 1;\n\n    return Success;\n  }\n\n  static Expected<YAML::Node> wrap(const std::vector<T>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n    for (const auto& val : value) {\n      auto maybe = ArgOverride<T>::wrap(val);\n      if (!maybe) { return Unexpected{maybe.error()}; }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n/**\n * @brief Specialized ArgOverride for parameters of type std::array<T,N>\n *\n * @tparam T A valid argument type\n * @tparam N length of the array\n */\ntemplate <typename T, std::size_t N>\nstruct ArgOverride<std::array<T, N>> {\n  static Expected<void> apply(ArgInfo& info) {\n    // Get the element info\n    ArgInfo element_info;\n    const auto result = ArgOverride<T>::apply(element_info);\n    if (!result) { return ForwardError(result); }\n\n    info.type = element_info.type;\n    info.type_name = element_info.type_name;\n\n    // Fetch the shape of <T> and update it to the current ArgInfo\n    for (int32_t i = 0; i < element_info.rank; ++i) { info.shape[i] = element_info.shape[i]; }\n\n    // An array increases the rank by 1 and adds shape [N] to <T>.\n    info.shape[element_info.rank] = N;\n    info.rank = element_info.rank + 1;\n\n    return Success;\n  }\n\n  static Expected<YAML::Node> wrap(const std::array<T, N>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n    for (const auto& val : value) {\n      auto maybe = ArgOverride<T>::wrap(val);\n      if (!maybe) { return Unexpected{maybe.error()}; }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n/**\n * @brief Specialized ArgOverride for parameters of type ProxyComponent\n *\n */\ntemplate <>\nstruct ArgOverride<ProxyComponent> {\n  static Expected<void> apply(ArgInfo& info) {\n    info.type = GXF_PARAMETER_TYPE_HANDLE;\n    info.rank = 0;\n    return Success;\n  }\n\n  static Expected<YAML::Node> wrap(const ProxyComponent& value) { return YAML::Node(); }\n};\n\n/**\n * @brief Argument interface to enable configuring parameters in GXF Components from\n * the application layer. All parameter types from gxf_parameter_type_t enum is supported.\n *\n */\nclass Arg {\n public:\n  ~Arg() = default;\n\n  // Constructors\n  explicit Arg(const std::string& key) : key_(key) {}\n\n  template <typename T>\n  Arg(const std::string& key, const T& value) {\n    key_ = key;\n    auto result = ArgOverride<T>::apply(info_);\n    if (!result) {\n      GXF_LOG_ERROR(\"Invalid Arg [%s] of type [%s] with error [%s]\", key.c_str(),\n                    TypenameAsString<T>(), GxfResultStr(result.error()));\n      return;\n    }\n    set_value<T>(value);\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key.c_str(), info_.type_name.c_str());\n  }\n\n  template <typename T>\n  Arg(const std::string& key, const Handle<T>& value) {\n    key_ = key;\n    auto result = ArgOverride<Handle<T>>::apply(info_);\n    if (!result) {\n      GXF_LOG_ERROR(\"Invalid Arg type %s \", key.c_str());\n      return;\n    }\n    set_value<Handle<T>>(value);\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key.c_str(), info_.type_name.c_str());\n    if (!value.is_null()) {\n      handle_cid_ = value->cid();\n      // handle_tid_ = value->tid();\n      GxfComponentTypeId(value.context(), info_.type_name.c_str(), &handle_tid_);\n    }\n  }\n\n  // Special case for string literals typed arguments\n  Arg(const std::string& key, const char* value) {\n    key_ = key;\n    auto result = ArgOverride<std::string>::apply(info_);\n    if (!result) {\n      GXF_LOG_ERROR(\"Invalid Arg [%s] of type [%s] with error [%s]\", key.c_str(),\n                    TypenameAsString<std::string>(), GxfResultStr(result.error()));\n      return;\n    }\n    set_value<std::string>(value);\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key.c_str(), info_.type_name.c_str());\n  }\n\n  Arg(const std::string& key, const ProxyComponent& value) {\n    key_ = key;\n    info_.type = GXF_PARAMETER_TYPE_HANDLE;\n    // info_.type_name = value.type_name();\n    handle_cid_ = kUnspecifiedUid;\n    info_.rank = 0;\n    set_value<ProxyComponent>(value);\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key.c_str(), info_.type_name.c_str());\n  }\n\n  template <typename T, typename = std::enable_if_t<!std::is_lvalue_reference<T>::value>>\n  Arg(const std::string& key, T&& value) {\n    using DeducedT = std::remove_reference_t<std::remove_cv_t<T>>;\n    key_ = key;\n    auto result = ArgOverride<DeducedT>::apply(info_);\n    if (!result) {\n      GXF_LOG_ERROR(\"Invalid Arg type %s \", key.c_str());\n      return;\n    }\n    set_value<T>(std::forward<T>(value));\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key.c_str(), info_.type_name.c_str());\n  }\n\n  template <typename T, typename = std::enable_if_t<!std::is_lvalue_reference<T>::value>>\n  Arg(const std::string& key, Handle<T>&& value) {\n    using DeducedT = std::remove_reference_t<std::remove_cv_t<Handle<T>>>;\n    key_ = key;\n    auto result = ArgOverride<DeducedT>::apply(info_);\n    if (!result) {\n      GXF_LOG_ERROR(\"Invalid Arg type %s \", key.c_str());\n      return;\n    }\n    if (!value.is_null()) {\n      handle_cid_ = value->cid();\n      // handle_tid_ = value->tid();\n      GxfComponentTypeId(value.context(), info_.type_name.c_str(), &handle_tid_);\n    }\n    set_value<Handle<T>>(std::forward<Handle<T>>(value));\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key.c_str(), info_.type_name.c_str());\n  }\n\n  Arg(const std::string& key, ProxyComponent&& value) {\n    key_ = key;\n    info_.type = GXF_PARAMETER_TYPE_HANDLE;\n    // info_.type_name = value.type_name();\n    handle_cid_ = kUnspecifiedUid;\n    info_.rank = 0;\n    set_value<ProxyComponent>(std::forward<ProxyComponent>(value));\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key.c_str(), info_.type_name.c_str());\n  }\n\n  template <typename T, typename = std::enable_if_t<!std::is_same_v<Arg, std::decay_t<T>>>>\n  Arg& operator=(const T& value) {\n    auto result = ArgOverride<T>::apply(info_);\n    if (!result) {\n      GXF_LOG_ERROR(\"Invalid Arg type %s \", key_.c_str());\n      return *this;\n    }\n    set_value<T>(value);\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key_.c_str(), info_.type_name.c_str());\n    return *this;\n  }\n\n  template <typename T, typename = std::enable_if_t<!std::is_same_v<Arg, std::decay_t<T>>>>\n  Arg& operator=(const Handle<T>& value) {\n    auto result = ArgOverride<Handle<T>>::apply(info_);\n    if (!result) {\n      GXF_LOG_ERROR(\"Invalid Arg type %s \", key_.c_str());\n      return *this;\n    }\n    if (!value.is_null()) {\n      handle_cid_ = value->cid();\n      // handle_tid_ = value->tid();\n      GxfComponentTypeId(value.context(), info_.type_name.c_str(), &handle_tid_);\n    }\n    set_value<Handle<T>>(value);\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key_.c_str(), info_.type_name.c_str());\n    return *this;\n  }\n\n  Arg& operator=(ProxyComponent& value) {\n    info_.type = GXF_PARAMETER_TYPE_HANDLE;\n    // info_.type_name = value.type_name();\n    handle_cid_ = kUnspecifiedUid;\n    info_.rank = 0;\n    set_value<ProxyComponent>(value);\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key_.c_str(), info_.type_name.c_str());\n    return *this;\n  }\n\n  template <typename T, typename = std::enable_if_t<!std::is_same_v<Arg, std::decay_t<T>> &&\n                                                    !std::is_lvalue_reference<T>::value>>\n  Arg&& operator=(T&& value) {\n    using DeducedT = std::remove_reference_t<std::remove_cv_t<T>>;\n    auto result = ArgOverride<DeducedT>::apply(info_);\n    if (!result) {\n      GXF_LOG_ERROR(\"Invalid Arg type %s \", key_.c_str());\n      return std::move(*this);\n    }\n    set_value<T>(std::forward<T>(value));\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key_.c_str(), info_.type_name.c_str());\n    return std::move(*this);\n  }\n\n  template <typename T, typename = std::enable_if_t<!std::is_same_v<Arg, std::decay_t<Handle<T>>> &&\n                                                    !std::is_lvalue_reference<Handle<T>>::value>>\n  Arg&& operator=(Handle<T>&& value) {\n    using DeducedT = std::remove_reference_t<std::remove_cv_t<Handle<T>>>;\n    auto result = ArgOverride<DeducedT>::apply(info_);\n    if (!result) {\n      GXF_LOG_ERROR(\"Invalid Arg type %s \", key_.c_str());\n      return std::move(*this);\n    }\n    if (!value.is_null()) {\n      handle_cid_ = value->cid();\n      // handle_tid_ = value->tid();\n      GxfComponentTypeId(value.context(), info_.type_name.c_str(), &handle_tid_);\n    }\n    set_value<Handle<T>>(std::forward<Handle<T>>(value));\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key_.c_str(), info_.type_name.c_str());\n    return std::move(*this);\n  }\n\n  Arg&& operator=(ProxyComponent&& value) {\n    info_.type = GXF_PARAMETER_TYPE_HANDLE;\n    // info_.type_name = value.type_name();\n    handle_cid_ = kUnspecifiedUid;\n    info_.rank = 0;\n    set_value<ProxyComponent>(value);\n    GXF_LOG_VERBOSE(\"Arg: [%s], Type : [%s] created\", key_.c_str(), info_.type_name.c_str());\n    return std::move(*this);\n  }\n\n  template <typename T, typename = std::enable_if_t<IsDefaultConstructible_v<T>>>\n  T as() const {\n    const T* value = std::any_cast<T>(&value_);\n    if (value == nullptr) {\n      GXF_LOG_ERROR(\"Arg [%s] cannot be read as [%s]\", key_.c_str(), TypenameAsString<T>());\n      return T();\n    }\n    return *value;\n  }\n\n  const gxf_uid_t handle_uid() const { return handle_cid_; }\n\n  const gxf_tid_t handle_tid() const { return handle_tid_; }\n\n  const char* key() const { return key_.c_str(); }\n\n  const std::string arg_type_name() const { return info_.type_name; }\n\n  const ArgInfo arg_info() const { return info_; }\n\n  const YAML::Node yaml_node() const { return yaml_node_; }\n\n  const int32_t rank() const { return info_.rank; }\n\n  const std::array<int32_t, ParameterInfo<int32_t>::kMaxRank> shape() const { return info_.shape; }\n\n  const gxf_parameter_type_t parameter_type() const { return info_.type; }\n\n  bool has_value() const { return value_.has_value(); }\n\n  std::any value() const { return value_; }\n\n private:\n  std::any value_;\n  std::string key_;\n  ArgInfo info_;\n  gxf_uid_t handle_cid_ = kNullUid;\n  gxf_tid_t handle_tid_ = GxfTidNull();\n  YAML::Node yaml_node_{};\n\n  template <typename T>\n  void set_value(const T& value) {\n    value_ = value;\n    auto maybe_yaml = ArgOverride<T>::wrap(value);\n    if (!maybe_yaml) {\n      GXF_LOG_ERROR(\"Arg [%s] failed to parse as a YAML node with error [%s]\", key_.c_str(),\n                    GxfResultStr(maybe_yaml.error()));\n    } else {\n      yaml_node_ = maybe_yaml.value();\n    }\n  }\n\n  template <typename T>\n  void set_value(T&& value) {\n    auto maybe_yaml = ArgOverride<T>::wrap(value);\n    if (!maybe_yaml) {\n      GXF_LOG_ERROR(\"Arg [%s] failed to parse as a YAML node with error [%s]\", key_.c_str(),\n                    GxfResultStr(maybe_yaml.error()));\n    } else {\n      yaml_node_ = maybe_yaml.value();\n    }\n    value_ = std::move(value);\n  }\n};\n\n}  // namespace gxf\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GRAPH_ARG_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/arg_parse.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_GRAPH_ARG_PARSE_HPP_\n#define NVIDIA_GXF_GRAPH_ARG_PARSE_HPP_\n\n#include <string>\n#include <vector>\n\n#include \"gxf/app/arg.hpp\"\n#include \"gxf/app/proxy_component.hpp\"\n#include \"gxf/core/component.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief Function returns a list of elements filtered from an input parameter pack of\n * arguments\n *\n * @tparam T Type of parameter to be filtered from the parameter pack\n * @return std::vector<T>\n */\ntemplate <typename T, typename First, typename... Rest>\nstd::vector<T> parseArgsOfType(const First& first, const Rest&... rest) {\n  std::vector<T> arg_list;\n  if constexpr (std::is_same_v<T, First>) { arg_list.push_back(first); }\n  if constexpr (sizeof...(rest) > 0) {\n    auto rest_list = parseArgsOfType<T>(rest...);\n    arg_list.insert(arg_list.end(), rest_list.begin(), rest_list.end());\n  }\n  return arg_list;\n}\n\n\n// Returns a list of proxyComponent args from a list of args\n// The proxy component args are removed from the input arg list\nstd::vector<Arg> filterProxyComponents(std::vector<Arg>& args);\n\n// Sets a parameter value for a component using the info stored in Arg type\nExpected<void> applyArg(Handle<Component> component, const Arg& arg);\n\n// Returns an arg from a list of args\nExpected<Arg> findArg(const std::vector<Arg>& args, const std::string& key,\n  const gxf_parameter_type_t type);\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GRAPH_ARG_PARSE_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/config_parser.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_GRAPH_CONFIG_PARSER_HPP_\n#define NVIDIA_GXF_GRAPH_CONFIG_PARSER_HPP_\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"gxf/core/expected.hpp\"\n#include \"yaml-cpp/yaml.h\"\n\nnamespace YAML { class Node; }\n\nnamespace nvidia {\nnamespace gxf {\n\n// identifiers\nstatic constexpr const char* kSegmentConfig = \"segment_config\";\nstatic constexpr const char* kMemberType = \"member\";\nstatic constexpr const char* kMemberParam = \"parameters\";\nstatic constexpr const char* kEnabledSegments = \"enabled_segments\";\nstatic constexpr const char* kWorker = \"worker\";\nstatic constexpr const char* kDriver = \"driver\";\n// attributes\nstatic constexpr const char* kName = \"name\";\nstatic constexpr const char* kEnabled = \"enabled\";\nstatic constexpr const char* kPort = \"port\";\nstatic constexpr const char* kDriverIp = \"driver_ip\";\nstatic constexpr const char* kDriverPort = \"driver_port\";\nstatic constexpr const char* kRemoteAccess = \"remote_access\";\nstatic constexpr const char* kServerIpAddress = \"server_ip_address\";\n\n/**\n * @brief Read and parse YAML config files\n *\n */\nclass ConfigParser {\n public:\n  struct SegmentConfig {\n    struct EnabledSegments {\n      std::vector<std::string> names;\n    };\n    struct Worker {\n      bool enabled = true;\n      std::string name;\n      int32_t port = 0;\n      std::string driver_ip;\n      int32_t driver_port = 0;\n    };\n    struct Driver {\n      bool enabled = false;\n      std::string name;\n      int32_t port = 0;\n    };\n    EnabledSegments enabled_segments;\n    Worker worker;\n    Driver driver;\n    bool enable_all_segments = true;\n  };\n  ConfigParser() {\n    segment_control_ = std::make_shared<SegmentConfig>();\n  }\n  /**\n   * Main API\n  */\n  Expected<void> setFilePath(const std::string& file_path);\n  Expected<void> setFilePath(int argc, char** argv);\n  Expected<std::shared_ptr<SegmentConfig>> getSegmentConfig();\n\n private:\n  std::string file_path_;\n  std::shared_ptr<SegmentConfig> segment_control_;\n\n private:\n  std::string getExecutablePath();\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GRAPH_CONFIG_PARSER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/driver.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_APPLICATION_DRIVER_HPP_\n#define NVIDIA_GXF_APPLICATION_DRIVER_HPP_\n\n#include <memory>\n#include <string>\n\n#include \"gxf/app/segment.hpp\"\n#include \"gxf/std/graph_driver.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nclass Application;\n\n/**\n * @brief GraphDriver representation in Application API layer\n * It drives the execution of all remote GraphWorkers\n *\n */\nclass Driver {\n public:\n  Driver(Application* owner, const std::string& name);\n\n  std::string name() { return name_; }\n  Expected<void> setPort(uint32_t port) { port_ = port; return Success; }\n  // commit the setup into GraphWorker\n  Expected<void> commit();\n private:\n  Application* owner_;\n  std::string name_;\n  GraphEntityPtr driver_entity_;\n  // core component\n  Handle<GraphDriver> graph_driver_;\n  // server interface object\n  Handle<IPCServer> server_;\n  // client interface object\n  Handle<IPCClient> client_;\n\n  // driver server's own port\n  uint32_t port_;\n};\ntypedef std::shared_ptr<Driver> DriverPtr;\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_APPLICATION_DRIVER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/entity_group.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_ENTITY_GROUP_HPP_\n#define NVIDIA_GXF_ENTITY_GROUP_HPP_\n\n#include <map>\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"gxf/app/graph_entity.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief  A wrapper over EntityGroup C APIs to manage a programmable\n * entity group in C++ API layer\n *\n */\nclass EntityGroup {\n public:\n  EntityGroup() = default;\n\n  ~EntityGroup() = default;\n\n  EntityGroup(const EntityGroup&) = delete;\n\n  EntityGroup& operator=(const EntityGroup&) = delete;\n\n  /**\n   * @brief Creates a programmable entity group with the runtime context and sets its name\n   *\n   * @param context A valid GXF context\n   * @param name Name of the graph entity\n   */\n  Expected<void> setup(gxf_context_t context, const char* name);\n\n  /**\n   * @brief Add single entity into this entity group.\n   * All Resouce components within the group will be automatically resolved to owner components\n   * that has corresponding type of Resource members registered.\n   *\n   * @param GraphEntityPtr An entity in C++ API representation\n   */\n  Expected<void> add(GraphEntityPtr entity);\n\n  /**\n   * @brief Add a list of entities into this entity group.\n   * All Resouce components within the group will be automatically resolved to owner components\n   * that has corresponding type of Resource members registered.\n   *\n   * @param entity_members A list of entities in C++ API representation\n   */\n  Expected<void> add(std::vector<GraphEntityPtr> entity_members);\n\n  /**\n   * @return This entity group ID.\n   */\n  gxf_uid_t gid() const { return gid_; }\n\n  /**\n   * @return This entity group name.\n   */\n  std::string name() const { return name_; }\n\n private:\n  gxf_uid_t gid_{kNullUid};\n  std::string name_;\n  std::map<std::string, GraphEntityPtr> entity_members_;\n};\n\ntypedef std::shared_ptr<EntityGroup> EntityGroupPtr;\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_ENTITY_GROUP_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/extension_manager.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_GRAPH_EXTENSION_MANAGER_HPP_\n#define NVIDIA_GXF_GRAPH_EXTENSION_MANAGER_HPP_\n\n#include <map>\n#include <set>\n#include <shared_mutex>  // NOLINT\n#include <vector>\n\n#include \"common/fixed_vector.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/extension.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief Loads extensions and allows to create instances of their components.\n *\n */\nclass ExtensionManager {\n public:\n  ExtensionManager() = default;\n  ~ExtensionManager() = default;\n\n  ExtensionManager(const ExtensionManager&) = delete;\n  ExtensionManager(ExtensionManager&&) = delete;\n  ExtensionManager& operator=(const ExtensionManager&) = delete;\n  ExtensionManager& operator=(ExtensionManager&&) = delete;\n\n  // Loads a GXF extension from the given file\n  Expected<void> registerExtensions(gxf_context_t context);\n  Expected<void> load(const char* filename);\n  Expected<void> load(Extension* extension, void* handle = nullptr);\n  Expected<void> loadManifest(const char* filename);\n  Expected<void> unloadAll();\n\n  // Gets list of TIDs for loaded Extensions.\n  // [in/out] extension_num is for capacity of parameter extensions\n  // [out] extensions is memory to write TIDs to\n  Expected<void> getExtensions(uint64_t* extension_count, gxf_tid_t* extensions);\n\n  // Gets description for specified (loaded) extension and list of components It provides\n  Expected<void> getExtensionInfo(gxf_tid_t eid, gxf_extension_info_t* info);\n\n private:\n  std::set<void*> handles_;\n  std::map<gxf_tid_t, Extension*> component_factory_;\n  std::map<gxf_tid_t, Extension*> extension_factory_;\n  std::vector<Extension*> factories_;\n\n  mutable std::shared_timed_mutex mutex_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GRAPH_EXTENSION_MANAGER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/graph_entity.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_GRAPH_ENTITY_HPP_\n#define NVIDIA_GXF_GRAPH_ENTITY_HPP_\n\n#include <cassert>\n#include <map>\n#include <memory>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"gxf/app/arg.hpp\"\n#include \"gxf/app/arg_parse.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/clock.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/resources.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief  A wrapper over nvidia::gxf::Entity to manage a programmable\n * graph entity.\n *\n */\nclass GraphEntity {\n public:\n  GraphEntity() = default;\n\n  ~GraphEntity() = default;\n\n  GraphEntity(const GraphEntity&) = delete;\n\n  GraphEntity& operator=(const GraphEntity&) = delete;\n\n  /**\n   * @brief Creates a programmable entity with the runtime context and sets its name\n   *\n   * @param context A valid GXF context\n   * @param name Name of the graph entity\n   */\n  Expected<void> setup(gxf_context_t context, const char* name) {\n    const GxfEntityCreateInfo entity_create_info = {name, GXF_ENTITY_CREATE_PROGRAM_BIT};\n    auto result = GxfCreateEntity(context, &entity_create_info, &eid_);\n    if (result != GXF_SUCCESS) {\n      GXF_LOG_ERROR(\"Failed to create entity [%s] with error %s\", name, GxfResultStr(result));\n      return Unexpected{result};\n    }\n    GxfEntityRefCountInc(context, eid_);\n    Entity::Own(context, eid_).map([&](Entity entity) { entity_ = std::move(entity); });\n    return Success;\n  }\n\n  /**\n   * @brief Creates a generic component of type T and sets the parameter values from Args\n   * pack of args.\n   *\n   * Transmitters, Receivers, Clocks and Scheduling Term component names have to be unique.\n   *\n   * @tparam T Type of component. Must be derived from nvidia::gxf::Component\n   * @param name Name of the component\n   * @param args Args must be of type Arg\n   * @return Handle<T> Handle to newly created component. Null handle if component was not created.\n   */\n  template <typename T, typename... Args>\n  Handle<T> add(const char* name = nullptr, Args && ... args) {\n    std::vector<Arg> arg_list;\n    if constexpr (sizeof...(args) > 0) {\n      arg_list = parseArgsOfType<Arg>(std::forward<Args>(args)...);\n    }\n    return add<T>(name, std::move(arg_list));\n  }\n\n  /**\n   * @brief Creates a generic component of type T and sets the parameter values from arg_list.\n   *\n   * Transmitters, Receivers, Clocks and Scheduling Term component names have to be unique.\n   *\n   * @tparam T Type of component. Must be derived from nvidia::gxf::Component\n   * @param name Name of the component\n   * @param arg_list vector of Arg used for initializing the component's parameters.\n   * @return Handle<T> Handle to newly created component. Null handle if component was not created.\n   */\n  template <typename T>\n  Handle<T> add(const char* name, std::vector<Arg> arg_list) {\n    auto maybe_component = addComponent<T>(name, arg_list);\n    if (!maybe_component) { return Handle<T>::Null(); }\n    return maybe_component.value();\n  }\n\n  /**\n   * @brief Finds all components of given type. Returns an empty vector if component is not found\n   *\n   * @tparam T Type of component to search for\n   * @return FixedVector<Handle<T>, N> List of handles to components of the same type\n   */\n  template <typename T, size_t N = kMaxComponents>\n  FixedVector<Handle<T>, N> findAll() const {\n    auto maybe_result = entity_.findAll<T, N>();\n    return maybe_result ? maybe_result.value() : FixedVector<Handle<T>, N>();\n  }\n\n  /**\n   * @brief Gets a component by type and name. Returns null handle if no such component.\n   *\n   * @tparam T Type of component to search for\n   * @param name Name of the component to look for\n   * @return Handle<T> Handle to component, if component is found. Null handle if\n   * no such component.\n   */\n  template <typename T>\n  Handle<T> get(const char* name = nullptr) const {\n    auto maybe_handle = entity_.get<T>(name);\n    if (!maybe_handle) {\n      GXF_LOG_WARNING(\"Failed to find component [%s] with name [%s] in entity [%s] with error %s\",\n                      TypenameAsString<T>(), name, entity_.name(),\n                      GxfResultStr(maybe_handle.error()));\n      return Handle<T>::Null();\n    }\n    return maybe_handle.value();\n  }\n\n  /**\n   * @brief Get a component by type and name. Returns an Unexpected in the case that the\n   * component is not found. Unlike `get` no error is logged if a component is not found.\n   *\n   * @tparam T Type of component to search for\n   * @param name Name of the component to look for\n   * @return Expected<Handle<Component>> Handle to the component, if component is found.\n   * Otherwise, an Unexpected is returned.\n   */\n  template <typename T>\n  Expected<Handle<T>> try_get(const char* name = nullptr) const {\n    auto maybe_handle = entity_.get<T>(name);\n    if (!maybe_handle) { return Unexpected{GXF_ENTITY_COMPONENT_NOT_FOUND}; }\n    return maybe_handle.value();\n  }\n\n  /**\n   * @brief Gets a component by type and name. Returns null handle if no such component.\n   *\n   * @param type_name Fully qualified C++ type name of the component to search for\n   * @param name Name of the component to look for\n   * @return Handle<Component> Handle to component, if component is found. Null handle if\n   * no such component.\n   */\n  Handle<Component> get(const char* type_name, const char* name = nullptr) const;\n\n  /**\n   * @brief Get a component by type and name. Returns an Unexpected in the case that the\n   * component is not found. Unlike `get` no error is logged if a component is not found.\n   *\n   * @param type_name Fully qualified C++ type name of the component to search for\n   * @param name Name of the component to look for\n   * @return Expected<Handle<Component>> Handle to the component, if component is found.\n   * Otherwise, an Unexpected is returned.\n   */\n  Expected<Handle<Component>> try_get(const char* type_name, const char* name = nullptr) const;\n\n  /**\n   * @brief Adds a codelet of type T with given name and sets the parameter values from Args\n   *\n   * @tparam T Type of codelet. Must be derived from nvidia::gxf::Codelet\n   * @param name name of the codelet\n   * @param args Args must be of type Arg\n   * @return Handle<T> Handle to newly created component\n   */\n  template <typename T, typename... Args>\n  Handle<T> addCodelet(const char* name = nullptr, Args... args) {\n    static_assert(std::is_base_of<Codelet, T>::value, \"Requested type is not a codelet\");\n    if (!codelet_.is_null()) {\n      GXF_LOG_ERROR(\"Graph Entity is already configured with a codelet [%s]\", codelet_->name());\n      return Handle<T>::Null();\n    }\n\n    auto codelet = add<T>(name, args...);\n    if (!codelet) { return codelet; }\n\n    codelet_ = codelet;\n    return codelet;\n  }\n\n  /**\n   * @brief Adds a codelet with a given C++ type name.\n   *\n   * @param type_name The fully qualified C++ type name of the codelet component\n   * @param name Name of the codelet\n   * @param arg_list Arguments for the codelet\n   * @return Handle<Codelet> Handle to newly created codelet component\n   */\n  Handle<Codelet> addCodelet(const char* type_name, const char* name = nullptr,\n                             const std::vector<Arg>& arg_list = {});\n\n  /**\n   * @brief Adds a component with a given C++ type name.\n   *\n   * @param type_name The fully qualified C++ type name of the component\n   * @param name Name of the component\n   * @param arg_list Arguments for the component\n   * @return Handle<Component> Handle to newly created component\n   */\n  Handle<Component> addComponent(const char* type_name, const char* name = nullptr,\n                                 const std::vector<Arg>& arg_list = {});\n\n  /**\n   * @brief Adds a component of Clock type T and sets the parameter values from Args\n   *\n   * @tparam T Type of clock. Must be derived from nvidia::gxf::Clock.\n   * @param name Name of the clock\n   * @param args Args must be of type Arg\n   * @return Handle<T> Handle to newly created clock component\n   */\n  template <typename T, typename... Args>\n  Handle<T> addClock(const char* name = nullptr, Args... args) {\n    static_assert(std::is_base_of<Clock, T>::value, \"Requested type is not a clock\");\n    auto clock = add<T>(name, args...);\n    return clock;\n  }\n\n  /**\n   * @brief Adds a clock component with a given C++ type name.\n   *\n   * @param type_name The fully qualified C++ type name of the clock\n   * @param name Name of the clock\n   * @param arg_list Arguments for the clock component\n   * @return Handle<Clock> Handle to newly created clock\n   */\n  Handle<Clock> addClock(const char* type_name, const char* name = nullptr,\n                         const std::vector<Arg>& arg_list = {});\n\n  /**\n   * @brief Get the Clock object from a graph entity. Returns null handle if no clock component\n   * has been created yet. Returns the first clock if no component name is provided. If name is\n   * provided, exact instance of the clock is returned if found else a Null handle.\n   *\n   * @param name Name of the clock component to lookup\n   * @return Handle<Clock>\n   */\n  Handle<Clock> getClock(const char* name = nullptr);\n\n  /**\n   * @brief Adds a component of SchedulingTerm type T and sets the parameter values from Args\n   *\n   * @tparam T Type of SchedulingTerm. Must be derived from nvidia::gxf::SchedulingTerm\n   * @param name name of the scheduling term\n   * @param args Args must be of type Arg\n   * @return Handle<T> Handle to newly created scheduling term component\n   */\n  template <typename T, typename... Args>\n  Handle<T> addSchedulingTerm(const char* name = nullptr, Args... args) {\n    static_assert(std::is_base_of<SchedulingTerm, T>::value,\n                  \"Requested type is not a SchedulingTerm\");\n    auto term = add<T>(name, args...);\n    return term;\n  }\n\n  /**\n   * @brief Adds a scheduling term component with a given C++ type name.\n   *\n   * @param type_name The fully qualified C++ type name of the scheduling term\n   * @param name Name of the scheduling term\n   * @return Handle<SchedulingTerm> Handle to newly created scheduling term\n   */\n  Handle<SchedulingTerm> addSchedulingTerm(const char* type_name, const char* name = nullptr,\n                                           const std::vector<Arg>& arg_list = {});\n\n  /**\n   * @brief Adds a component of Transmitter type T with name and sets the parameter values from\n   * Args\n   *\n   * Name of the transmitter should match the parameter name of the underlying codelet\n   * The name of the transmitter component is updated based on the parameter rank info.\n   * A Downstream receptive scheduling term is also added to monitor the transmitter component\n   *\n   * If codelet parameter is a scalar, name of the transmitter is also same as the parameter key\n   * Parameter<Handle<Transmitter>> | name - \"key\"\n   *\n   * If codelet parameter is a vector/array, the name of the transmitter component is key_%d where\n   * 'd' is the index of this transmitter in the codelet parameter.\n   * Parameter<Vector<Handle<Transmitter>> | name - \"key_0\", \"key_1\", \"key_2\"\n   *\n   * @tparam T Type of Transmitter. Must be derived from nvidia::gxf::Transmitter\n   * @param name Name of the transmitter component\n   * @param omit_term Boolean flag controlling whether or not a default downstream receptive\n   *                  scheduling term is added. If true, no scheduling term is added.\n   * @param args Args must be of type Arg\n   * @return Handle<T> Handle to newly created transmitter component\n   */\n  template <typename T, typename... Args>\n  Handle<T> addTransmitter(const char* name, bool omit_term = false, Args... args) {\n    static_assert(std::is_base_of<Transmitter, T>::value, \"Requested type is not a Transmitter\");\n    auto maybe_tx_name = formatTxName(name);\n    if (!maybe_tx_name) { return Handle<T>::Null(); }\n    auto tx_name = maybe_tx_name.value();\n\n    auto tx = add<T>(tx_name.c_str(), args...);\n    if (!tx) { return tx; }\n\n    if (!omit_term) {\n      // auto adds a downstream receptive scheduling term\n      auto term = this->add<DownstreamReceptiveSchedulingTerm>(tx_name.c_str());\n      term->setTransmitter(tx);\n    }\n\n    std::string full_name = std::string(this->name()) + \"/\" + tx_name;\n    auto result = updatePort(name, full_name);\n    if (!result) {\n      GXF_LOG_ERROR(\"Failed to add Transmitter [%s] with error [%s]\", tx_name.c_str(),\n                    GxfResultStr(result.error()));\n    }\n\n    return tx;\n  }\n\n  /**\n   * @brief Adds a component of Transmitter of the corresponding type_name.\n   * Name of the transmitter should match the parameter name of the underlying codelet\n   * The name of the transmitter component is updated based on the parameter rank info.\n   *\n   * If codelet parameter is a scalar, name of the transmitter is also same as the parameter key\n   * Parameter<Handle<Transmitter>> | name - \"key\"\n   *\n   * If codelet parameter is a vector/array, the name of the transmitter component is key_%d where\n   * 'd' is the index of this transmitter in the codelet parameter.\n   * Parameter<Vector<Handle<Transmitter>> | name - \"key_0\", \"key_1\", \"key_2\"\n   *\n   * @param type_name The fully qualified C++ type name of the transmitter component\n   * @param name Name of the transmitter component\n   * @param arg_list Arguments for the transmitter component\n   * @param omit_term Boolean flag controlling whether or not a default downstream receptive\n   *                  scheduling term is added. If true, no scheduling term is added.\n   * @return Handle<Transmitter> Handle to newly created transmitter component\n   */\n  Handle<Transmitter> addTransmitter(const char* type_name, const char* name = nullptr,\n                                     const std::vector<Arg>& arg_list = {}, bool omit_term = false);\n\n  /**\n   * @brief Transmitter component lookup using name\n   *\n   * @param name name of a transmitter component which has been previously created\n   * @return Handle<Transmitter> Handle to transmitter component if found, Null handle if\n   * no such component.\n   */\n  Handle<Transmitter> getTransmitter(const char* name) {\n    for (auto it = tx_queues_.begin(); it != tx_queues_.end(); ++it) {\n      if (!strcmp(it->second->name(), name)) { return it->second; }\n    }\n    return Handle<Transmitter>::Null();\n  }\n\n  /**\n   * @brief Adds a component of Receiver type T with name and sets the parameter values from Args\n   *\n   * Name of the receiver should match the parameter name of the underlying codelet\n   * The name of the receiver component is updated based on the parameter rank info.\n   * A Message available scheduling term is also added to monitor the receiver component\n   *\n   * If codelet parameter is a scalar, name of the receiver is also same as the parameter key\n   * Parameter<Handle<Receiver>> | name - \"key\"\n   *\n   * If codelet parameter is a vector/array, the name of the receiver component is key_%d where\n   * 'd' is the index of this receiver in the codelet parameter.\n   * Parameter<Vector<Handle<Receiver>> | name - \"key_0\", \"key_1\", \"key_2\"\n   *\n   * @tparam T Type of Receiver. Must be derived from nvidia::gxf::Receiver\n   * @param name  Name of the receiver component\n   * @param omit_term Boolean flag controlling whether or not a default message available\n   *                  scheduling term is added. If true, no scheduling term is added.\n   * @param args Args must be of type Arg\n   * @return Handle<T> Handle to newly created receiver component\n   */\n  template <typename T, typename... Args>\n  Handle<T> addReceiver(const char* name, bool omit_term = false, Args... args) {\n    static_assert(std::is_base_of<Receiver, T>::value, \"Requested type is not a Receiver\");\n    auto maybe_rx_name = formatRxName(name);\n    if (!maybe_rx_name) { return Handle<T>::Null(); }\n    auto rx_name = maybe_rx_name.value();\n\n    auto rx = add<T>(rx_name.c_str(), args...);\n    if (!rx) { return rx; }\n\n    if (!omit_term) {\n      // auto adds a message available scheduling term\n      auto term = this->add<MessageAvailableSchedulingTerm>(rx_name.c_str());\n      term->setReceiver(rx);\n    }\n\n    std::string full_name = std::string(this->name()) + \"/\" + rx_name;\n    auto result = updatePort(name, full_name);\n    if (!result) {\n      GXF_LOG_ERROR(\"Failed to add Receiver [%s] with error [%s]\", rx_name.c_str(),\n                    GxfResultStr(result.error()));\n    }\n\n    return rx;\n  }\n\n  /**\n   * @brief Adds a component of Receiver of the corresponding type_name.\n   * Name of the receiver should match the parameter name of the underlying codelet\n   * The name of the receiver component is updated based on the parameter rank info.\n   *\n   * If codelet parameter is a scalar, name of the receiver is also same as the parameter key\n   * Parameter<Handle<Receiver>> | name - \"key\"\n   *\n   * If codelet parameter is a vector/array, the name of the receiver component is key_%d where\n   * 'd' is the index of this receiver in the codelet parameter.\n   * Parameter<Vector<Handle<Receiver>> | name - \"key_0\", \"key_1\", \"key_2\"\n   *\n   * @param type_name The fully qualified C++ type name of the receiver component\n   * @param name Name of the receiver component\n   * @param arg_list Arguments for the receiver component\n   * @param omit_term Boolean flag controlling whether or not a default message available\n   *                  scheduling term is added. If true, no scheduling term is added.\n   * @return Handle<Receiver> Handle to newly created receiver component\n   */\n  Handle<Receiver> addReceiver(const char* type_name, const char* name = nullptr,\n                               const std::vector<Arg>& arg_list = {}, bool omit_term = false);\n\n  /**\n   * @brief Receiver component lookup using name\n   *\n   * @param name name of a receiver component which has been previously created\n   * @return Handle<Receiver> Handle to receiver component if found, Null handle if\n   * no such component.\n   */\n  Handle<Receiver> getReceiver(const char* name) {\n    for (auto it = rx_queues_.begin(); it != rx_queues_.end(); ++it) {\n      if (!strcmp(it->second->name(), name)) { return it->second; }\n    }\n    return Handle<Receiver>::Null();\n  }\n\n  /**\n   * @brief Update the capacity and min_size parameter of a transmitter and its corresponding\n   * downstream receptive scheduling term\n   *\n   * @param name Name of the transmitter component\n   * @param capacity capacity of the transmitter to be set\n   * @param policy policy of the transmitter to be set\n   * @param min_size min size of the downstream receptive term to be set\n   * @return Expected<void> On success the function returns Success\n   */\n  Expected<void> configTransmitter(const char* name, uint64_t capacity, uint64_t policy,\n                                   uint64_t min_size) {\n    if (tx_queues_.find(name) == tx_queues_.end()) {\n      GXF_LOG_ERROR(\"Transmitter [%s] is not found in entity [%s]\", name, this->name());\n      return Unexpected{GXF_ENTITY_COMPONENT_NOT_FOUND};\n    }\n\n    Handle<Component> tx = tx_queues_.at(name);\n    tx->setParameter(\"capacity\", capacity);\n    tx->setParameter(\"policy\", policy);\n\n    if (terms_.find(name) == terms_.end()) {\n      GXF_LOG_ERROR(\"Scheduling term [%s] is not found in entity [%s]\", name, this->name());\n      return Unexpected{GXF_ENTITY_COMPONENT_NOT_FOUND};\n    }\n\n    Handle<Component> term = terms_.at(name);\n    term->setParameter(\"min_size\", min_size);\n\n    return Success;\n  }\n\n  /**\n   * @brief Update the capacity and min_size parameter of a receiver and its corresponding\n   * message available scheduling term\n   *\n   * @param name Name of the receiver component\n   * @param capacity capacity of the receiver to be set\n   * @param policy policy of the receiver to be set\n   * @param min_size min size of the message available term to be set\n   * @return Expected<void> On success the function returns Success\n   */\n  Expected<void> configReceiver(const char* name, uint64_t capacity, uint64_t policy,\n                                uint64_t min_size) {\n    if (rx_queues_.find(name) == rx_queues_.end()) {\n      GXF_LOG_ERROR(\"Receiver [%s] is not found in entity [%s]\", name, this->name());\n      return Unexpected{GXF_ENTITY_COMPONENT_NOT_FOUND};\n    }\n\n    Handle<Component> rx = rx_queues_.at(name);\n    rx->setParameter(\"capacity\", capacity);\n    rx->setParameter(\"policy\", policy);\n\n    if (terms_.find(name) == terms_.end()) {\n      GXF_LOG_ERROR(\"Scheduling term [%s] is not found in entity [%s]\", name, this->name());\n      return Unexpected{GXF_ENTITY_COMPONENT_NOT_FOUND};\n    }\n\n    Handle<Component> term = terms_.at(name);\n    term->setParameter(\"min_size\", min_size);\n\n    return Success;\n  }\n\n  Expected<void> activate() { return entity_.activate(); }\n\n  Expected<void> deactivate() { return entity_.deactivate(); }\n\n  gxf_context_t context() const { return entity_.context(); }\n\n  gxf_uid_t eid() const { return entity_.eid(); }\n\n  bool is_null() const { return entity_.is_null(); }\n\n  Handle<Codelet> get_codelet() { return codelet_; }\n\n  /**\n   * @brief The name of the entity or empty string if no name has been given to the entity.\n   *\n   * @return const char* pointer to name of the entity\n   */\n  const char* name() const { return entity_.name(); }\n\n  /**\n   * @brief Given a name for a transmitter to be connected to the codelet, return a formatted\n   * string back which can be used for a new transmitter component creation\n   * If the codelet's tx parameter is a scalar, the tx name is the same as the parameter key\n   * If the codelet's tx parameter is a vector, the tx name would be \"key_0\", \"key_1\" ...\n   *\n   * @param tx_name name of transmitter component\n   * @return Expected<std::string> formatted name of transmitter component\n   */\n  Expected<std::string> formatTxName(const char* tx_name);\n\n  /**\n   * @brief Given a name for a receiver to be connected to the codelet, return a formatted\n   * string back which can be used for a new receiver component creation\n   * If the codelet's rx parameter is a scalar, the rx name is the same as the parameter key\n   * If the codelet's rx parameter is a vector, the rx name would be \"key_0\", \"key_1\" ...\n   *\n   * @param rx_name name of receiver component\n   * @return Expected<std::string> formatted name of receiver component\n   */\n  Expected<std::string> formatRxName(const char* rx_name);\n\n  Expected<void> updatePort(const char* key, std::string value);\n\n private:\n  // Creates a generic component of type T and sets the parameter values from arg_list.\n  //\n  // Transmitters, Receivers, Clocks and Scheduling Term component names have to be unique.\n  // If the component type has been registered on any of the extensions which have been loaded, the\n  // new type would be registered to a runtime extension.\n  template <typename T, typename... Args>\n  Expected<Handle<T>> addComponent(const char* name = nullptr,\n                                   const std::vector<Arg>& arg_list = {}) {\n    static_assert(std::is_base_of<Component, T>::value, \"Requested type is not a component\");\n    // Check if another component with same name and base type exists\n    // return failure if it exists, otherwise proceed with component creation\n    if constexpr (std::is_base_of<Codelet, T>::value) {\n      if (!codelet_.is_null() && !strcmp(codelet_->name(), name)) {\n        GXF_LOG_ERROR(\"Codelet with same name [%s] already exists in entity [%s]\", name,\n                      entity_.name());\n        return Unexpected{GXF_FAILURE};\n      }\n    } else if constexpr (std::is_base_of<SchedulingTerm, T>::value) {\n      if (terms_.find(name) != terms_.end()) {\n        GXF_LOG_ERROR(\"Scheduling term with same name [%s] already exists in entity [%s]\", name,\n                      entity_.name());\n        return Unexpected{GXF_FAILURE};\n      }\n    } else if constexpr (std::is_base_of<Transmitter, T>::value) {\n      if (tx_queues_.find(name) != tx_queues_.end()) {\n        GXF_LOG_ERROR(\"Transmitter with same name [%s] already exists in entity [%s]\", name,\n                      entity_.name());\n        return Unexpected{GXF_FAILURE};\n      }\n    } else if constexpr (std::is_base_of<Receiver, T>::value) {\n      if (rx_queues_.find(name) != rx_queues_.end()) {\n        GXF_LOG_ERROR(\"Receiver with same name [%s] already exists in entity [%s]\", name,\n                      entity_.name());\n        return Unexpected{GXF_FAILURE};\n      }\n    } else if constexpr (std::is_base_of<Clock, T>::value) {\n      if (clocks_.find(name) != clocks_.end()) {\n        GXF_LOG_ERROR(\"Clock with same name [%s] already exists in entity [%s]\", name,\n                      entity_.name());\n        return Unexpected{GXF_FAILURE};\n      }\n    }\n\n    Expected<Handle<T>> maybe_component = entity_.add<T>(name);\n    if (!maybe_component) {\n      GXF_LOG_ERROR(\"Failed to add handle for component named [%s] to entity [%s] with error %s\",\n                    name, entity_.name(), GxfResultStr(maybe_component.error()));\n      return ForwardError(maybe_component);\n    }\n    Handle<T> component = maybe_component.value();\n\n    for (auto arg : arg_list) { applyArg(component, arg); }\n\n    if constexpr (std::is_base_of<Codelet, T>::value) {\n      if (!codelet_.is_null()) {\n        GXF_LOG_DEBUG(\n            \"Graph Entity is already configured with a codelet [%s]. New codelet \"\n            \"will be created but not managed by entity [%s]\",\n            codelet_->name(), entity_.name());\n      } else {\n        codelet_ = component;\n      }\n    } else if constexpr (std::is_base_of<SchedulingTerm, T>::value) {\n      terms_.emplace(name, component);\n    } else if constexpr (std::is_base_of<Transmitter, T>::value) {\n      tx_queues_.emplace(name, component);\n    } else if constexpr (std::is_base_of<Receiver, T>::value) {\n      rx_queues_.emplace(name, component);\n    } else if constexpr (std::is_base_of<Clock, T>::value) {\n      clocks_.emplace(name, component);\n    }\n\n    components_.emplace(component.get()->cid(), component);\n    return component;\n  }\n\n  // Create a Handle of the specified type corresponding to a fully qualified C++ type name and\n  // add it to the entity.\n  template <typename T>\n  Expected<Handle<T>> createHandle(const char* type_name, const char* name = nullptr) {\n    gxf_tid_t tid;\n    gxf_context_t context = entity_.context();\n\n    gxf_result_t result = GxfComponentTypeId(context, type_name, &tid);\n    if (!isSuccessful(result)) {\n      GXF_LOG_ERROR(\"Typename [%s] not found. Is this type registered?\", type_name);\n      return Unexpected{result};\n    }\n\n    auto maybe_untyped_handle = entity_.add(tid, name);\n    if (!maybe_untyped_handle) {\n      GXF_LOG_ERROR(\"Failed to add handle for [%s] to entity [%s] with error %s\", type_name,\n                    entity_.name(), GxfResultStr(maybe_untyped_handle.error()));\n      return ForwardError(maybe_untyped_handle);\n    }\n\n    auto maybe_handle = Handle<T>::Create(maybe_untyped_handle.value());\n    if (!maybe_handle) {\n      GXF_LOG_ERROR(\"Failed to convert untyped handle to handle with error %s\",\n                    GxfResultStr(maybe_handle.error()));\n      return ForwardError(maybe_handle);\n    }\n\n    return maybe_handle.value();\n  }\n\n  // A single codelet per GraphEntity\n  Handle<Codelet> codelet_{Handle<Codelet>::Null()};\n\n  // global component lookup based on uid\n  std::map<gxf_uid_t, Handle<Component>> components_;\n\n  // scheduling term component lookup by name\n  std::map<std::string, Handle<SchedulingTerm>> terms_;\n\n  // transmitter component lookup by name\n  std::map<std::string, Handle<Transmitter>> tx_queues_;\n\n  // receiver component lookup by name\n  std::map<std::string, Handle<Receiver>> rx_queues_;\n\n  // clock component lookup by name\n  std::map<std::string, Handle<Clock>> clocks_;\n\n  // resource lookup by name\n  std::map<std::string, Handle<ResourceBase>> resources_;\n\n  gxf_uid_t eid_{kNullUid};\n  Entity entity_;\n};\n\ntypedef std::shared_ptr<GraphEntity> GraphEntityPtr;\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GRAPH_ENTITY_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/graph_utils.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_GRAPH_UTILS_HPP_\n#define NVIDIA_GXF_GRAPH_UTILS_HPP_\n\n#include <string>\n\nnamespace nvidia {\nnamespace gxf {\n\n\nstatic gxf_tid_t generate_tid() {\n    std::random_device rd;\n    std::mt19937_64 generator(rd());\n    std::uniform_int_distribution<uint64_t> dis(0, UINT64_MAX);\n\n    // Generate two 64-bit random numbers to create a UUID\n    uint64_t random1 = dis(generator);\n    uint64_t random2 = dis(generator);\n\n    // Combine the random numbers to create a UUID string\n    std::string uuid_str = std::to_string(random1) + std::to_string(random2);\n    GXF_LOG_VERBOSE(\"UUID generated %s\", uuid_str.c_str());\n\n    return gxf_tid_t{random1, random2};\n}\n\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GRAPH_UTILS_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/proxy_component.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_PROXY_COMPONENT_HPP_\n#define NVIDIA_GXF_PROXY_COMPONENT_HPP_\n\n#include <map>\n#include <string>\n#include <vector>\n\n#include \"common/assert.hpp\"\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/expected_macro.hpp\"\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief A type to aid in lazy component creation by storing component type\n * and arg info\n *\n */\nclass ProxyComponent {\n public:\n  ProxyComponent() {}\n  ProxyComponent(const std::string& type_name, const std::string& component_name,\n                 const std::vector<Arg>& args)\n      : typename_(type_name), component_name_(component_name), args_(args) {}\n\n  std::string type_name() const { return typename_; }\n\n  std::string name() const { return component_name_; }\n\n  std::vector<Arg> args() const { return args_; }\n\n private:\n  std::string typename_;\n  std::string component_name_;\n  std::vector<Arg> args_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_PROXY_COMPONENT_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/segment.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_SEGMENT_HPP_\n#define NVIDIA_GXF_SEGMENT_HPP_\n\n#include <algorithm>\n#include <cassert>\n#include <map>\n#include <memory>\n#include <random>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"common/assert.hpp\"\n#include \"gxf/app/arg.hpp\"\n#include \"gxf/app/arg_parse.hpp\"\n#include \"gxf/app/entity_group.hpp\"\n#include \"gxf/app/graph_entity.hpp\"\n#include \"gxf/app/graph_utils.hpp\"\n#include \"gxf/app/proxy_component.hpp\"\n#include \"gxf/core/expected_macro.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/default_extension.hpp\"\n#include \"gxf/std/resources.hpp\"\n#include \"gxf/std/scheduler.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief A entity - entity connection specified using tx and rx component names\n * tx - transmitter component name\n * rx - receiver component name\n * The queue names should match the parameter keys of the codelet in the corresponding GraphEntity\n * for a successful connection between the two graph entities.\n */\ntypedef struct PortPair {\n  PortPair(std::string tx, std::string rx) : tx(tx), rx(rx) {}\n  std::string tx;\n  std::string rx;\n} PortPair;\n\n/**\n * @brief An open port in an segment is specified using entity and queue component names.\n * SegmentPort name is in the format \"<Entity Name>.<Queue Name>\"\n * Entity Name - name of a graph entity created in the segment\n * Queue Name - tx or rx component name which should match the parameter keys of the codelet in\n *              the corresponding GraphEntity for a successful connection\n */\n\ntypedef struct SegmentPort {\n  auto split_string(std::string port_name) {\n    std::string entity;\n    std::string queue;\n\n    // Find the position of the dot\n    size_t index = port_name.find('.');\n    if (index != std::string::npos) {\n      entity = port_name.substr(0, index);\n      queue = port_name.substr(index + 1);\n    } else {\n      GXF_LOG_ERROR(\n          \"Invalid port name [%s]. Port names should follow [Entity.Queue] naming convention\",\n          port_name.c_str());\n    }\n\n    return std::make_pair(entity, queue);\n  }\n  auto split_string2(std::string port_name) {\n    size_t first_pivot = port_name.find(\".\");\n    size_t second_pivot = port_name.substr(first_pivot + 1).find(\".\");\n    size_t third_pivot = port_name.find_last_of(\".\");\n    if (first_pivot == std::string::npos\n      || second_pivot == std::string::npos\n      || third_pivot == std::string::npos) {\n      GXF_LOG_ERROR(\n          \"Invalid port name [%s]. For port names with 3 dots, \"\n          \"it should follow [Segment.Entity.Queue] naming convention\",\n          port_name.c_str());\n    }\n    std::string segment = port_name.substr(0, first_pivot);\n    std::string entity = port_name.substr(first_pivot + 1, second_pivot);\n    std::string queue = port_name.substr(third_pivot + 1);\n    return std::make_tuple(segment, entity, queue);\n  }\n\n  std::string to_string() {\n    if (!segment.empty()) {\n      return segment + \".\" + entity + \".\" + queue;\n    } else {\n      return entity + \".\" + queue;\n    }\n  }\n\n  SegmentPort(std::string name) {\n    int count = std::count(name.begin(), name.end(), '.');\n    if (count == 2) {\n      std::tie(segment, entity, queue) = split_string2(name);\n    } else {\n      std::tie(entity, queue) = split_string(name);\n    }\n  }\n  std::string segment;\n  std::string entity;\n  std::string queue;\n\n  bool operator==(const SegmentPort& other) const {\n    return segment == other.segment && entity == other.entity && queue == other.queue;\n  }\n} SegmentPort;\n\n\n/**\n * @brief A segment - segment connection specified using the segment port info\n * SegmentPort name is in the format \"<Entity Name>.<Queue Name>\" where the queues are tx\n * or rx components.\n * tx - SegmentPort in the source segment\n * rx - SegmentPort in the sink segment\n *\n * The queue names should match the parameter keys of the codelet in the corresponding GraphEntity\n * for a successful connection between the two segments\n */\ntypedef struct SegmentPortPair {\n  SegmentPortPair(std::string tx, std::string rx) : tx(tx), rx(rx) {}\n  SegmentPort tx;\n  SegmentPort rx;\n\n  bool operator==(const SegmentPortPair& other) const {\n    return tx == other.tx && rx == other.rx;\n  }\n} SegmentPortPair;\n\n/**\n * @brief Enum representing the type of scheduler to be used in the application.\n * This enum is primarily used as an input to setScheduler api\n *\n */\nenum class SchedulerType: int8_t {\n  kGreedy = 0,\n  kMultiThread,\n  KEventBased,\n};\n\nconst constexpr SchedulerType Greedy = SchedulerType::kGreedy;\nconst constexpr SchedulerType MultiThread = SchedulerType::kMultiThread;\nconst constexpr SchedulerType EventBased = SchedulerType::KEventBased;\n\n/**\n * @brief Segment is a group of graph entities created in a single GXF runtime context. A segment\n * will have its own scheduler. Graph entities in a segment are connected with each other via\n * double buffer transmitter and receiver components. A segment can also be connected other\n * segments via ucx transmitters and receivers.\n *\n * Segments are created and managed by the nvidia::gxf::Application class.\n */\nclass Segment {\n public:\n  Segment() = default;\n\n  virtual ~Segment() = default;\n\n  Segment(Segment&&) = delete;\n\n  Segment& operator=(Segment&&) = delete;\n\n  Segment(const Segment&) = delete;\n\n  Segment& operator=(const Segment&) = delete;\n\n  virtual void compose() {}\n\n  /**\n   * @brief Creates a graph entity with a codelet of type CodeletT along with a parameter pack of\n   * Arg & ProxyComponent. The codelet component will be used to auto populate connection queues and\n   * their corresponding scheduling terms. Args can be used to specify a variable list of components to be\n   * created along with the codelet in the graph entity. Args can also be used to specify a variable list\n   * of Arg type to update any parameter values of the codelet\n   *\n   * @tparam CodeletT Codelet component to be added in the Graph Entity\n   * @tparam Args Pack of Arg or ProxyComponent objects\n   * @param name Name of the graph entity\n   * @param args Pack of Arg or ProxyComponent objects\n   * @return GraphEntityPtr A newly created graph entity object with the requested components.\n   */\n  template <typename CodeletT, typename... Args>\n  GraphEntityPtr makeEntity(const char* name, Args... args) {\n    static_assert(std::is_base_of<Codelet, CodeletT>::value, \"Requested type is not a Codelet\");\n    gxf_tid_t tid;\n    gxf_result_t result = GxfComponentTypeId(context_, TypenameAsString<CodeletT>(), &tid);\n    if (result == GXF_FACTORY_UNKNOWN_CLASS_NAME) {\n      // dynamically register the new component\n      auto result = registerCodelet<CodeletT>();\n      if (result != GXF_SUCCESS) { return nullptr; }\n    }\n\n    auto entity = createGraphEntity(name);\n    auto codelet = entity->addCodelet<CodeletT>(name);\n\n    if constexpr (sizeof...(args) > 0) {\n      // First create all proxy components\n      std::vector<ProxyComponent> proxy_list = parseArgsOfType<ProxyComponent>(args...);\n      for (auto proxy : proxy_list) { createFromProxy(proxy, entity); }\n\n      // parse all codelet args\n      std::vector<Arg> arg_list = parseArgsOfType<Arg>(args...);\n\n      // filter all proxy component args and create them\n      auto proxy_args = filterProxyComponents(arg_list);\n      for (auto arg : proxy_args) {\n        auto proxy = arg.as<ProxyComponent>();\n        auto handle = createFromProxy(proxy, entity);\n        applyArg(codelet, Arg(arg.key(), handle));\n      }\n\n      for (auto arg : arg_list) { applyArg(codelet, arg); }\n    }\n\n    return entity;\n  }\n\n\n  /**\n   * @brief Creates a graph entity without a codelet and with a parameter pack of Arg\n   * & ProxyComponent. Args can be used to specify a variable list of components to be\n   * created along with the graph entity.\n   *\n   * @tparam Args Pack of Arg or ProxyComponent objects\n   * @param name Name of the graph entity\n   * @param args Pack of Arg or ProxyComponent objects\n   * @return GraphEntityPtr A newly created graph entity object with the requested components.\n   */\n  template <typename... Args>\n  GraphEntityPtr makeEntity(const char* name, Args... args) {\n    auto entity = createGraphEntity(name);\n\n    if constexpr (sizeof...(args) > 0) {\n      // First create all proxy components\n      std::vector<ProxyComponent> proxy_list = parseArgsOfType<ProxyComponent>(args...);\n      for (auto proxy : proxy_list) { createFromProxy(proxy, entity); }\n    }\n\n    return entity;\n  }\n\n  EntityGroupPtr makeEntityGroup(const char* name,\n    const std::vector<GraphEntityPtr>& entity_members = {}) {\n    auto entity_group = createEntityGroup(name);\n    entity_group->add(entity_members);\n\n    return entity_group;\n  }\n\n  /**\n   * @brief Creates a scheduling term of requested type and applies parameter component values\n   * from a parameter pack of arguments. This api does not create the requested gxf native component.\n   * A Proxy component value is returned which has the type info and arg list needed to create this\n   * scheduling term. createFromProxy() api is used to create this component given any specific GraphEntity.\n   *\n   * @tparam T Type of GXF scheduling term. Type must be derived from nvidia::gxf::SchedulingTerm type.\n   * @param name name of the component\n   * @param args Parameter pack of arguments / parameter values to be applied to the component\n   * @return ProxyComponent\n   */\n  template <typename T, typename... Args>\n  ProxyComponent makeTerm(const char* name, Args... args) {\n    static_assert(std::is_base_of<SchedulingTerm, T>::value,\n                  \"Requested type is not a SchedulingTerm\");\n    std::vector<Arg> arg_list;\n    const std::string type_name(TypenameAsString<T>());\n    if constexpr (sizeof...(args) > 0) { arg_list = parseArgsOfType<Arg>(args...); }\n    return ProxyComponent(type_name, name, arg_list);\n  }\n\n  /**\n   * @brief Creates a resource of requested type and applies parameter component values\n   * from a parameter pack of arguments. This api does not create the requested gxf native component.\n   * A Proxy component value is returned which has the type info and arg list needed to create this\n   * resource. createFromProxy() api is used to create this component given any specific GraphEntity.\n   *\n   * @tparam T Type of GXF resource. Type must be derived from nvidia::gxf::ResourceBase type.\n   * @param name name of the component\n   * @param args Parameter pack of arguments / parameter values to be applied to the component\n   * @return ProxyComponent\n   */\n  template <typename T, typename... Args>\n  ProxyComponent makeResource(const char* name, Args... args) {\n    // TODO(chandrahasj) Enable this check when allocator\n    // types have been updated with resource base type\n    // static_assert(std::is_base_of<ResourceBase, T>::value,\n    //               \"Requested type is not a Resource\");\n    std::vector<Arg> arg_list;\n    const std::string type_name(TypenameAsString<T>());\n    if constexpr (sizeof...(args) > 0) { arg_list = parseArgsOfType<Arg>(args...); }\n    return ProxyComponent(type_name, name, arg_list);\n  }\n\n  /**\n   * @brief Adds a clock component to the segment and applies parameter component values\n   * from a parameter pack of arguments.\n   *\n   * @tparam T Type of GXF clock component. Type must be derived from nvidia::gxf::Clock type.\n   * @param name Name of the clock component\n   * @param args Parameter pack of arguments / parameter values to be applied to the component\n   * @return Handle<Clock> Handle to newly created clock component. Null handle if component was not\n   * created.\n   */\n  template <typename ClockT, typename... Args>\n  Handle<Clock> setClock(const char* name, Args... args) {\n    static_assert(std::is_base_of<Clock, ClockT>::value, \"Requested type is not a Clock\");\n    if (!clock_entity_) { clock_entity_ = createGraphEntity(\"ClockEntity_\" + name_); }\n\n    auto clock = clock_entity_->add<ClockT>(name);\n    std::vector<Arg> arg_list;\n    if constexpr (sizeof...(args) > 0) { arg_list = parseArgsOfType<Arg>(args...); }\n    for (auto arg : arg_list) { applyArg(clock, arg); }\n    return clock;\n  }\n\n  /**\n   * @brief Adds a scheduler component to the segment and applies parameter component values\n   * from a parameter pack of arguments.\n   *\n   * @tparam T Type of GXF scheduler component. Type must be derived from nvidia::gxf::Scheduler type.\n   * @param args Parameter pack of arguments / parameter values to be applied to the component\n   * @return Handle<Scheduler> Handle to newly created scheduler component. Null handle if component was not\n   * created.\n   */\n  template <SchedulerType schedulerType, typename... Args>\n  Handle<Scheduler> setScheduler(Args... args) {\n    std::vector<Arg> arg_list;\n    if constexpr (sizeof...(args) > 0) { arg_list = parseArgsOfType<Arg>(args...); }\n\n    return setScheduler(schedulerType, arg_list);\n  }\n\n  /**\n   * @brief Add a scheduler to the segment based on the input SchedulerType enum. If the segment contains\n   * a clock component, the same component will be reused to configure the scheduler. If no clock components\n   * are found in the segment, a new RealTimeClock component will be added to the segment.\n   *\n   * @param scheduler Type of the scheduler to be added. One of kGreedy, kMultithread or kEventBased\n   * @return Handle<Scheduler> Handle to newly created scheduler component. Null handle if component was not\n   * created.\n   */\n  Handle<Scheduler> setScheduler(const SchedulerType& scheduler, std::vector<Arg> arg_list = {});\n\n  /**\n   * @brief Adds a double buffer queue based connection between two entities with\n   * 1:1 tx and rx connectivity.\n   *\n   * @param source Origin graph entity for the connection\n   * @param target Destination graph entity for the connection\n   * @return Expected<void> On success the function returns Success\n   */\n  virtual Expected<void> connect(GraphEntityPtr& source, GraphEntityPtr& target);\n\n  /**\n   * @brief Adds a single double buffer queue based connection between two entities\n   * with a port pair specified.\n   *\n   * @param source Origin graph entity for the connection\n   * @param target Destination graph entity for the connection\n   * @param port_pair Port pair containing info of connection to be created\n   * @return Expected<void> On success the function returns Success\n   */\n  virtual Expected<void> connect(GraphEntityPtr& source, GraphEntityPtr& target,\n                                 PortPair port_pair);\n\n\n  /**\n   * @brief Adds multiple double buffer queue based connections between two entities\n   * with many : many tx and rx. Connections between two graph entities are created\n   * sequentially.\n   *\n   * @param source Origin graph entity for the connection\n   * @param target Destination graph entity for the connection\n   * @param port_pairs List of port pairs containing info of connections to be created\n   * @return Expected<void> On success the function returns Success\n   */\n  virtual Expected<void> connect(GraphEntityPtr& source, GraphEntityPtr& target,\n                                 std::vector<PortPair> port_pairs);\n  /**\n   * @brief Get the Entity object with given name. A nullptr is returned if entity is\n   * not found.\n   *\n   * @param name Name of the graph entity. API is Case sensitive.\n   * @return std::shared_ptr<GraphEntity> Pointer to underlying graph entity object\n   */\n  std::shared_ptr<GraphEntity> getEntity(const char* name) {\n    for (auto& [uid, entity] : entities_) {\n      if (std::strcmp(name, entity->name()) == 0) { return entity; }\n    }\n    GXF_LOG_ERROR(\"Graph Entity [%s] not found\", name);\n    return nullptr;\n  }\n\n  /**\n   * @brief Fetch the name of the segment\n   *\n   * @return const char* pointer to name of the segment\n   */\n  const char* name() const { return name_.c_str(); }\n\n  /**\n   * @brief Fetch the context of a segment\n   *\n   * @return gxf_context_t\n   */\n  gxf_context_t context() { return context_; }\n\n  /**\n   * @brief Activates all the graph entities in the segment\n   *\n   * @return Expected<void> On success the function returns Success\n   */\n  Expected<void> activate() {\n    GXF_LOG_INFO(\"Activating segment [%s] ....\", name_.c_str());\n    gxf_result_t result = GxfGraphActivate(context_);\n    return result == GXF_SUCCESS ? Success : Unexpected{result};\n  }\n\n  /**\n   * @brief Deactivates all the graph entities in the segment\n   *\n   * @return Expected<void> On success the function returns Success\n   */\n  Expected<void> deactivate() {\n    GXF_LOG_INFO(\"Deactivating segment [%s] ....\", name_.c_str());\n    gxf_result_t result = GxfGraphDeactivate(context_);\n    return result == GXF_SUCCESS ? Success : Unexpected{result};\n  }\n\n  /**\n   * @brief A blocking api to run the segment. This thread is blocked (sleeping)\n   * until the segment execution is complete.\n   *\n   * @return Expected<void> On success the function returns Success\n   */\n  Expected<void> run() {\n    GXF_LOG_INFO(\"Running segment [%s] ....\", name_.c_str());\n    gxf_result_t result = GxfGraphRun(context_);\n    return result == GXF_SUCCESS ? Success : Unexpected{result};\n  }\n\n  /**\n   * @brief A non blocking api to execute a segment. API returns immediately after\n   * starting the segment execution. wait() can be used to wait until execution\n   * has finished.\n   * @return Expected<void> On success the function returns Success\n   */\n  Expected<void> runAsync() {\n    GXF_LOG_INFO(\"Running segment [%s] ....\", name_.c_str());\n    gxf_result_t result = GxfGraphRunAsync(context_);\n    return result == GXF_SUCCESS ? Success : Unexpected{result};\n  }\n\n  /**\n   * @brief A non blocking api to stop a previously running segment. Segment is not guaranteed\n   * to have stopped when this api returns. wait() can be used to wait until the execution\n   * has finished.\n   *\n   * @return Expected<void> On success the function returns Success\n   */\n  Expected<void> interrupt() {\n    GXF_LOG_INFO(\"Interrupting segment [%s] ....\", name_.c_str());\n    gxf_result_t result = GxfGraphInterrupt(context_);\n    return result == GXF_SUCCESS ? Success : Unexpected{result};\n  }\n\n  /**\n   * @brief A blocking API to wait until the segment execution has completed.\n   *\n   * @return gxf_result_t On success the function returns GXF_SUCCESS.\n   */\n  Expected<void> wait() {\n    GXF_LOG_INFO(\"Waiting on segment [%s]....\", name_.c_str());\n    gxf_result_t result = GxfGraphWait(context_);\n    return result == GXF_SUCCESS ? Success : Unexpected{result};\n  }\n\n  /**\n   * @brief Sets the severity level of the logs (corresponding to GXF_LOG_* logging macros)\n   *  for a segment\n   *\n   * @param severity a valid severity level as defined in `gxf_severity_t`. Logs corresponding to\n   *                any level <= severity will be logged.\n   * @return gxf_result_t On success the function returns GXF_SUCCESS.\n   */\n  gxf_result_t setSeverity(gxf_severity_t severity) { return GxfSetSeverity(context_, severity); }\n\n  /**\n   * @brief Saves the segment information containing entities, components and their corresponding\n   * parameter values in a yaml representation\n   *\n   * @param filepath path to save the resulting graph yaml file\n   * @return Expected<void> On success the function returns Success\n   */\n  Expected<void> saveToFile(const char* filepath) {\n    gxf_result_t code = GxfGraphSaveToFile(context_, filepath);\n    return ExpectedOrCode(code);\n  }\n\n  /**\n   * @brief Loads parameters for graph entities composed in the segment / application.\n   * YAML file follows the GXF graph specification\n   *\n   * @param filepath path to a valid parameters file\n   * @return Expected<void> On success the function returns Success\n   */\n  Expected<void> loadParameterFile(const char* filepath) {\n    gxf_result_t code = GxfGraphLoadFile(context_, filepath);\n    return ExpectedOrCode(code);\n  }\n\n  /**\n   * @brief Create a Network Context in the segment which can be used by UCX Connections\n   * added in the application. A new graph entity with the name \"NetworkContext\" will be added\n   * to the segment context with a UcxContext component and a corresponding entity and component\n   * serializers\n   *\n   * @return Expected<void> On success the function returns Success\n   */\n  Expected<void> createNetworkContext();\n\n  /**\n   * @brief Creates a component in graph entity based on the type information from the ProxyComponent\n   *\n   * @param component A proxy component object\n   * @param entity A pointer to graph entity to be used for creating the component\n   * @return Handle<Component> Handle to newly created component\n   */\n  Handle<Component> createFromProxy(ProxyComponent& component, GraphEntityPtr& entity);\n\n  /**\n   * @brief This function is expected to be called by the application layer to assign a context to the\n   * segment and a runtime extension for on the fly registration of components\n   *\n   * @param segment_context A valid GXF context to be assigned to the segment\n   * @param name A valid name for the segment\n   * @param runtime_ext Pointer to a GXF extension which can be used to register any components at runtime\n   * @return Expected<void> On success the function returns Success\n   */\n  Expected<void> setup(gxf_context_t segment_context, const char* name,\n                       std::shared_ptr<DefaultExtension> runtime_ext) {\n    name_.assign(name);\n    context_ = segment_context;\n    runtime_ext_ = runtime_ext;\n    is_setup_ = true;\n    return Success;\n  }\n\n  Expected<void> setName(const char* name) {\n    name_ = name;\n    return Success;\n  }\n\n  Expected<void> checkConfiguration() { return Success; }\n\n protected:\n  /**\n   * @brief Creates a programmable graph entity with the given name\n   *\n   * @param name Name of the graph entity\n   * @return GraphEntityPtr Pointer to newly created entity\n   */\n  GraphEntityPtr createGraphEntity(std::string name) {\n    GXF_LOG_DEBUG(\"Creating graph entity [%s]\", name.c_str());\n    auto entity = std::make_shared<GraphEntity>();\n    if (!entity->setup(context_, name.c_str())) {\n      return nullptr;\n    }\n    entities_.emplace(entity->eid(), entity);\n    return entity;\n  }\n\n  /**\n   * @brief Creates a programmable graph entity group with the given name\n   *\n   * @param name Name of the graph entity\n   * @return EntityGroupPtr Pointer to newly created entity group\n   */\n  EntityGroupPtr createEntityGroup(std::string name) {\n    GXF_LOG_DEBUG(\"Creating graph entity group [%s]\", name.c_str());\n    auto entity_group = std::make_shared<EntityGroup>();\n    if (!entity_group->setup(context_, name.c_str())) {\n      return nullptr;\n    }\n    entity_groups_.emplace(entity_group->gid(), entity_group);\n    return entity_group;\n  }\n\n  // Registers a new codelet type with the runtime extension\n  template <typename T>\n  gxf_result_t registerCodelet() {\n    static_assert(std::is_base_of<Codelet, T>::value, \"Requested type is not a Codelet\");\n    gxf_tid_t new_tid = generate_tid();\n    auto result = runtime_ext_->add<T, Codelet>(new_tid, TypenameAsString<T>(),\n                                                TypenameAsString<T>(), TypenameAsString<T>());\n    if (!result) { return ToResultCode(result); }\n    return GxfRegisterComponentInExtension(context_, new_tid, runtime_ext_->tid());\n  }\n\n  // Extension to register components on the fly\n  std::shared_ptr<DefaultExtension> runtime_ext_;\n  std::map<gxf_uid_t, GraphEntityPtr> entities_;\n  std::map<gxf_uid_t, EntityGroupPtr> entity_groups_;\n  GraphEntityPtr scheduler_entity_;\n  GraphEntityPtr clock_entity_;\n  GraphEntityPtr connect_entity_;\n  GraphEntityPtr network_ctx_;\n  gxf_context_t context_ = kNullContext;\n  std::string name_;\n  bool is_setup_{false};\n};\n\ntypedef std::shared_ptr<Segment> SegmentPtr;\n\nstruct SegmentConnection {\n  SegmentConnection(SegmentPtr source, SegmentPtr target, std::vector<SegmentPortPair> port_maps)\n    : source(source), target(target), port_maps(port_maps) {}\n  SegmentPtr source;\n  SegmentPtr target;\n  std::vector<SegmentPortPair> port_maps;\n\n  bool operator==(const SegmentConnection& other) const {\n    if (source != other.source || target != other.target) {\n      return false;\n    }\n    if (port_maps.size() != other.port_maps.size()) {\n      return false;\n    }\n    for (size_t i = 0; i < port_maps.size(); i++) {\n      if (!(port_maps.at(i) == other.port_maps.at(i))) {\n        return false;\n      }\n    }\n    return true;\n  }\n\n  struct Hash {\n    std::size_t operator()(const SegmentConnection& sc) const {\n      std::size_t h1 = std::hash<SegmentPtr>{}(sc.source);\n      std::size_t h2 = std::hash<SegmentPtr>{}(sc.target);\n      std::size_t h3 = std::hash<std::size_t>{}(sc.port_maps.size());\n      return h1 ^ h2 ^ h3;\n    }\n  };\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SEGMENT_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/app/worker.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_APPLICATION_WORKER_HPP_\n#define NVIDIA_GXF_APPLICATION_WORKER_HPP_\n\n#include <memory>\n#include <string>\n#include <unordered_set>\n\n#include \"gxf/app/segment.hpp\"\n#include \"gxf/std/graph_worker.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nclass Application;\n\n/**\n * @brief GraphWorker representation in Application API layer\n * It manages the execution of selected segments\n *\n */\nclass Worker {\n public:\n  Worker(Application* owner, const std::string& name);\n\n  std::string name() { return name_; }\n  Expected<void> setPort(uint32_t port) { port_ = port; return Success; }\n  Expected<void> setDriverIp(const std::string& ip) { driver_ip_ = ip; return Success; }\n  Expected<void> setDriverPort(uint32_t port) { driver_port_ = port; return Success; }\n  // set selected segments to run\n  Expected<void> setSegments(const std::unordered_set<std::string>& segment_names) {\n    segment_names_ = segment_names;\n    return Success;\n  }\n  // commit the setup into GraphWorker\n  Expected<void> commit();\n\n private:\n  Application* owner_;\n  std::string name_;\n  GraphEntityPtr worker_entity_;\n  // core component\n  Handle<GraphWorker> graph_worker_;\n  // server interface object\n  Handle<IPCServer> server_;\n  // client interface object\n  Handle<IPCClient> client_;\n\n  // worker server's own port\n  uint32_t port_ = 0;\n  // target driver server's IP address\n  std::string driver_ip_;\n  // target driver server's port\n  uint32_t driver_port_ = 0;\n  // selected segments to run by this worker\n  std::unordered_set<std::string> segment_names_;\n};\ntypedef std::shared_ptr<Worker> WorkerPtr;\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_APPLICATION_WORKER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/behavior_tree/constant_behavior.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_BT_CONSTANT_BEHAVIOR_HPP_\n#define NVIDIA_GXF_BT_CONSTANT_BEHAVIOR_HPP_\n\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/controller.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Constant Behavior Codelet switches its own status to the configured desired\n// ||constant_status| after each tick.\nclass ConstantBehavior : public Codelet {\n public:\n  virtual ~ConstantBehavior() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t tick() override;\n\n private:\n  // the desired status to switch to during each tick\n  Parameter<size_t> constant_status_;\n  enum ConstantBehaviorType {\n    CONSTANT_SUCCESS = 0,\n    CONSTANT_FAILURE = 1,\n  };\n\n  // its own scheduling term to start/stop itself\n  Parameter<Handle<nvidia::gxf::BTSchedulingTerm>> s_term_;\n\n  SchedulingConditionType ready_conditions;\n  SchedulingConditionType never_conditions;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_BT_CONSTANT_BEHAVIOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/behavior_tree/entity_count_failure_repeat_controller.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_BT_ENTITY_COUNT_FAILURE_REPEAT_CONTROLLER_HPP_\n#define NVIDIA_GXF_BT_ENTITY_COUNT_FAILURE_REPEAT_CONTROLLER_HPP_\n\n#include \"gxf/core/parameter_parser_std.hpp\"\n#include \"gxf/std/controller.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Repeat the entity on failure up to |max_repeat_count_| times, then deactivate\n// the entity\nclass EntityCountFailureRepeatController : public Controller {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_controller_status_t control(gxf_uid_t eid, Expected<void> code) override;\n\n private:\n  size_t repeat_count_;\n  Parameter<size_t> max_repeat_count_;\n  gxf_controller_status_t controller_status;\n  // Take gxf_result_t codelet::tick() to entity_state_t\n  // controller_status.behavior_status\n  entity_state_t setBehaviorStatus(gxf_result_t tick_result);\n  // Set execution status based on behavior status and repeat counter\n  gxf_execution_status_t setExecStatus(entity_state_t& behavior_status);\n  // if code = EntityItem::execute() returns FAILURE & exec_status !=\n  // GXF_EXECUTE_FAILURE set behavior status to GXF_BEHAVIOR_RUNNING so that\n  // parent codelet knows it's running\n  Parameter<bool> return_behavior_running_if_failure_repeat_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_BT_ENTITY_COUNT_FAILURE_REPEAT_CONTROLLER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/behavior_tree/parallel_behavior.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_BT_PARALLEL_BEHAVIOR_HPP_\n#define NVIDIA_GXF_BT_PARALLEL_BEHAVIOR_HPP_\n\n#include <vector>\n\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/controller.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Runs its child entities in parallel. By default, succeeds when all child\n// entities succeed and fails when all child entities fail.\nclass ParallelBehavior : public Codelet {\n public:\n  virtual ~ParallelBehavior() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t tick() override;\n\n private:\n  // parent codelet points to children's scheduling terms to schedule child\n  // entities\n  Parameter<std::vector<Handle<nvidia::gxf::BTSchedulingTerm> > > children_;\n  std::vector<Handle<nvidia::gxf::BTSchedulingTerm> > children;\n  std::vector<gxf_uid_t> children_eid;\n  // its own scheduling term to start/stop itself\n  Parameter<Handle<nvidia::gxf::BTSchedulingTerm> > s_term_;\n  Handle<nvidia::gxf::BTSchedulingTerm> s_term;\n  size_t getNumChildren() const;\n  entity_state_t GetChildStatus(size_t child_id);\n  gxf_result_t startChild(size_t child_id);\n  gxf_result_t stopAllChild();\n  Parameter<int64_t> success_threshold_;\n  Parameter<int64_t> failure_threshold_;\n  SchedulingConditionType ready_conditions;\n  SchedulingConditionType never_conditions;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_BT_PARALLEL_BEHAVIOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/behavior_tree/repeat_behavior.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_BT_REPEAT_BEHAVIOR_HPP_\n#define NVIDIA_GXF_BT_REPEAT_BEHAVIOR_HPP_\n\n#include <vector>\n\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/controller.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Repeat Behavior\n// Repeats its only child entity. By default, won’t repeat when the child entity\n// fails. This can be customized using parameters.\nclass RepeatBehavior : public Codelet {\n public:\n  virtual ~RepeatBehavior() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t tick() override;\n\n private:\n  // parent codelet points to children's scheduling terms to schedule child\n  // entities\n  Parameter<std::vector<Handle<nvidia::gxf::BTSchedulingTerm> > > children_;\n  std::vector<Handle<nvidia::gxf::BTSchedulingTerm> > children;\n  std::vector<gxf_uid_t> children_eid;\n  // its own scheduling term to start/stop itself\n  Parameter<Handle<nvidia::gxf::BTSchedulingTerm> > s_term_;\n  Handle<nvidia::gxf::BTSchedulingTerm> s_term;\n  Parameter<bool> repeat_after_failure_;\n  size_t getNumChildren() const;\n  entity_state_t GetChildStatus(size_t child_id);\n  gxf_result_t startChild(size_t child_id);\n  SchedulingConditionType ready_conditions;\n  SchedulingConditionType never_conditions;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_BT_REPEAT_BEHAVIOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/behavior_tree/selector_behavior.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_BT_SELECTOR_BEHAVIOR_HPP_\n#define NVIDIA_GXF_BT_SELECTOR_BEHAVIOR_HPP_\n\n#include <vector>\n\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/controller.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Selector Behavior\n// Runs all child entities in sequence until one succeeds, then reports success.\n// If all child entities fail (or no child entities are present), this codelet\n// fails.\nclass SelectorBehavior : public Codelet {\n public:\n  virtual ~SelectorBehavior() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t tick() override;\n\n private:\n  // parent codelet points to children's scheduling terms to schedule child\n  // entities\n  Parameter<std::vector<Handle<nvidia::gxf::BTSchedulingTerm> > > children_;\n  std::vector<Handle<nvidia::gxf::BTSchedulingTerm> > children;\n  std::vector<gxf_uid_t> children_eid;\n  // its own scheduling term to start/stop itself\n  Parameter<Handle<nvidia::gxf::BTSchedulingTerm> > s_term_;\n  Handle<nvidia::gxf::BTSchedulingTerm> s_term;\n  size_t getNumChildren() const;\n  entity_state_t GetChildStatus(size_t child_id);\n  gxf_result_t startChild(size_t child_id);\n  size_t current_child_id;\n  SchedulingConditionType ready_conditions;\n  SchedulingConditionType never_conditions;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_BT_SELECTOR_BEHAVIOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/behavior_tree/sequence_behavior.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_BT_SEQUENCE_BEHAVIOR_HPP_\n#define NVIDIA_GXF_BT_SEQUENCE_BEHAVIOR_HPP_\n\n#include <vector>\n\n#include \"common/fixed_vector.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/controller.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Sequence Behavior\n// Runs its child entities in sequence, in the order in which they are defined.\n// Succeeds when all child entities succeed, or fails as soon as one child\n// entity fails.\nclass SequenceBehavior : public Codelet {\n public:\n  virtual ~SequenceBehavior() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t tick() override;\n\n private:\n  // parent codelet points to children's scheduling terms to schedule child\n  // entities\n  Parameter<FixedVector<Handle<nvidia::gxf::BTSchedulingTerm>, kMaxComponents> >\n      children_;\n  FixedVector<Handle<nvidia::gxf::BTSchedulingTerm>, kMaxComponents> children;\n  std::vector<gxf_uid_t> children_eid;\n  // its own scheduling term to start/stop itself\n  Parameter<Handle<nvidia::gxf::BTSchedulingTerm> > s_term_;\n  Handle<nvidia::gxf::BTSchedulingTerm> s_term;\n  size_t getNumChildren() const;\n  entity_state_t GetChildStatus(size_t child_id);\n  gxf_result_t startChild(size_t child_id);\n  size_t current_child_id;\n  SchedulingConditionType ready_conditions;\n  SchedulingConditionType never_conditions;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_BT_SEQUENCE_BEHAVIOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/behavior_tree/switch_behavior.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_BT_SWITCH_BEHAVIOR_HPP_\n#define NVIDIA_GXF_BT_SWITCH_BEHAVIOR_HPP_\n\n#include <vector>\n\n#include \"common/fixed_vector.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/controller.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Switch Behavior\n// Runs the child entity with the index defined as desired_behavior.\nclass SwitchBehavior : public Codelet {\n public:\n  virtual ~SwitchBehavior() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t tick() override;\n\n private:\n  // parent codelet points to children's scheduling terms to schedule child\n  // entities\n  // Parameter<std::vector<Handle<nvidia::gxf::BTSchedulingTerm> > > children_;\n  // std::vector<Handle<nvidia::gxf::BTSchedulingTerm> > children;\n  Parameter<FixedVector<Handle<nvidia::gxf::BTSchedulingTerm>, kMaxComponents> >\n      children_;\n  FixedVector<Handle<nvidia::gxf::BTSchedulingTerm>, kMaxComponents> children;\n  std::vector<gxf_uid_t> children_eid;\n  // its own scheduling term to start/stop itself\n  Parameter<Handle<nvidia::gxf::BTSchedulingTerm> > s_term_;\n  Handle<nvidia::gxf::BTSchedulingTerm> s_term;\n\n  // the child entity to switch to. this child entity will run when this parent\n  // entity runs and the parent entity will return this child entity's behavior\n  // status may not be empty\n  Parameter<size_t> desired_behavior_;\n  size_t getNumChildren() const;\n  entity_state_t GetChildStatus(size_t child_id);\n  gxf_result_t startChild(size_t child_id);\n  size_t current_child_id;\n  SchedulingConditionType ready_conditions;\n  SchedulingConditionType never_conditions;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_BT_SWITCH_BEHAVIOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/behavior_tree/timer_behavior.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_BT_TIMER_BEHAVIOR_HPP_\n#define NVIDIA_GXF_BT_TIMER_BEHAVIOR_HPP_\n\n#include \"gxf/std/clock.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/controller.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Timer Behavior\n// Waits for a specified amount of time delay,\n// and switches to the configured result switch_status afterwards.\nclass TimerBehavior : public Codelet {\n public:\n  virtual ~TimerBehavior() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t tick() override;\n\n private:\n  // a specified time (in seconds) after which the status of this node will be\n  // changed\n  Parameter<double> delay_;\n\n  // the desired status to switch to when the tick time is delay_ seconds after\n  // first tick Setting this value to “running” will lead to undefined behavior\n  // in the application.\n  Parameter<size_t> switch_status_;\n  enum SwitchStatus {\n    kSwitchToSuccess = 0,\n    kSwitchToFailure = 1,\n    kSwitchToRunning = 2,\n  };\n\n  // Use this clock to get wait time since first tick\n  Parameter<Handle<Clock>> clock_;\n\n  // its own scheduling term to start/stop itself\n  Parameter<Handle<nvidia::gxf::BTSchedulingTerm>> s_term_;\n\n  bool is_first_tick_ = true;\n\n  // timestamp of the last tick which caused a status switch\n  int64_t last_switch_timestamp_ = 0;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_BT_TIMER_BEHAVIOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/common_expected_macro.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma once\n\n#include <optional>\n#include <string>\n#include <type_traits>\n#include <utility>\n\n#include \"common/expected.hpp\"\n\n// Concatenates its two arguments.\n#define EXPECTED_MACRO_INTERNAL_CONCAT(a, b) EXPECTED_MACRO_INTERNAL_CONCAT_IMPL(a, b)\n#define EXPECTED_MACRO_INTERNAL_CONCAT_IMPL(a, b) a##b\n\n// Converts its argument to a string at compile time.\n#define EXPECTED_MACRO_INTERNAL_TO_STRING(x) EXPECTED_MACRO_INTERNAL_TO_STRING_IMPL(x)\n#define EXPECTED_MACRO_INTERNAL_TO_STRING_IMPL(x) #x\n\n// Gets the current location in the source code in the format \"file:line\".\n#define EXPECTED_MACRO_INTERNAL_FILE_LINE() __FILE__ \":\" EXPECTED_MACRO_INTERNAL_TO_STRING(__LINE__)\n\n// Helper to support logging a default message and an optional custom message.\n#define EXPECTED_MACRO_INTERNAL_LOG_IN_EXPECT_MACRO(expression_result, expression_string, ...) \\\n  nvidia::expected_macro::LogHelper(__FILE__, __LINE__, expression_result, expression_string,      \\\n                                ##__VA_ARGS__);\n\n// Helper to check the return type of the expression used in RETURN_IF_ERROR.\n#define EXPECTED_MACRO_INTERNAL_CHECK_EXPRESSION_IS_RESULT(expression_result) \\\n  static_assert(                                                                                   \\\n      nvidia::expected_macro::IsStatus<decltype(expression_result)>::value ||              \\\n      nvidia::expected_macro::IsUnwrappable<decltype(expression_result)>::value ,          \\\n      EXPECTED_MACRO_INTERNAL_FILE_LINE()                                                          \\\n          \": RETURN_IF_ERROR can only be used with expressions that return a result type. You \"    \\\n          \"can register a type as a result type unwrappable with one of\"                           \\\n          \"`template <> struct IsUnwrappable<MyType> : std::true_type {};` or \"                    \\\n          \"`template <> struct IsStatus<MyType> : std::true_type {}`. \");\n\n// Helper to check the return type of the expression used in UNWRAP_OR_RETURN.\n#define EXPECTED_MACRO_INTERNAL_CHECK_EXPRESSION_IS_UNWRAPPABLE(expression_result) \\\n  static_assert(                                                                                   \\\n      nvidia::expected_macro::IsUnwrappable<decltype(expression_result)>::value,           \\\n      EXPECTED_MACRO_INTERNAL_FILE_LINE()                                                          \\\n          \": UNWRAP_OR_RETURN can only be used with expressions that return an unwrappable type. \" \\\n          \"You can register a type as being unwrappable with \"                                     \\\n          \"`template <> struct IsUnwrappable<MyType> : std::true_type {};`. For expressions \"      \\\n          \"returning a status instead of an unwrappable use `RETURN_IF_ERROR` instead.\");\n\n// Evaluates an expression that returns a result type. If the returned result contains an error it\n// returns the error. This macro can only be used in functions that also return a result type.\n//\n// Per default the macro already creates an error message that includes the evaluated expression. If\n// needed an optional string can be passed that will be appended to the default error message. It is\n// also possible to use format specifiers to customize the string.\n//\n// It is also possible to pass the Severity used for logging as an additional argument. This is\n// required if using a custom error message.\n//\n// Example:\n// Expected<void> DoSomething();\n// Expected<void> DoAnotherThing();\n//\n// Expected<void> foo(){\n//   RETURN_IF_ERROR(DoSomething());\n//   RETURN_IF_ERROR(DoAnotherThing(), Severity::WARNING);\n//   RETURN_IF_ERROR(DoAnotherThing(), Severity::WARNING, \"Custom error message.\");\n// }\n#define RETURN_IF_ERROR(expression, ...)                                                    \\\n  ({                                                                                      \\\n    auto maybe_result = (expression);                                                       \\\n    EXPECTED_MACRO_INTERNAL_CHECK_EXPRESSION_IS_RESULT(maybe_result)                        \\\n    if (!nvidia::expected_macro::IsValid(maybe_result)) {                           \\\n      EXPECTED_MACRO_INTERNAL_LOG_IN_EXPECT_MACRO(maybe_result, #expression, ##__VA_ARGS__) \\\n      return nvidia::expected_macro::ProxyFactory::FromStatusOrUnwrappable(maybe_result);   \\\n    }                                                                                       \\\n  })\n\n// Evaluates an expression that returns a type that can be casted to a boolean. If the expression is\n// false it returns an error. This macro can only be used in functions returning a result type.\n//\n// The difference to RETURN_IF_ERROR is that this macro will always cast the expression to a\n// boolean. Thus if your expression returns a status that can be more than just true/false you will\n// loose the additional information. In those cases RETURN_IF_ERROR is preferred. Use this macro\n// only to check preconditions like a < b, or c != nullptr.\n//\n// Per default the macro already creates an error message that includes the evaluated expression. If\n// needed an optional string can be passed that will be appended to the default error message. It is\n// also possible to use format specifiers to customize the string.\n//\n// It is also possible to pass the Severity used for logging as an additional argument. This is\n// required if using a custom error message.\n//\n// Example:\n// Expected<void> foo(){\n//   RETURN_IF_FALSE(1 > 2);\n//   RETURN_IF_FALSE(1 > 2, Severity::WARNING);\n//   RETURN_IF_FALSE(1 > 2, Severity::WARNING, \"1 is not bigger than 2\");\n// }\n#define RETURN_IF_FALSE(expression, ...)                                              \\\n  do {                                                                                \\\n    const bool result = static_cast<bool>(expression);                                \\\n    if (!result) {                                                                    \\\n      EXPECTED_MACRO_INTERNAL_LOG_IN_EXPECT_MACRO(result, #expression, ##__VA_ARGS__) \\\n      return nvidia::expected_macro::ProxyFactory::FromStatusOrUnwrappable(result);   \\\n    }                                                                                 \\\n  } while (0)\n\n// Evaluates an expression that returns an unwrappable type. If the returned type contains an error\n// it returns the error, else it unwraps the value. This macro can only be used in functions\n// returning a result type.\n//\n// Per default the macro already creates an error message that includes the evaluated expression. If\n// needed an optional string can be passed that will be appended to the default error message. It is\n// also possible to use format specifiers to customize the string.\n//\n// It is also possible to pass the Severity used for logging as an additional argument.\n//\n// Note that this macro uses expression-statements (i.e. the ({ }) surrounding the macro) which are\n// a non-standard functionality. However they are present in almost all compilers. We currently only\n// know of MSVC that does not support this.\n//\n// Example:\n// Expected<std::string> GetString();\n// Expected<std::string> GetAnotherString();\n//\n// Expected<int> CountCombinedStringLength(){\n//   const std::string str1 = UNWRAP_OR_RETURN(GetString());\n//   std::string str2;\n//   str2 = UNWRAP_OR_RETURN(GetAnotherString(), \"This should not fail. Str1 has value %s.\",\n//       str1.c_str());\n//   const std::string str3 = UNWRAP_OR_RETURN(GetAnotherString(), Severity::WARNING);\n//   const std::string str4 = UNWRAP_OR_RETURN(GetAnotherString(), Severity::WARNING,\n//       \"Custom error message\");\n//   return str1.size() + str2.size() + str3.size() + str4.size();\n// }\n#define UNWRAP_OR_RETURN(expression, ...)                                                   \\\n  ({                                                                                        \\\n    auto maybe_result = (expression);                                                       \\\n    EXPECTED_MACRO_INTERNAL_CHECK_EXPRESSION_IS_UNWRAPPABLE(maybe_result)                    \\\n    if (!nvidia::expected_macro::IsValid(maybe_result)) {                                       \\\n      EXPECTED_MACRO_INTERNAL_LOG_IN_EXPECT_MACRO(maybe_result, #expression, ##__VA_ARGS__) \\\n      return nvidia::expected_macro::ProxyFactory::FromUnwrappable(maybe_result);             \\\n    }                                                                                       \\\n    std::move(maybe_result.value());                                                        \\\n  })\n\n// All functions in the following namespace have to be specialized for a given result type in order\n// to enable the macro for the type.\nnamespace nvidia::expected_macro {\n\n// Type trait to check if a type can be used as a status.\ntemplate <typename /*Type*/>\nstruct IsStatus : std::false_type {};\n\n// Type trait to check if a type can be used as an unwrappable.\ntemplate <typename /*Type*/>\nstruct IsUnwrappable : std::false_type {};\n\n// The value that represents the default error for a given status type.\ntemplate <typename Status>\nconstexpr Status DefaultError();\n\n// Check if a result types is valid.\ntemplate <typename Result>\nconstexpr bool IsValid(const Result& result) {\n  return static_cast<bool>(result);\n}\n\n// Helper struct to get the status type of a result. This is needed for the StatusValue helper below\n// to automatically infer the return type.\n//\n// Per default we use the same type as the\n// result, which holds for all status types. Thus this only has to be specialized for unwrappable\n// types.\ntemplate <typename Result>\nstruct StatusType {\n  using Type = Result;\n};\n\n// Helper function to get the status of a result. We have to use a static member function instead of\n// a free function because we want the possibility for a user to only provide a partial template\n// specialization of this.\ntemplate <typename Result>\nstruct StatusValue {\n  static constexpr typename StatusType<Result>::Type Get(const Result& result) { return result; }\n};\n\n// Get the name corresponding to a status, s.t. it can be printed.\ntemplate <typename Status, typename = void>\nstruct StatusName {\n  static std::string Get(Status status);\n};\n\n// Get the invalid unwrappable corresponding to an unwrappable. Eg. std::nullopt for std::optional.\ntemplate <typename Unwrappable, typename Status, typename = void>\nstruct InvalidUnwrappable {\n  static Unwrappable Get(Status status);\n};\n\nconstexpr Severity kDefaultSeverity = Severity::ERROR;\n\ntemplate <typename Result>\nconstexpr typename StatusType<Result>::Type GetStatus(const Result& result) {\n  return StatusValue<Result>::Get(result);\n}\n\ntemplate <typename Status>\nstd::string GetStatusName(Status status) {\n  return StatusName<Status>::Get(status);\n}\n\ntemplate <typename Unwrappable, typename Status>\nUnwrappable GetInvalidUnwrappable(Status status) {\n  return InvalidUnwrappable<Unwrappable, Status>::Get(status);\n}\n\n// ProxyFactory to create an ResultProxy. We create a separate class for these functions\n// because ResultProxy is templated and thus could not be used without explicitly\n// specifying the template.\n\ntemplate <typename Status>\nclass ResultProxy;\n\n// ProxyFactory to create an ResultProxy. We create a separate class for these functions\n// because ResultProxy is templated and thus could not be used without explicitly\n// specifying the template.\nclass ProxyFactory {\n public:\n  // Constructs the proxy from an unwrappable type. The unwrappable has to be in an error state. We\n  // do not check this because the macro should have already done this check. We use static\n  // methods instead of constructors to explicitly disallow construction from certain types in\n  // different situations.\n  template <\n      typename Unwrappable, typename = std::enable_if_t<IsUnwrappable<Unwrappable>::value>>\n  static ResultProxy<typename StatusType<Unwrappable>::Type> FromUnwrappable(\n      const Unwrappable& unwrappable) {\n    return ResultProxy(GetStatus(unwrappable));\n  }\n\n  // Constructs the proxy from a status type. The status has to be in an error state. We do not\n  // check this because the macro should have already done this check. We use static methods instead\n  // of constructors to explicitly disallow construction from certain types in different situations.\n  template <typename Result>\n  static ResultProxy<typename StatusType<Result>::Type> FromStatusOrUnwrappable(\n      const Result& result) {\n    return ResultProxy(GetStatus(result));\n  }\n};\n\n// A proxy class to allow implicit casting of one result type to another result type. Thus in a\n// function that returns a result type one can simply return this proxy and then it will implicitly\n// cast to the appropriate return type.\ntemplate <typename Status>\nclass ResultProxy {\n public:\n  // Casts the proxy to a status or unwrappable type. Note that this cast is not allowed to be\n  // explicit.\n  template <\n      typename T,\n      typename = std::enable_if_t<IsStatus<T>::value || IsUnwrappable<T>::value>>\n  constexpr operator T() const {\n    if constexpr (IsStatus<T>::value) {\n      return castToStatus<T>();\n    } else {\n      return castToInvalidUnwrappable<T>();\n    }\n  }\n\n private:\n  // Constructs the proxy from a status type. The status has to be in an error state.\n  // We do not check for this because we rely on the macro already having done that check.\n  constexpr explicit ResultProxy(Status error) : error_(error) {}\n\n  // Casts the proxy to a status type. If the error type is not equal to the proxy's error type,\n  // a default error is used.\n  template <typename OtherStatus>\n  constexpr OtherStatus castToStatus() const {\n    static_assert(IsStatus<OtherStatus>::value, \"OtherStatus has to be a status type.\");\n    if constexpr (std::is_same_v<OtherStatus, Status>) {\n      return error_;\n    } else {\n      return DefaultError<OtherStatus>();\n    }\n  }\n\n  // Casts the proxy to an invalid unwrappable type (eg. std::nullopt or nvidia::Unexpected).\n  template <typename Unwrappable>\n  constexpr Unwrappable castToInvalidUnwrappable() const {\n    static_assert(\n        IsUnwrappable<Unwrappable>::value, \"Unwrappable has to be a status type.\");\n    using OtherStatus = typename StatusType<Unwrappable>::Type;\n    return GetInvalidUnwrappable<Unwrappable>(castToStatus<OtherStatus>());\n  }\n\n  Status error_;\n  // TODO(tazhang): enable the following.\n  // static_assert(IsStatus<Status>::value, \"Status has to be a status type.\");\n\n  friend ProxyFactory;\n};\n\n// Helper function for the logging in the above macros. This version should be used when the user\n// also specifies the logging severity. The variadic arguments can be used to do string\n// interpolation in the custom_text variable.\ntemplate <typename ExpressionResult, typename... Args>\nvoid LogHelper(const char* file, int line, const ExpressionResult& expression_result,\n               const std::string& expression_string, Severity severity,\n               const std::string& custom_txt = \"\", Args... args) {\n  const auto error = GetStatus(expression_result);\n  const std::string text = \"Expression '\" + expression_string + \"' failed with error '\" +\n                           GetStatusName(error) + \"'. \" + custom_txt;\n\n  // GCC is not able to do format security validation when the string is coming from a variadic\n  // template, even if the string is originally a char* ignore this warning until a more recent GCC\n  // version fixes this behavior.\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wformat-security\"\n  ::nvidia::Log(file, line, severity, text.c_str(), args...);\n#pragma GCC diagnostic pop\n}\n\n// Overload of the LogHelper above. This version does not take the severity as an argument and used\n// the default severity instead.\ntemplate <typename ExpressionResult, typename... Args>\nvoid LogHelper(const char* file, int line, const ExpressionResult& expression_result,\n               const std::string& expression_string, const std::string& custom_text = \"\",\n               Args... args) {\n  LogHelper(file, line, expression_result, expression_string, kDefaultSeverity, custom_text,\n            args...);\n}\n\n}  // namespace nvidia::expected_macro\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/component.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CORE_COMPONENT_HPP\n#define NVIDIA_GXF_CORE_COMPONENT_HPP\n\n#include <set>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"common/assert.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/core/parameter.hpp\"\n#include \"gxf/core/parameter_storage.hpp\"\n#include \"gxf/core/registrar.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Components are parts of an entity and provide their functionality. The Component class is the\n// base class of all GXF components.\nclass Component {\n public:\n  virtual ~Component() = default;\n\n  Component(const Component& component) = delete;\n  Component(Component&& component) = delete;\n  Component& operator=(const Component& component) = delete;\n  Component& operator=(Component&& component) = delete;\n\n  // Used to register all parameters of the components. Do not use this function for other purposes\n  // as it might be called at anytime by the runtime.\n  //\n  // Example:\n  //   class Foo : public Component {\n  //    public:\n  //     gxf_result_t registerInterface(Registrar* registrar) override {\n  //       registrar->parameter(count_, \"count\", 1);\n  //     }\n  //     Parameter<int> count_;\n  //   };\n  virtual gxf_result_t registerInterface(Registrar* registrar) {\n    registrar_ = registrar;\n    parameter_storage_ = registrar_->getParameterStorage();\n    registrar->registerParameterlessComponent();\n    return GXF_SUCCESS;\n  }\n\n  // Use to start the lifetime of a component and should be used instead of the constructor.\n  // Called after all components of an entity are created. The order in which components within\n  // the same entity are initialized is undefined.\n  virtual gxf_result_t initialize() { return GXF_SUCCESS; }\n\n  // Use to end the lifetime of a component and should be used instead of the deconstructor.\n  // The order in which components within the same entity are deinitialized is undefined.\n  virtual gxf_result_t deinitialize() { return GXF_SUCCESS; }\n\n  gxf_context_t context() const noexcept { return context_; }\n  gxf_uid_t eid() const noexcept { return eid_; }\n  gxf_uid_t cid() const noexcept { return cid_; }\n  gxf_tid_t tid() const noexcept { return tid_; }\n  const char* type_name() const noexcept { return typename_; }\n\n  // The entity which owns this component\n  Entity entity() const noexcept {\n    // FIXME(v1) check that value exists\n    return Entity::Shared(context(), eid()).value();\n  }\n\n  // Gets the name of the component\n  const char* name() const noexcept {\n    const char* result;\n    const gxf_result_t code = GxfComponentName(context(), cid(), &result);\n    return (code == GXF_SUCCESS) ? result : \"\";\n  }\n\n  // This function shall only be called by GXF and is used to setup the component.\n  void internalSetup(gxf_context_t context, gxf_uid_t eid, gxf_uid_t cid, Registrar* registrar) {\n    context_ = context;\n    eid_ = eid;\n    cid_ = cid;\n    GxfComponentType(context_, cid_, &tid_);\n    GxfComponentTypeName(context_, tid_, &typename_);\n    if (registrar) {\n      registrar_ = registrar;\n      parameter_storage_ = registrar_->getParameterStorage();\n      parameter_registrar_ = registrar_->getParameterRegistrar();\n    }\n  }\n\n  // query value of component parameter \"key\"\n  template<typename T>\n  Expected<T> getParameter(const char* key) {\n    return parameter_storage_->get<T>(cid_, key);\n  }\n\n  // set the parameter \"key\" and of type T with value\n  template <typename T>\n  Expected<void> setParameter(const char* key, T value) {\n    if (!parameter_storage_) { return Unexpected{GXF_ARGUMENT_NULL}; }\n    return parameter_storage_->set<T>(cid_, key, std::move(value));\n  }\n\n  // set a parameter \"key\" of handle type with value\n  template <typename T>\n  Expected<void> setParameter(const char* key, Handle<T>& value) {\n    if (!parameter_storage_) { return Unexpected{GXF_ARGUMENT_NULL}; }\n    return parameter_storage_->setHandle(cid_, key, value->cid());\n  }\n\n  // set the parameter \"key\" with the value in yaml node\n  Expected<void> parseParameter(const char* key, const YAML::Node& node, std::string prefix = \"\") {\n    return parameter_storage_->parse(cid_, key, node, prefix);\n  }\n\n  // wrap the current value of the parameter \"key\" in a yaml node\n  Expected<YAML::Node> wrapParameter(const char* key) {\n    return parameter_storage_->wrap(cid_, key);\n  }\n\n  // query all parameters in the component of type T and return their ComponentParameterInfo struct\n  template<typename T>\n  Expected<std::vector<ParameterRegistrar::ComponentParameterInfo>> getParametersOfType() {\n    gxf_tid_t tid = GxfTidNull();\n    auto result = GxfComponentType(context_, cid_, &tid);\n    if (!isSuccessful(result)) {\n      GXF_LOG_ERROR(\"Failed to find component type for cid [%ld]\", cid_);\n      return Unexpected{result};\n    }\n\n    return parameter_registrar_->getParametersOfType<T>(tid);\n  }\n\n  // query componentParameterInfo of parameter \"key\"\n  Expected<ParameterRegistrar::ComponentParameterInfo> getParameterInfo(const char* key) {\n    gxf_tid_t tid = GxfTidNull();\n    auto result = GxfComponentType(context_, cid_, &tid);\n    if (!isSuccessful(result)) {\n      GXF_LOG_ERROR(\"Failed to find component type for cid [%ld]\", cid_);\n      return Unexpected{result};\n    }\n\n    ParameterRegistrar::ComponentParameterInfo info =\n        *(GXF_UNWRAP_OR_RETURN(parameter_registrar_->getComponentParameterInfoPtr(tid, key)));\n    return info;\n  }\n\n protected:\n  Component() = default;\n\n  gxf_context_t context_ = kNullContext;\n  gxf_uid_t eid_ = kNullUid;\n  gxf_uid_t cid_ = kNullUid;\n  gxf_tid_t tid_ = GxfTidNull();\n  const char* typename_ = \"UNKNOWN\";\n  Registrar* registrar_ = nullptr;\n  ParameterRegistrar* parameter_registrar_ = nullptr;\n  ParameterStorage* parameter_storage_ = nullptr;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/entity.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CORE_ENTITY_HPP_\n#define NVIDIA_GXF_CORE_ENTITY_HPP_\n\n#include <utility>\n\n#include \"common/assert.hpp\"\n#include \"common/fixed_vector.hpp\"\n#include \"common/type_name.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/expected_macro.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/core/handle.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief All GXF objects are entities. An entity owns multiple components which define the functionality\n * of the entity. Entities themselves are nothing more than a unique identifier. Entities created using the\n * C++ type is ref counted. The ref count is automatically decreased when the entity object is destructed or\n * goes out of scope.\n *\n */\nclass Entity {\n public:\n  /**\n   * @brief Creates a new entity using the given context and optionally set the given name. The\n   * caller of this api own's the object. The reference count is set to 1 and it is automatically\n   * reduced when this object is destroyed or goes out of scope.\n   *\n   * @param context A valid GXF context\n   * @param name Name for the entity object to be created\n   * @return Expected<Entity> Entity object wrapped in an expected type to capture error on failure\n   */\n  static Expected<Entity> New(gxf_context_t context, const char* name = nullptr) {\n    gxf_uid_t eid;\n    const GxfEntityCreateInfo info = {0};\n    void* item_ptr = nullptr;\n    gxf_result_t code = GxfCreateEntityAndGetItem(context, &info, &eid, &item_ptr);\n    if (code != GXF_SUCCESS) { return Unexpected{code}; }\n\n    return Shared(context, eid, item_ptr);\n  }\n\n  /**\n   * @brief Creates an entity handle based on an existing ID and takes ownership.\n   * Reference count is not increased.\n   *\n   * @param context A valid GXF context\n   * @param eid The unique object ID (UID) of a valid entity\n   * @param item_ptr An optional entity item pointer\n   * @return Expected<Entity> Entity object wrapped in an expected type to capture error on failure\n   */\n  static Expected<Entity> Own(gxf_context_t context, gxf_uid_t eid, void* item_ptr = nullptr) {\n    Entity result;\n    result.context_ = context;\n    result.eid_ = eid;\n    result.entity_item_ptr_ = item_ptr;\n    return result;\n  }\n\n  /**\n   * @brief Creates an entity handle based on an existing ID and shares ownership.\n   * Reference count is increased by one.\n   *\n   * @param context  A valid GXF context\n   * @param eid The unique object ID (UID) of a valid entity\n   * @param item_ptr An optional entity item pointer\n   * @return Expected<Entity> Entity object wrapped in an expected type to capture error on failure\n   */\n  static Expected<Entity> Shared(gxf_context_t context, gxf_uid_t eid, void* ptr = nullptr) {\n    Entity result;\n    result.context_ = context;\n    result.eid_ = eid;\n    result.entity_item_ptr_ = ptr;\n    const gxf_result_t code = GxfEntityRefCountInc(context, eid);\n    if (code != GXF_SUCCESS) {\n      return Unexpected{code};\n    } else {\n      return result;\n    }\n  }\n\n  /**\n   * @brief Construct a new entity object using default constructor.\n   * This is a null entity without a valid context of entity ID.\n   *\n   */\n  Entity() = default;\n\n  /**\n   * @brief Construct a new entity object by copying from another entity object.\n   *\n   * @param other A valid Entity object\n   */\n  Entity(const Entity& other) {\n    eid_ = other.eid();\n    context_ = other.context();\n    entity_item_ptr_ = other.entity_item_ptr();\n    if (eid_ != kNullUid) {\n      // FIXME(dweikersdorf) How do we deal with failure?\n      GxfEntityRefCountInc(context_, eid_);\n    }\n  }\n\n  /**\n   * @brief Construct a new entity object by moving the contents from an existing entity object.\n   *\n   * @param other A valid Entity object\n   */\n  Entity(Entity&& other) {\n    context_ = other.context_;\n    eid_ = other.eid_;\n    entity_item_ptr_ = other.entity_item_ptr_;\n    other.context_ = kNullContext;\n    other.eid_ = kNullUid;\n    other.entity_item_ptr_ = kNullUid;\n  }\n\n  /**\n   * @brief Operator overload for copy assignment operation\n   *\n   * @param other A valid Entity object\n   * @return Entity& The resulting copied entity object\n   */\n  Entity& operator=(const Entity& other) {\n    // In case other point to the same entity, nothing needs to be done.\n    if (eid_ == other.eid() && context_ == other.context()) {\n      return *this;\n    }\n    if (eid_ != kNullUid) {\n      release();\n    }\n    context_ = other.context();\n    eid_ = other.eid();\n    entity_item_ptr_ = other.entity_item_ptr();\n    if (eid_ != kNullUid) {\n      // FIXME(dweikersdorf) How do we deal with failure?\n      GxfEntityRefCountInc(context_, eid_);\n    }\n    return *this;\n  }\n\n  /**\n   * @brief Operator overload for move assignment operation\n   *\n   * @param other A valid Entity object\n   * @return Entity& The resulting moved entity object\n   */\n  Entity& operator=(Entity&& other) {\n    // In case other is this, then nothing should be done.\n    if (&other == this) {\n      return *this;\n    }\n    if (eid_ != kNullUid) {\n      release();\n    }\n    context_ = other.context_;\n    eid_ = other.eid_;\n    entity_item_ptr_ = other.entity_item_ptr_;\n    other.entity_item_ptr_ = nullptr;\n    other.context_ = kNullContext;\n    other.eid_ = kNullUid;\n    return *this;\n  }\n\n  /**\n   * @brief Destroy the Entity object. Reduces the reference count by 1.\n   *\n   */\n  ~Entity() {\n    if (eid_ != kNullUid) {\n      release();\n    }\n  }\n\n  /**\n   * @brief Activates the entity. See GxfEntityActivate in gxf.h\n   *\n   * @return Expected<void>\n   */\n  Expected<void> activate() {\n    return ExpectedOrCode(GxfEntityActivate(context(), eid()));\n  }\n\n  /**\n   * @brief Deactivates the entity. See GxfEntityDectivate in gxf.h\n   *\n   * @return Expected<void>\n   */\n  Expected<void> deactivate() {\n    return ExpectedOrCode(GxfEntityDeactivate(context(), eid()));\n  }\n\n  /**\n   * @brief Clone an entity from an existing entity object. The returned entity shares the\n   * ownership with the entity being cloned from. Reference count is increased by one.\n   *\n   * @return Expected<Entity>\n   */\n  Expected<Entity> clone() const {\n    return Shared(context(), eid(), entity_item_ptr());\n  }\n\n  /**\n   * @brief Returns the GXF context of the entity.\n   *\n   * @return gxf_context_t\n   */\n  gxf_context_t context() const { return context_; }\n\n  /**\n   * @brief Returns the  unique object ID (UID) of the entity\n   *\n   * @return gxf_uid_t\n   */\n  gxf_uid_t eid() const { return eid_; }\n\n  /**\n   * @brief Checks if an entity object is null (empty) or not\n   *\n   * @return true If entity is not null (empty) entity object\n   * @return false If the entity is a valid object\n   */\n  bool is_null() const { return eid_ == kNullUid; }\n\n  /**\n   * @brief The name of the entity or empty string if no name has been given to the entity.\n   *\n   * @return const char* pointer to name of the entity\n   */\n  const char* name() const {\n    const char* ptr;\n    const gxf_result_t result = GxfEntityGetName(context_, eid_, &ptr);\n    return (result == GXF_SUCCESS) ? ptr : \"\";\n  }\n\n  /**\n   * @brief Adds a component with given type ID.\n   *\n   * @param tid A valid type ID of a registered component\n   * @param name Name to be given to the newly created component instance\n   * @return Expected<UntypedHandle> An untyped handle to component or error on failure\n   */\n  Expected<UntypedHandle> add(gxf_tid_t tid, const char* name = nullptr) {\n    gxf_uid_t cid;\n    void* comp_ptr = nullptr;\n    GXF_RETURN_IF_ERROR(check_entity_item_ptr());\n    const auto result = GxfComponentAddAndGetPtr(context(), entity_item_ptr(), tid,\n                                                 name, &cid, &comp_ptr);\n    if (result != GXF_SUCCESS) {\n      return Unexpected{result};\n    }\n    return UntypedHandle::Create(context(), cid, tid, comp_ptr);\n  }\n\n  /**\n   * @brief Removes a component with given type ID.\n   *\n   * @param tid A valid type ID of a registered component\n   * @param name Name to be given to the newly created component instance\n   * @return Expected<void> or error on failure\n   */\n  Expected<void> remove(gxf_tid_t tid, const char* name = nullptr) {\n    // TODO(pshekdar): Add new API GxfComponentRemoveCpp\n    const auto result = GxfComponentRemove(context(), eid(), tid, name);\n    return ExpectedOrCode(result);\n  }\n\n  /**\n   * @brief Adds a component with given template component type\n   *\n   * @tparam T A valid template type of a registered component\n   * @param name Name to be given to the newly created component instance\n   * @return Expected<Handle<T>> Typed Handle to the component instance\n   */\n  template <typename T>\n  Expected<Handle<T>> add(const char* name = nullptr) {\n    gxf_tid_t tid;\n    const auto result_1 = GxfComponentTypeId(context(), TypenameAsString<T>(), &tid);\n    if (result_1 != GXF_SUCCESS) {\n      return Unexpected{result_1};\n    }\n    gxf_uid_t cid;\n    void* comp_ptr = nullptr;\n    GXF_RETURN_IF_ERROR(check_entity_item_ptr());\n    const auto result_2 = GxfComponentAddAndGetPtr(context(), entity_item_ptr(), tid, name, &cid,\n                                                   &comp_ptr);\n    if (result_2 != GXF_SUCCESS) {\n      return Unexpected{result_2};\n    }\n    return Handle<T>::Create(context(), cid, tid, comp_ptr);\n  }\n\n  /**\n   * @brief Removes a component with given template component type\n   *\n   * @tparam T A valid template type of a registered component\n   * @param name Name of the component\n   * @return Expected<void> or error on failure\n   */\n  template <typename T>\n  Expected<void> remove(const char* name = nullptr) {\n    gxf_tid_t tid;\n    // TODO(pshekdar): Add new API GxfComponentRemoveCpp\n    const auto result_1 = GxfComponentTypeId(context(), TypenameAsString<T>(), &tid);\n    if (result_1 != GXF_SUCCESS) {\n      return Unexpected{result_1};\n    }\n    const auto result_2 = GxfComponentRemove(context(), eid(), tid, name);\n    return ExpectedOrCode(result_2);\n  }\n\n  /**\n   * @brief Removes a component with given uid\n   *\n   * @tparam cid A valid uid of the component\n   * @return Expected<void> or error on failure\n   */\n  Expected<void> remove(gxf_uid_t& cid) {\n    const auto result = GxfComponentRemoveWithUID(context(), cid);\n    return ExpectedOrCode(result);\n  }\n\n  /**\n   * @brief Gets a component by type ID. Asserts if no such component.\n   *\n   * @param tid A valid type ID of a registered component\n   * @param name Name of the component to lookup\n   * @return Expected<UntypedHandle> An untyped handle to component or error on failure\n   */\n  Expected<UntypedHandle> get(gxf_tid_t tid, const char* name = nullptr) const {\n    gxf_uid_t cid;\n    void* comp_ptr = nullptr;\n    GXF_RETURN_IF_ERROR(check_entity_item_ptr());\n    const auto result = GxfComponentFindAndGetPtr(context(), eid(),\n                                                  static_cast<void*>(entity_item_ptr()),\n                                                  tid, name, nullptr, &cid, &comp_ptr);\n    if (result != GXF_SUCCESS) {\n      return Unexpected{result};\n    }\n    return UntypedHandle::Create(context(), cid, tid, comp_ptr);\n  }\n\n  /**\n   * @brief Gets a component by type. Asserts if no such component.\n   *\n   * @tparam T A valid template type of a registered component\n   * @param name Name of the component to lookup\n   * @return Expected<Handle<T>> Typed Handle to the component instance or error on failure\n   */\n  template <typename T>\n  Expected<Handle<T>> get(const char* name = nullptr) const {\n    gxf_tid_t tid;\n    GXF_RETURN_IF_ERROR(check_entity_item_ptr());\n    const auto result_1 = GxfComponentTypeId(context(), TypenameAsString<T>(), &tid);\n    if (result_1 != GXF_SUCCESS) {\n      return Unexpected{result_1};\n    }\n    gxf_uid_t cid;\n    void* comp_ptr = nullptr;\n    const auto result_2 = GxfComponentFindAndGetPtr(context(), eid(),\n                                                    static_cast<void*>(entity_item_ptr()),\n                                                    tid, name, nullptr, &cid, &comp_ptr);\n    if (result_2 != GXF_SUCCESS) {\n      return Unexpected{result_2};\n    }\n    return Handle<T>::Create(context(), cid, tid, comp_ptr);\n  }\n\n  /**\n   * @brief Finds all components in an entity. A fixed-size vector of untyped handles of all the\n   * components are returned.\n   *\n   * @tparam N Capacity of the FixedVector\n   * @return Expected<FixedVector<UntypedHandle, N>> A fixed-size vector of untyped handles of all\n   * the components allocated on stack\n   */\n  template <size_t N = kMaxComponents>\n  Expected<FixedVector<UntypedHandle, N>> findAll() const {\n    const gxf_context_t c_context = context();\n    const gxf_uid_t c_eid = eid();\n    gxf_uid_t cids[N];\n    uint64_t num_cids = N;\n    const gxf_result_t code = GxfComponentFindAll(c_context, c_eid, &num_cids, cids);\n    if (code != GXF_SUCCESS) {\n      return Unexpected{code};\n    }\n    /* coverity[stack_use_local_overflow] */\n    FixedVector<UntypedHandle, N> components;\n    for (size_t i = 0; i < num_cids; i++) {\n      const auto result = UntypedHandle::Create(c_context, cids[i])\n          .map([&](UntypedHandle component) {\n            return components.push_back(component)\n                .substitute_error(GXF_EXCEEDING_PREALLOCATED_SIZE);\n          });\n      if (!result) {\n        return ForwardError(result);\n      }\n    }\n    return components;\n  }\n\n  /**\n   * @brief Finds all components of given type.\n   *\n   * @tparam T A valid template type of a registered component\n   * @tparam N Capacity of the FixedVector\n   * @return Expected<FixedVector<Handle<T>, N>> A fixed-size vector of typed handles of given\n   * component type allocated on stack\n   */\n  template <typename T, size_t N = kMaxComponents>\n  Expected<FixedVector<Handle<T>, N>> findAll() const {\n    const gxf_context_t c_context = context();\n    const gxf_uid_t c_eid = eid();\n    gxf_tid_t tid;\n    const gxf_result_t code = GxfComponentTypeId(c_context, TypenameAsString<T>(), &tid);\n    if (code != GXF_SUCCESS) {\n      return Unexpected{code};\n    }\n    /* coverity[stack_use_local_overflow] */\n    FixedVector<Handle<T>, N> components;\n    for (int offset = 0; static_cast<size_t>(offset) < N; offset++) {\n      gxf_uid_t cid;\n      const gxf_result_t code = GxfComponentFind(c_context, c_eid, tid, nullptr, &offset, &cid);\n      if (code != GXF_SUCCESS) {\n        break;\n      }\n      const auto result = Handle<T>::Create(c_context, cid)\n          .map([&](Handle<T> component) {\n            return components.push_back(component)\n                .substitute_error(GXF_EXCEEDING_PREALLOCATED_SIZE);\n          });\n      if (!result) {\n        return ForwardError(result);\n      }\n    }\n    return components;\n  }\n\n  /**\n   * @brief Finds all components in an entity. A fixed-size vector of untyped handles of all the\n   * components are returned.\n   *\n   * @tparam N Capacity of the FixedVector\n   * @return Expected<FixedVector<UntypedHandle>> A fixed-size vector of untyped handles of all\n   * the components allocated on heap\n   */\n  template <size_t N = kMaxComponents>\n  Expected<FixedVector<UntypedHandle>> findAllHeap() const {\n    const gxf_context_t c_context = context();\n    const gxf_uid_t c_eid = eid();\n    gxf_uid_t cids[N];\n    uint64_t num_cids = N;\n    const gxf_result_t code = GxfComponentFindAll(c_context, c_eid, &num_cids, cids);\n    if (code != GXF_SUCCESS) {\n      return Unexpected{code};\n    }\n    FixedVector<UntypedHandle> components;\n    components.reserve(num_cids);\n    for (size_t i = 0; i < num_cids; i++) {\n      const auto result = UntypedHandle::Create(c_context, cids[i])\n          .map([&](UntypedHandle component) {\n            return components.push_back(component)\n                .substitute_error(GXF_EXCEEDING_PREALLOCATED_SIZE);\n          });\n      if (!result) {\n        return ForwardError(result);\n      }\n    }\n    return components;\n  }\n\n  /**\n   * @brief Finds all components of given type.\n   *\n   * @tparam T A valid template type of a registered component\n   * @tparam N Capacity of the FixedVector\n   * @return Expected<FixedVector<Handle<T>>> A fixed-size vector of typed handles of given\n   *  component type allocated on heap\n   */\n  template <typename T, size_t N = kMaxComponents>\n  Expected<FixedVector<Handle<T>>> findAllHeap() const {\n    const gxf_context_t c_context = context();\n    const gxf_uid_t c_eid = eid();\n    gxf_tid_t tid;\n    const gxf_result_t code = GxfComponentTypeId(c_context, TypenameAsString<T>(), &tid);\n    if (code != GXF_SUCCESS) {\n      return Unexpected{code};\n    }\n    FixedVector<Handle<T>> components;\n    components.reserve(N);\n    for (int offset = 0; static_cast<size_t>(offset) < N; offset++) {\n      gxf_uid_t cid;\n      const gxf_result_t code = GxfComponentFind(c_context, c_eid, tid, nullptr, &offset, &cid);\n      if (code != GXF_SUCCESS) {\n        break;\n      }\n      const auto result = Handle<T>::Create(c_context, cid)\n          .map([&](Handle<T> component) {\n            return components.push_back(component)\n                .substitute_error(GXF_EXCEEDING_PREALLOCATED_SIZE);\n          });\n      if (!result) {\n        return ForwardError(result);\n      }\n    }\n    return components;\n  }\n\n  /**\n   * @brief Returns the pointer to Entity Item for the entity\n   *\n   * @return void * entity item ptr\n   */\n  void* entity_item_ptr() const {\n    check_entity_item_ptr();\n    return entity_item_ptr_;\n  }\n\n private:\n  /**\n   * @brief Set the entity item ptr if its not set already\n   *\n   * @return Expected<void> Success or error code\n   */\n  Expected<void> check_entity_item_ptr() const {\n    if (entity_item_ptr_ != nullptr) {\n      return Success;\n    }\n    auto code = GxfEntityGetItemPtr(context(), eid(),\n                                    reinterpret_cast<void**>(&(entity_item_ptr_)));\n    return code == GXF_SUCCESS ? Success : Unexpected(code);\n  }\n\n  void release() {\n    GxfEntityRefCountDec(context_, eid_);  // TODO(v2) We should use the error code, but we can't\n                                           //          do anything about it..\n    eid_ = kNullUid;\n    entity_item_ptr_ = nullptr;\n  }\n\n  gxf_context_t context_ = kNullContext;\n  gxf_uid_t eid_ = kNullUid;\n  mutable void* entity_item_ptr_ = nullptr;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CORE_ENTITY_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/expected.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CORE_EXPECTED_HPP\n#define NVIDIA_GXF_CORE_EXPECTED_HPP\n\n#include <type_traits>\n#include <utility>\n\n#include \"common/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Expected type for GXF which uses gxf_result_t as error code.\ntemplate <typename T>\nusing Expected = Expected<T, gxf_result_t>;\n\n// Unexpected type for GXF which uses gxf_result_t as error code.\nusing Unexpected = Unexpected<gxf_result_t>;\n\n// Special value which can be used instead of {} to return from a function which returns an\n// Expected<void>.\nconst Expected<void> Success{};\n\n// Extracts the error code as an unexpected.\ntemplate <typename T>\nUnexpected ForwardError(const Expected<T>& expected) {\n  return Unexpected{expected.error()};\n}\n\n// Extracts the error code as an unexpected.\ntemplate <typename T>\nUnexpected ForwardError(Expected<T>&& expected) {\n  return Unexpected{std::move(expected.error())};\n}\n\n// Interprets an expected as a result code. Returns GXF_SUCESS if the result has a value and the\n// result's error code otherwise.\ntemplate <typename T>\ngxf_result_t ToResultCode(const Expected<T>& result) {\n  return result ? GXF_SUCCESS : result.error();\n}\n\n// If the result code is GXF_SUCCESS the function returns Success, otherwise it returns an\n// unexpected with the given error code.\ninline\nExpected<void> ExpectedOrCode(gxf_result_t code) {\n  if (code == GXF_SUCCESS) {\n    return Success;\n  } else {\n    return Unexpected{code};\n  }\n}\n\n// If the result code is GXF_SUCCESS the function returns the given value, otherwise it returns an\n// unexpected with the given error code.\ntemplate <typename T>\nExpected<std::remove_cv_t<std::remove_reference_t<T>>>\nExpectedOrCode(gxf_result_t code, T&& value) {\n  if (code == GXF_SUCCESS) {\n    return std::forward<T>(value);\n  } else {\n    return Unexpected{code};\n  }\n}\n\ntemplate <typename S, typename T>\nExpected<std::remove_cv_t<std::remove_reference_t<T>>>\nExpectedOrError(const Expected<S>& code, T&& value) {\n  if (code) {\n    return std::forward<T>(value);\n  } else {\n    return ForwardError(code);\n  }\n}\n\ninline\ngxf_result_t AccumulateError(gxf_result_t previous, gxf_result_t current) {\n  return current != GXF_SUCCESS ? current : previous;\n}\n\ninline\nExpected<void> AccumulateError(Expected<void> previous, Expected<void> current) {\n  return !current ? current : previous;\n}\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/expected_macro.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma once\n\n#include <string>\n\n#include \"gxf/core/common_expected_macro.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"magic_enum.hpp\"  // NOLINT(build/include)\n\n//////////////////////////////////////////\n// Configuration for using bool as status:\n//////////////////////////////////////////\nnamespace nvidia::expected_macro {\n\n// This section contains the configuration to use the expected macro with\n// different return types like bool, int, std::optional (and in the future std::expected).\ntemplate <>\nstruct IsStatus<bool> : std::true_type {};\n\ntemplate <>\nconstexpr bool DefaultError<bool>() {\n  return false;\n}\n\ntemplate <>\nstruct StatusName<bool> {\n  static std::string Get(bool status) { return status ? \"Success\" : \"Failure\"; }\n};\n\n/////////////////////////////////////////\n// Configuration for using int as status:\n/////////////////////////////////////////\ntemplate <>\nstruct IsStatus<int> : std::true_type {};\n\ntemplate <>\nconstexpr bool IsValid<int>(const int& status) {\n  return status == 0;\n}\n\ntemplate <>\nconstexpr int DefaultError<int>() {\n  return 1;\n}\n\ntemplate <>\nstruct StatusName<int> {\n  static std::string Get(int status) { return std::to_string(status); }\n};\n\n//////////////////////////////////////////\n// Configuration for using enum as status:\n//////////////////////////////////////////\ntemplate <typename EnumStatus>\nstruct StatusName<EnumStatus, typename std::enable_if_t<std::is_enum_v<EnumStatus>>> {\n  static std::string Get(EnumStatus status) { return std::string(magic_enum::enum_name(status)); }\n};\n\n////////////////////////////////////////////////////////\n// Configuration for using std::optional as unwrappable:\n////////////////////////////////////////////////////////\ntemplate <typename Value>\nstruct IsUnwrappable<std::optional<Value>> : std::true_type {};\n\ntemplate <typename Value>\nstruct StatusType<std::optional<Value>> {\n  using Type = bool;\n};\n\ntemplate <typename Value>\nstruct StatusValue<std::optional<Value>> {\n  static constexpr bool Get(const std::optional<Value>& optional) { return optional.has_value(); }\n};\n\ntemplate <typename Value>\nstruct InvalidUnwrappable<std::optional<Value>, bool> {\n  static std::optional<Value> Get(bool status) { return std::nullopt; }\n};\n\n///////////////////////////////////////////////////////////\n// Configuration for using nvidia::Expected as unwrappable:\n///////////////////////////////////////////////////////////\ntemplate <typename Value, typename Status>\nstruct IsUnwrappable<::nvidia::Expected<Value, Status>> : std::true_type {};\n\ntemplate <typename Value, typename Status>\nstruct StatusType<::nvidia::Expected<Value, Status>> {\n  using Type = Status;\n};\n\ntemplate <typename Value, typename Status>\nstruct StatusValue<::nvidia::Expected<Value, Status>> {\n  static constexpr Status Get(const ::nvidia::Expected<Value, Status>& expected) {\n    return expected.error();\n  }\n};\n\ntemplate <typename Value, typename Status>\nstruct InvalidUnwrappable<::nvidia::Expected<Value, Status>, Status> {\n  static ::nvidia::Expected<Value, Status> Get(Status status) {\n    return ::nvidia::Unexpected<Status>(status);\n  }\n};\n\n// This customizes the expected macro, s.t. it can be used with gxf_result_t.\ntemplate <>\nstruct IsStatus<gxf_result_t> : std::true_type {};\n\ntemplate <>\nconstexpr bool IsValid<gxf_result_t>(const gxf_result_t& status) {\n  return status == GXF_SUCCESS;\n}\n\ntemplate <>\nconstexpr gxf_result_t DefaultError<gxf_result_t>() {\n  return GXF_FAILURE;\n}\n\ntemplate <>\nstruct StatusName<gxf_result_t> {\n  static std::string Get(gxf_result_t status) { return GxfResultStr(status); }\n};\n\n// For back-compatibility we define an alias to the original name of the macro when it was gxf\n// specific.\n#define GXF_RETURN_IF_ERROR RETURN_IF_ERROR\n#define GXF_UNWRAP_OR_RETURN UNWRAP_OR_RETURN\n}  // namespace nvidia::expected_macro\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/filepath.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CORE_FILEPATH_HPP_\n#define NVIDIA_GXF_CORE_FILEPATH_HPP_\n\n#include <string>\n#include \"yaml-cpp/yaml.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n\n// Specialization of ParameterParser for a file parameter type to identify parameters that are file\n// system paths\nstruct FilePath : public std::string {\n  FilePath() : std::string() {}\n\n  FilePath(const char* s) : std::string(s) {}\n\n  FilePath(const std::string& str) : std::string(str) {}\n};\n\n\n}  // namespace gxf\n}  // namespace nvidia\n\ntemplate<>\nstruct YAML::convert<nvidia::gxf::FilePath> {\n    static Node encode(const nvidia::gxf::FilePath& data) {\n      Node node = YAML::Load(data);\n      return node;\n    }\n\n    static bool decode(const Node& node, nvidia::gxf::FilePath& data) {\n      if (!node.IsScalar()) {\n        return false;\n      }\n      data = node.as<std::string>();\n      return true;\n    }\n};\n\n\n\n#endif  // NVIDIA_GXF_CORE_FILEPATH_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/gxf.h",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CORE_GXF_H_\n#define NVIDIA_GXF_CORE_GXF_H_\n\n#ifdef __cplusplus\n#include <cstdint>\n#include <fstream>\n#else\n#include <stdint.h>\n#endif\n\n#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n// --  Result type  --------------------------------------------------------------------------------\n\n/// @brief GXF error and result codes which are used by almost all GXF functions.\ntypedef enum {\n  GXF_SUCCESS = 0,\n  GXF_FAILURE = 1,\n\n  GXF_NOT_IMPLEMENTED,\n  GXF_FILE_NOT_FOUND,\n  GXF_INVALID_ENUM,\n  GXF_NULL_POINTER,\n  GXF_UNINITIALIZED_VALUE,\n\n  GXF_ARGUMENT_NULL,\n  GXF_ARGUMENT_OUT_OF_RANGE,\n  GXF_ARGUMENT_INVALID,\n\n  GXF_OUT_OF_MEMORY,\n  GXF_MEMORY_INVALID_STORAGE_MODE,\n\n  GXF_CONTEXT_INVALID,\n\n  GXF_EXTENSION_NOT_FOUND,\n  GXF_EXTENSION_FILE_NOT_FOUND,\n  GXF_EXTENSION_NO_FACTORY,\n\n  GXF_FACTORY_TOO_MANY_COMPONENTS,\n  GXF_FACTORY_DUPLICATE_TID,\n  GXF_FACTORY_UNKNOWN_TID,\n  GXF_FACTORY_ABSTRACT_CLASS,\n  GXF_FACTORY_UNKNOWN_CLASS_NAME,\n  GXF_FACTORY_INVALID_INFO,\n  GXF_FACTORY_INCOMPATIBLE,\n\n  GXF_ENTITY_NOT_FOUND,\n  GXF_ENTITY_NAME_EXCEEDS_LIMIT,\n  GXF_ENTITY_COMPONENT_NOT_FOUND,\n  GXF_ENTITY_COMPONENT_NAME_EXCEEDS_LIMIT,\n  GXF_ENTITY_CAN_NOT_ADD_COMPONENT_AFTER_INITIALIZATION,\n  GXF_ENTITY_CAN_NOT_REMOVE_COMPONENT_AFTER_INITIALIZATION,\n  GXF_ENTITY_MAX_COMPONENTS_LIMIT_EXCEEDED,\n\n  GXF_PARAMETER_NOT_FOUND,\n  GXF_PARAMETER_ALREADY_REGISTERED,\n  GXF_PARAMETER_INVALID_TYPE,\n  GXF_PARAMETER_OUT_OF_RANGE,\n  GXF_PARAMETER_NOT_INITIALIZED,\n  GXF_PARAMETER_CAN_NOT_MODIFY_CONSTANT,\n  GXF_PARAMETER_PARSER_ERROR,\n  GXF_PARAMETER_NOT_NUMERIC,\n  GXF_PARAMETER_MANDATORY_NOT_SET,\n\n  GXF_CONTRACT_INVALID_SEQUENCE,\n  GXF_CONTRACT_PARAMETER_NOT_SET,\n  GXF_CONTRACT_MESSAGE_NOT_AVAILABLE,\n\n  GXF_INVALID_LIFECYCLE_STAGE,\n  GXF_INVALID_EXECUTION_SEQUENCE,\n  GXF_REF_COUNT_NEGATIVE,\n  GXF_RESULT_ARRAY_TOO_SMALL,\n\n  GXF_INVALID_DATA_FORMAT,\n\n  GXF_EXCEEDING_PREALLOCATED_SIZE,\n\n  GXF_QUERY_NOT_ENOUGH_CAPACITY,\n  GXF_QUERY_NOT_APPLICABLE,\n  GXF_QUERY_NOT_FOUND,\n\n  GXF_NOT_FINISHED,\n\n  GXF_HTTP_GET_FAILURE,\n  GXF_HTTP_POST_FAILURE,\n\n  GXF_ENTITY_GROUP_NOT_FOUND,\n  GXF_RESOURCE_NOT_INITIALIZED,\n  GXF_RESOURCE_NOT_FOUND,\n\n  GXF_CONNECTION_BROKEN,\n  GXF_CONNECTION_ATTEMPTS_EXCEEDED,\n\n  GXF_IPC_CONNECTION_FAILURE,\n  GXF_IPC_CALL_FAILURE,\n  GXF_IPC_SERVICE_NOT_FOUND,\n} gxf_result_t;\n\n/// @brief Checks if a result code is GXF_SUCCESS or not\n///\n/// @param result A GXF result code\n/// @return A boolean value indicating if the result code is GXF_SUCCESS\nbool isSuccessful(gxf_result_t result);\n\n/// @brief Gets a string describing an GXF error code.\n///\n/// The caller does not get ownership of the return C string and must not delete it.\n///\n/// @param result A GXF error code\n/// @return A pointer to a C string with the error code description.\nconst char* GxfResultStr(gxf_result_t result);\n\n// --  Unique identifiers  -------------------------------------------------------------------------\n\n/// @brief Type of unique GXF object identifiers (UID/uid)\n///\n/// Uids are used to reference entities and components throughout the GXF API.\ntypedef int64_t gxf_uid_t;\n\n/// @brief A GXF UID which can be used to indicate an invalid or unused GXF UID.\n#define kNullUid 0L\n/// @brief A GXF UID which can be used to indicate an unspecified component during\n/// graph load operation. This component should be updated in a subsequent\n/// graph/parameters file. Failing to do so will result in an error during graph activation\n#define kUnspecifiedUid -1L\n\n/// @brief Type of unique GXF type identifier (TID/tid)\n///\n/// Tids are used to uniquely identify the type of a component, instead of for example using\n/// a string with the type name.\ntypedef struct {\n  uint64_t hash1;\n  uint64_t hash2;\n} gxf_tid_t;\n\n/// @brief Gives the null value of a GXF tid\n///\n/// The null tid is used to mark a tid as invalid or to indicate that a tid is not set.\n///\n/// @return The null tid\ninline gxf_tid_t GxfTidNull() {\n  gxf_tid_t result;\n  result.hash1 = 0UL;\n  result.hash2 = 0UL;\n  return result;\n}\n\n/// @brief Determines if a GXF tid is null\n///\n/// @param tid A GXF tid\n/// @return Returns 1 if 'tid' is the null tid; and 0 otherwise.\ninline uint32_t GxfTidIsNull(gxf_tid_t tid) {\n  return (tid.hash1 == 0UL && tid.hash2 == 0UL) ? 1 : 0;\n}\n\n// --  Context  ------------------------------------------------------------------------------------\n\n/// @brief Type for context handle\ntypedef void* gxf_context_t;\n\n/// @brief An invalid context\n#define kNullContext nullptr\n\n/// @brief GXF Core Version\n#define kGxfCoreVersion \"5.1.0\"\n\n/// @brief Creates a new GXF context\n///\n/// A GXF context is required for all almost all GXF operations. The context must be destroyed with\n/// 'GxfContextDestroy'. Multiple contexts can be created in the same process, however they can not\n/// communicate with each other.\n///\n/// @param context The new GXF context is written to the given pointer.\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfContextCreate(gxf_context_t* context);\n\n/// @brief Creates a new runtime context from shared context.\n///\n/// A shared runtime context is used for sharing entities between graphs running\n/// within the same process.\n///\n/// @param shared A valid GXF shared context.\n/// @param context The new GXF context is written to the given pointer.\ngxf_result_t GxfContextCreateShared(gxf_context_t shared, gxf_context_t* context);\n\n/// @brief Gets a GXF shared context.\n///\n/// A GXF context is required for all almost all GXF operations. The context\n/// must be destroyed with 'GxfContextDestroy'. A shared runtime context is used\n/// for sharing entities between graphs running within the same process.\n///\n/// @param shared The shared GXF context is written to this pointer.\n/// @param context A valid GXF context\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the\n/// GXF error codes.\ngxf_result_t GxfGetSharedContext(gxf_context_t context, gxf_context_t* shared);\n\n/// @brief Destroys a GXF context\n///\n/// Every GXF context must be destroyed by calling this function. The context must have been\n/// previously created with 'GxfContextCreate'. This will also destroy all entities and components\n/// which were created as part of the context.\n///\n/// @param context A valid GXF context.\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfContextDestroy(gxf_context_t context);\n\n// --  Extensions  ---------------------------------------------------------------------------------\n\n/// @brief Maximum number of extensions in a context\n#define kMaxExtensions 1024\n\n/// @brief Loads an extension from a pointer to the Extension object.\n///\n/// `extension_ptr` must be a pointer to a valid object with `Extension` type.\n/// @param context A valid GXF context\n/// @param extension_ptr A pointer to Extension\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfLoadExtensionFromPointer(gxf_context_t context, void* extension_ptr);\n\n/// @brief Registers a component with a GXF extension\n///\n/// A GXF extension need to register all of its components in the extension factory function. For\n/// convenience the helper macros in gxf/std/extension_factory_helper.hpp can be used.\n///\n/// The developer must choose a unique GXF tid with two random 64-bit integers. The developer\n/// must ensure that every GXF component has a unique tid. The name of the component must be\n/// the fully qualified C++ type name of the component. A component may only have a single base\n/// class and that base class must be specified with its fully qualified C++ type name as the\n/// parameter 'base_name'.\n///\n/// @see gxf/std/extension_factory_helper.hpp\n/// @see core/type_name.hpp\n///\n/// @param context A valid GXF context\n/// @param tid The chosen GXF tid\n/// @param name The type name of the component\n/// @param base_name The type name of the base class of the component\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfRegisterComponent(gxf_context_t context, gxf_tid_t tid, const char* name,\n                                  const char* base_name);\n\n/// @brief Registers a new component from an extension during runtime\n///\n/// Once an extension is loaded any newly added components to that extension can be registered\n/// with the context using this function\n///\n/// @param context A valid GXF context\n/// @param component_tid The valid GXF tid of a unregistered new component\n/// @param extension_tid The valid GXF tid of an extension which has already been loaded\n\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfRegisterComponentInExtension(gxf_context_t context, gxf_tid_t component_tid,\n                                             gxf_tid_t extension_tid);\n\n// --  Entities  -----------------------------------------------------------------------------------\n\n/// @brief Maximum number of entities in a context\n#define kMaxEntities 1024\n\n/// @brief various lifecycle states of an entity\ntypedef enum {\n  GXF_ENTITY_STATUS_NOT_STARTED = 0,\n  GXF_ENTITY_STATUS_START_PENDING,\n  GXF_ENTITY_STATUS_STARTED,\n  GXF_ENTITY_STATUS_TICK_PENDING,\n  GXF_ENTITY_STATUS_TICKING,\n  GXF_ENTITY_STATUS_IDLE,\n  GXF_ENTITY_STATUS_STOP_PENDING,\n  GXF_ENTITY_MAX\n} gxf_entity_status_t;\n\n/// @brief Gets a string describing an GXF entity status.\n///\n/// The caller does not get ownership of the return C string and must not delete it.\n///\n/// @param status A GXF entity status\n/// @return A pointer to a C string with the entity status description.\nconst char* GxfEntityStatusStr(gxf_entity_status_t status);\n\n/// @brief Used by behavior parent codelet in Behavior Tree denoting the result\n/// of codelet::tick()\n/// - GXF_BEHAVIOR_INIT is for codelet that have not yet started running.\n/// - GXF_BEHAVIOR_SUCCESS is for codelet that terminates with success after\n/// ticking\n/// - GXF_BEHAVIOR_RUNNING is for codelet that needs multiple ticks to complete\n/// - GXF_BEHAVIOR_FAILURE is for codelet that terminates with failure after\n/// ticking\n/// - GXF_BEHAVIOR_UNKNOWN is for non-behavior-tree codelet because we don't\n/// care about the behavior status returned by controller if it is not a BT\n/// codelet\ntypedef enum {\n  GXF_BEHAVIOR_INIT = 0,\n  GXF_BEHAVIOR_SUCCESS = 1,\n  GXF_BEHAVIOR_RUNNING = 2,\n  GXF_BEHAVIOR_FAILURE = 3,\n  GXF_BEHAVIOR_UNKNOWN = 4,\n} entity_state_t;\n\n/// @brief Activates a previously created and inactive entity\n///\n/// Activating an entity generally marks the official start of its lifetime and has multiple\n/// implications:\n/// - If mandatory parameters, i.e. parameter which do not have the flag \"optional\", are not set\n///   the operation will fail.\n/// - All components on the entity are initialized.\n/// - All codelets on the entity are scheduled for execution. The scheduler will start calling\n///   start, tick and stop functions as specified by scheduling terms.\n/// - After activation trying to change a dynamic parameters will result in a failure.\n/// - Adding or removing components of an entity after activation will result in a failure.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity\n/// @return GXF error code\ngxf_result_t GxfEntityActivate(gxf_context_t context, gxf_uid_t eid);\n\n/// @brief Deactivates a previously activated entity\n///\n/// Deactivating an entity generally marks the official end of its lifetime and has multiple\n/// implications:\n/// - All codelets are removed from the schedule. Already running entities are run to completion.\n/// - All components on the entity are deinitialized.\n/// - Components can be added or removed again once the entity was deactivated.\n/// - Mandatory and non-dynamic parameters can be changed again.\n///\n/// Note: In case that the entity is currently executing this function will wait and block until\n///       the current execution is finished.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity\n/// @return GXF error code\ngxf_result_t GxfEntityDeactivate(gxf_context_t context, gxf_uid_t eid);\n\n/// @brief Destroys a previously created entity\n///\n/// Destroys an entity immediately. The entity is destroyed even if the reference count has not\n/// yet reached 0. If the entity is active it is deactivated first.\n///\n/// Note: This function can block for the same reasons as 'GxfEntityDeactivate'.\n///\n/// @param context A valid GXF context\n/// @param eid The returned UID of the created entity\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityDestroy(gxf_context_t context, gxf_uid_t eid);\n\n/// @brief Finds an entity by its name\n///\n/// @param context A valid GXF context\n/// @param name A C string with the name of the entity. Ownership is not transferred.\n/// @param eid The returned UID of the entity\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityFind(gxf_context_t context, const char* name, gxf_uid_t* eid);\n\n/// @brief Finds all entities in the current application\n///\n/// Finds and returns all entity ids for the current application. If more than `max_entities` exist\n/// only `max_entities` will be returned. The order and selection of entities returned is arbitrary.\n///\n/// @param context A valid GXF context\n/// @param num_entities In/Out: the max number of entities that can fit in the buffer/the number of\n/// entities that exist in the application\n/// @param entities A buffer allocated by the caller for returned UIDs of all entities, with\n/// capacity for `num_entities`.\n/// @return GXF_SUCCESS if the operation was successful, GXF_QUERY_NOT_ENOUGH_CAPACITY if more\n/// entities exist in the application than `max_entities`, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityFindAll(gxf_context_t context, uint64_t* num_entities, gxf_uid_t* entities);\n\n/// @brief Increases the reference count for an entity by 1.\n///\n/// By default reference counting is disabled for an entity. This means that entities created with\n/// 'GxfCreateEntity' are not automatically destroyed. If this function is called for an entity\n/// with disabled reference count, reference counting is enabled and the reference count is set to\n/// 1. Once reference counting is enabled an entity will be automatically destroyed if the reference\n/// count reaches zero, or if 'GxfEntityDestroy' is called explicitly.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityRefCountInc(gxf_context_t context, gxf_uid_t eid);\n\n/// @brief Decreases the reference count for an entity by 1.\n///\n/// See 'GxfEntityRefCountInc' for more details on reference counting.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityRefCountDec(gxf_context_t context, gxf_uid_t eid);\n\n/// @brief Provides the reference count for an entity.\n///\n/// See 'GxfEntityRefCountInc' for more details on reference counting.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity\n/// @param count The reference count of a valid entity\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityGetRefCount(gxf_context_t context, gxf_uid_t eid, int64_t* count);\n\n/// @brief Gets the status of the entity.\n///\n/// See 'gxf_entity_status_t' for the various status.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity\n/// @param entity_status output; status of an entity eid\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityGetStatus(gxf_context_t context, gxf_uid_t eid,\n                              gxf_entity_status_t* entity_status);\n\n/// @brief Gets the name of the entity.\n///\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity\n/// @param entity_name output; name of the entity\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityGetName(gxf_context_t context, gxf_uid_t eid,\n                              const char** entity_name);\n\n/// @brief Gets the state of the entity.\n///\n/// See 'entity_state_t' for the various status.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity\n/// @param entity_state output; behavior status of an entity eid used by the\n/// behavior tree parent codelet\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the\n/// GXF error codes\ngxf_result_t GxfEntityGetState(gxf_context_t context, gxf_uid_t eid,\n                               entity_state_t* entity_state);\n\n/// @brief Notifies the occurrence of an event and inform the scheduler to check\n/// the status of the entity\n///\n/// The entity must have an 'AsynchronousSchedulingTerm' scheduling term\n/// component and it must be in \"EVENT_WAITING\" state for the notification to be\n/// acknowledged.\n///\n///  See 'AsynchronousEventState' for various states\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the\n/// GXF error codes.\n\ngxf_result_t GxfEntityEventNotify(gxf_context_t context, gxf_uid_t eid);\n\n/// @brief Various type of events used to communicate with a GXF scheduler\n///\n/// GXF_EVENT_EXTERNAL is supported by all GXF schedulers and the rest of the event types\n/// are supported by event based scheduler only. GXF_EVENT_EXTERNAL is typically intended\n/// to be used by events originating outside of the GXF framework by threads which are not\n/// owned by GXF. All other event types occur within GXF and each of them describe a specific\n/// event trigger scenario\n\ntypedef enum {\n  GXF_EVENT_CUSTOM = 0,\n  GXF_EVENT_EXTERNAL = 1,\n  GXF_EVENT_MEMORY_FREE = 2,\n  GXF_EVENT_MESSAGE_SYNC = 3,\n  GXF_EVENT_TIME_UPDATE = 4,\n  GXF_EVENT_STATE_UPDATE = 5,\n} gxf_event_t;\n\ngxf_result_t GxfEntityNotifyEventType(gxf_context_t context, gxf_uid_t eid, gxf_event_t event);\n\n/// @brief Gets a string describing an GXF event type\n///\n/// The caller does not get ownership of the return C string and must not delete it.\n///\n/// @param result A GXF error code\n/// @return A pointer to a C string with the error code description.\nconst char* GxfEventStr(gxf_event_t event);\n\n// --  Components  ---------------------------------------------------------------------------------\n\n/// @brief Maximum number of components in an entity or extension\n#define kMaxComponents 1024\n\n/// @brief Maximum number of characters in the name of an entity\n#define kMaxEntityNameSize 2048\n\n/// @brief Maximum number of characters in the name of a component\n#define kMaxComponentNameSize 256\n\n/// @brief Gets the GXF unique type ID (TID) of a component\n///\n/// Get the unique type ID which was used to register the component with GXF. The function expects\n/// the fully qualified C++ type name of the component including namespaces.\n///\n/// Example of a valid component type name: \"nvidia::gxf::PingTx\"\n///\n/// @param context A valid GXF context\n/// @param name The fully qualified C++ type name of the component\n/// @param tid The returned TID of the component\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentTypeId(gxf_context_t context, const char* name, gxf_tid_t* tid);\n\n/// @brief Gets the fully qualified C++ type name GXF component typename from a TID\n///\n/// Get the unique typename of the component with which it was registered using one of\n/// the GXF_EXT_FACTORY_ADD*() macros\n///\n/// @param context A valid GXF context\n/// @param tid The unique type ID (TID) of the component with which the component was registered\n/// @param name The returned name of the component\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentTypeName(gxf_context_t context, gxf_tid_t tid, const char** name);\n\n/// @brief Gets the fully qualified C++ type name GXF component typename from a UID\n///\n/// Get the unique typename of the component with which it was registered using one of\n/// the GXF_EXT_FACTORY_ADD*() macros\n///\n/// @param context A valid GXF context\n/// @param cid The UID of a valid component\n/// @param name The returned typename of the component\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentTypeNameFromUID(gxf_context_t context, gxf_uid_t cid, const char** name);\n\n/// @brief Gets the name of a component\n///\n/// Each component has a user-defined name which was used in the call to 'GxfComponentAdd'.\n/// Usually the name is specified in the GXF application file.\n///\n/// @param context A valid GXF context\n/// @param cid The unique object ID (UID) of the component\n/// @param name The returned name of the component\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentName(gxf_context_t context, gxf_uid_t cid, const char** name);\n\n/// @brief Gets the unique object ID of the entity of a component\n///\n/// Each component has a unique ID with respect to the context and is stored in one entity. This\n/// function can be used to retrieve the ID of the entity to which a given component belongs.\n///\n/// @param context A valid GXF context\n/// @param cid The unique object ID (UID) of the component\n/// @param eid The returned unique object ID (UID) of the entity\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentEntity(gxf_context_t context, gxf_uid_t cid, gxf_uid_t* eid);\n\n/// @brief Gets the pointer to an entity item\n///\n/// Each entity has a unique ID with respect to the context and is stored in the entity warden. This\n/// function can be used to retrieve the pointer to entity item stored in the entity warden for a\n/// given entity id.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of the entity\n/// @param ptr The returned pointer to the entity item\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityGetItemPtr(gxf_context_t context, gxf_uid_t eid, void** ptr);\n\n/// @brief Adds a new component to an entity\n///\n/// An entity can contain multiple components and this function can be used to add a new component\n/// to an entity. A component must be added before an entity is activated, or after it was\n/// deactivated. Components must not be added to active entities. The order of components is stable\n/// and identical to the order in which components are added (see 'GxfComponentFind').\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of the entity to which the component is added.\n/// @param tid The unique type ID (TID) of the component to be added to the entity.\n/// @param name The name of the new component. Ownership is not transferred.\n/// @param cid The returned UID of the created component\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentAdd(gxf_context_t context, gxf_uid_t eid, gxf_tid_t tid, const char* name,\n                             gxf_uid_t* cid);\n\n/// @brief Adds a new component to an entity\n///\n/// An entity can contain multiple components and this function can be used to add a new component\n/// to an Entity. A component must be added before an entity is activated, or after it was\n/// deactivated. Components must not be added to active entities. The order of components is stable\n/// and identical to the order in which components are added (see 'GxfComponentFind').\n///\n/// @param context A valid GXF context\n/// @param item_ptr The pointer to entity item\n/// @param tid The unique type ID (TID) of the component to be added to the entity.\n/// @param name The name of the new component. Ownership is not transferred.\n/// @param cid The returned UID of the created component\n/// @param comp_ptr The returned pointer to the created component object\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentAddAndGetPtr(gxf_context_t context, void* item_ptr, gxf_tid_t tid,\n                                      const char* name, gxf_uid_t* cid, void ** comp_ptr);\n\n/// @brief Removes a component from an entity\n///\n/// An entity can contain multiple components and this function can be used to remove a component\n/// from an entity. A component must be removed before an entity is activated, or after it was\n/// deactivated. Components must not be removed from active entities.\n///\n/// @param context A valid GXF context\n/// @param cid The UID of the component\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentRemoveWithUID(gxf_context_t context, gxf_uid_t cid);\n\n/// @brief Removes a component from an entity\n///\n/// An entity can contain multiple components and this function can be used to remove a component\n/// from an entity. A component must be removed before an entity is activated, or after it was\n/// deactivated. Components must not be removed from active entities.\n///\n/// @brief Adds an existing component to the interface of an entity\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of the entity to which the component is added.\n/// @param tid The unique type ID (TID) of the component to be added to the entity.\n/// @param name The name of the new component. Ownership is not transferred.\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentRemove(gxf_context_t context, gxf_uid_t eid, gxf_tid_t tid,\n const char * name);\n\n/// An entity can holds references to other components in its interface, so that when finding a\n/// component in an entity, both the component this entity holds and those it refers to will be\n/// returned.\n/// This supports the case when an entity contains a subgraph, then those components that has been\n/// declared in the subgraph interface will be put to the interface of the parent entity.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of the entity to which the component is added.\n/// @param cid The unique object ID of the component\n/// @param name The name of the new component. Ownership is not transferred.\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentAddToInterface(gxf_context_t context, gxf_uid_t eid,\n                                        gxf_uid_t cid, const char* name);\n\n/// @brief Finds a component in an entity\n///\n/// Searches components in an entity which satisfy certain criteria: component type, component\n/// name, and component min index. All three criteria are optional; in case no criteria is given\n/// the first component is returned. The main use case for \"component min index\" is a repeated\n/// search which continues at the index which was returned by a previous search.\n///\n/// In case no entity with the given criteria was found GXF_ENTITY_NOT_FOUND is returned.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of the entity which is searched.\n/// @param tid The component type ID (TID) of the component to find (optional)\n/// @param name The component name of the component to find (optional). Ownership not transferred.\n/// @param offset The index of the first component in the entity to search. Also contains the index\n///               of the component which was found.\n/// @param cid The returned UID of the searched component\n/// @return GXF_SUCCESS if a component matching the criteria was found, GXF_ENTITY_NOT_FOUND if no\n///         component matching the criteria was found, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentFind(gxf_context_t context, gxf_uid_t eid, gxf_tid_t tid, const char* name,\n                              int32_t* offset, gxf_uid_t* cid);\n\n/// @brief Finds a component in an entity and returns pointer to component\n///\n/// Searches components in an entity which satisfy certain criteria: component type, component name\n/// . All two criteria are optional; in case no criteria is given the first component is returned\n/// The main use case for \"component min index\" is a repeated search which continues at the index\n/// which was returned by a previous search.\n///\n/// In case no entity with the given criteria was found GXF_ENTITY_NOT_FOUND is returned.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of the entity which is searched.\n/// @param item_ptr The pointer to entity item\n/// @param tid The component type ID (TID) of the component to find (optional)\n/// @param name The component name of the component to find (optional). Ownership not transferred.\n/// @param offset The index of the first component in the entity to search. Also contains the index\n///               of the component which was found.\n/// @param cid The returned UID of the searched component\n/// @param ptr The returned pointer of the searched component\n/// @return GXF_SUCCESS if a component matching the criteria was found, GXF_ENTITY_NOT_FOUND if no\n///         component matching the criteria was found, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentFindAndGetPtr(gxf_context_t context, gxf_uid_t eid, void* item_ptr,\n                                       gxf_tid_t tid, const char* name, int32_t* offset,\n                                       gxf_uid_t* cid, void** ptr);\n\n/// @brief Finds all components in an entity\n///\n/// Finds and returns all component ids for the given entity. If more than `num_cids` exist\n/// GXF_QUERY_NOT_ENOUGH_CAPACITY will be returned and `num_cids` will be updated to the actual\n/// number of components in the entity.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of the entity which is searched.\n/// @param num_cids In/Out: the max number of components that can fit in the buffer/the number of\n/// components that exist in the entity\n/// @param cids A buffer allocated by the caller for returned UIDs of all components, with\n/// capacity for `num_cids`.\n/// @return GXF_SUCCESS if the operation was successful, GXF_QUERY_NOT_ENOUGH_CAPACITY if more\n/// components exist in the entity than `num_cids`, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentFindAll(gxf_context_t context, gxf_uid_t eid, uint64_t* num_cids,\n                                 gxf_uid_t* cids);\n\n/// @brief Gets the component type ID (TID) of a component\n///\n/// @param context A valid GXF context\n/// @param cid The component object ID (UID) for which the component type is requested.\n/// @param tid The returned TID of the component\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentType(gxf_context_t context, gxf_uid_t cid, gxf_tid_t* tid);\n\n/// @brief Verifies that a component exists, has the given type, gets a pointer to it.\n///\n/// @param context A valid GXF context\n/// @param tid The expected component type ID (TID) of the component\n/// @param pointer The returned pointer to the component object.\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentPointer(gxf_context_t context, gxf_uid_t uid, gxf_tid_t tid,\n                                 void** pointer);\n\n/// @brief Check if a registered type in an extension is derived from another registered type\n/// from the same or any other extension. This is useful query the component hierarchies using\n/// their type id's. Both the derived and base types have to be registered in an extension via\n/// one of the registered via GXF_EXT_FACTORY_ADD* macros.\n///\n/// @param context A valid GXF context\n/// @param derived The type ID (TID) of a derived type\n/// @param base The type ID (TID) of a base type\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfComponentIsBase(gxf_context_t context, gxf_tid_t derived, gxf_tid_t base,\n                                bool* result);\n\n// --  Parameter  ----------------------------------------------------------------------------------\n\n/// @brief Maximum number of parameters in a component\n#define kMaxParameters 1024\n\n/// @brief [experimental] The name of the parameter which stores the name of a component\n#define kInternalNameParameterKey \"__name\"\n\n/// @brief Flags describing the behavior of parameter\n///\n/// Parameter flags are specified when a parameter is registered as part of the component interface.\n/// Multiple flags can be OR combined.\nenum gxf_parameter_flags_t_ {\n  /// No additional flags are set (the default). This means the parameter is mandatory and static.\n  /// The parameter must be set before entity activation and can not be changed after entity\n  /// activation.\n  GXF_PARAMETER_FLAGS_NONE = 0,\n  /// The parameter value is optional and might not be available after entity activation.\n  /// This implies that it is not allowed to access the parameter with 'get()' in the C++ API.\n  /// Instead 'try_get' must be used.\n  GXF_PARAMETER_FLAGS_OPTIONAL = 1,\n  /// The parameter is dynamic an might change after entity activation. However it is still\n  /// guaranteed that parameters do not change during the initialize, deinitialize, start, tick,\n  /// or stop functions.\n  GXF_PARAMETER_FLAGS_DYNAMIC = 2\n};\n\n/// @brief Type used for parameter flags.\n///\n/// @see gxf_parameter_flags_t_\ntypedef uint32_t gxf_parameter_flags_t;\n\n// Sets a 8-bit signed integer parameter\ngxf_result_t GxfParameterSetInt8(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                 int8_t value);\n// Sets a 16-bit signed integer parameter\ngxf_result_t GxfParameterSetInt16(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                 int16_t value);\n// Sets a 32-bit signed integer parameter\ngxf_result_t GxfParameterSetInt32(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                  int32_t value);\n// Sets a 64-bit signed integer parameter\ngxf_result_t GxfParameterSetInt64(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                  int64_t value);\n// Sets a 8-bit unsigned integer parameter\ngxf_result_t GxfParameterSetUInt8(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                  uint8_t value);\n// Sets a 16-bit unsigned integer parameter\ngxf_result_t GxfParameterSetUInt16(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                   uint16_t value);\n// Sets a 32-bit unsigned integer parameter\ngxf_result_t GxfParameterSetUInt32(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                   uint32_t value);\n// Sets a 64-bit unsigned integer parameter\ngxf_result_t GxfParameterSetUInt64(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                   uint64_t value);\n// Sets a 32-bit floating point parameter\ngxf_result_t GxfParameterSetFloat32(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                    float value);\n// Sets a 64-bit floating point parameter\ngxf_result_t GxfParameterSetFloat64(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                    double value);\n// Sets a string parameter\ngxf_result_t GxfParameterSetStr(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                const char* value);\n// Sets a handle parameter\ngxf_result_t GxfParameterSetHandle(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                   gxf_uid_t cid);\n// Sets a boolean parameter\ngxf_result_t GxfParameterSetBool(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                 bool value);\n\n// Sets a String 1D-Vector parameter. Internally the data is stored in\n// a std::vector. The length of the vector should match the length of the registered parameter.\ngxf_result_t GxfParameterSet1DStrVector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                        const char* value[], uint64_t length);\n// Sets a Float 64 1D-Vector parameter. Internally the data is stored in\n// a std::vector. The length of the vector should match the length of the registered parameter.\ngxf_result_t GxfParameterSet1DFloat64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                            double* value, uint64_t length);\n// Sets a Float 64 2D-Vector parameter. Internally the data is stored in a\n// std::vector<std::vector<TYPE>. The height should match the length of the outer std::vector\n// and the width should match the length of all the inner std::vectors. Also, these height and\n// width should match the shape of the registered parameters.\ngxf_result_t GxfParameterSet2DFloat64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                            double** value, uint64_t height, uint64_t width);\n// Sets a signed 64-bit int 1D-Vector parameter. Internally the data is stored in\n// a std::vector. The length of the vector should match the length of the registered parameter.\ngxf_result_t GxfParameterSet1DInt64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                          int64_t* value, uint64_t length);\n// Sets a signed 64-bit int 2D-Vector parameter. Internally the data is stored in a\n// std::vector<std::vector<TYPE>. The height should match the length of the outer std::vector\n// and the width should match the length of all the inner std::vectors. Also, these height and\n// width should match the shape of the registered parameters.\ngxf_result_t GxfParameterSet2DInt64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                          int64_t** value, uint64_t height, uint64_t width);\n// Sets a unsigned 64-bit unsigned int 1D-Vector parameter. Internally the data is stored in\n// a std::vector. The length of the vector should match the length of the registered parameter.\ngxf_result_t GxfParameterSet1DUInt64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                           uint64_t* value, uint64_t length);\n// Sets a unsigned 64-bit unsigned int 2D-Vector parameter. Internally the data is stored in a\n// std::vector<std::vector<TYPE>. The height should match the length of the outer std::vector\n// and the width should match the length of all the inner std::vectors. Also, these height and\n// width should match the shape of the registered parameters.\ngxf_result_t GxfParameterSet2DUInt64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                           uint64_t** value, uint64_t height, uint64_t width);\n// Sets a signed 32-bit int 1-D Vector parameter. Internally the data is stored in\n// a std::vector. The length of the vector should match the length of the registered parameter.\ngxf_result_t GxfParameterSet1DInt32Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                          int32_t* value, uint64_t length);\n// Sets a signed 32-bit int 2-D Vector parameter. Internally the data is stored in a\n// std::vector<std::vector<TYPE>. The height should match the length of the outer std::vector\n// and the width should match the length of all the inner std::vectors. Also, these height and\n// width should match the shape of the registered parameters.\ngxf_result_t GxfParameterSet2DInt32Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                          int32_t** value, uint64_t height, uint64_t width);\n\n// Sets a parameter from YAML. The YAML node pointer should be a type of 'YAML::Node*'.\ngxf_result_t GxfParameterSetFromYamlNode(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                         void* yaml_node, const char* prefix);\n\n// Sets a FilePath parameter\ngxf_result_t GxfParameterSetPath(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                const char* value);\n\n// Gets a parameter as a YAML node. The YAML node pointer should be a type of 'YAML::Node*'.\ngxf_result_t GxfParameterGetAsYamlNode(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                       void* yaml_node);\n\n// Gets a 64-bit floating point parameter\ngxf_result_t GxfParameterGetFloat64(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                    double* value);\n// Gets a 32-bit floating point parameter\ngxf_result_t GxfParameterGetFloat32(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                    float* value);\n// Gets a 64-bit signed integer parameter\ngxf_result_t GxfParameterGetInt64(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                  int64_t* value);\n// Gets a 64-bit unsigned integer parameter\ngxf_result_t GxfParameterGetUInt64(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                   uint64_t* value);\n// Gets a 32-bit unsigned integer parameter\ngxf_result_t GxfParameterGetUInt32(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                   uint32_t* value);\n// Gets a 16-bit unsigned integer parameter\ngxf_result_t GxfParameterGetUInt16(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                   uint16_t* value);\n// Gets a string parameter\ngxf_result_t GxfParameterGetStr(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                const char** value);\n// Gets a file path parameter\ngxf_result_t GxfParameterGetPath(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                const char** value);\n// Gets a handle parameter\ngxf_result_t GxfParameterGetHandle(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                   gxf_uid_t* cid);\n// Gets a bool parameter\ngxf_result_t GxfParameterGetBool(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                 bool* value);\n// Gets a 32-bit signed integer parameter\ngxf_result_t GxfParameterGetInt32(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                  int32_t* value);\n// Gets the length of the 1D vector.\n// For 1D: rank = 1; shape = [length, ...]\ngxf_result_t GxfParameterGet1DFloat64VectorInfo(gxf_context_t context, gxf_uid_t uid,\n                                                const char* key, uint64_t* length);\n// Gets the height/width of the 2D vector.\n// For 2D: rank = 2; shape = [height, width, ...]\ngxf_result_t GxfParameterGet2DFloat64VectorInfo(gxf_context_t context, gxf_uid_t uid,\n                                                const char* key, uint64_t* height, uint64_t* width);\n// Gets the length of the 1D vector.\n// For 1D: rank = 1; shape = [length, ...]\ngxf_result_t GxfParameterGet1DInt64VectorInfo(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                              uint64_t* length);\n// Gets the height/width of the 2D vector.\n// For 2D: rank = 2; shape = [height, width, ...]\ngxf_result_t GxfParameterGet2DInt64VectorInfo(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                              uint64_t* height, uint64_t* width);\n// Gets the length of the 1D vector.\n// For 1D: rank = 1; shape = [length, ...]\ngxf_result_t GxfParameterGet1DUInt64VectorInfo(gxf_context_t context, gxf_uid_t uid,\n                                               const char* key, uint64_t* length);\n// Gets the height/width of the 2D vector.\n// For 2D: rank = 2; shape = [height, width, ...]\ngxf_result_t GxfParameterGet2DUInt64VectorInfo(gxf_context_t context, gxf_uid_t uid,\n                                               const char* key, uint64_t* height, uint64_t* width);\n// Gets the length of the 1D vector.\n// For 1D: rank = 1; shape = [length, ...]\ngxf_result_t GxfParameterGet1DInt32VectorInfo(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                              uint64_t* length);\n// Gets the height/width of the 2D vector.\n// For 2D: rank = 2; shape = [height, width, ...]\ngxf_result_t GxfParameterGet2DInt32VectorInfo(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                              uint64_t* height, uint64_t* width);\n// Gets a 1D-Vector of 64-bit floating point.\n// For 1D: rank = 1; shape = [length, ...]\n// The rank and shape should match with the internal registered std::vector parameter.\n// The length can be queried using GxfGetParameterInfo()\ngxf_result_t GxfParameterGet1DFloat64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                            double* value, uint64_t* length);\n// Gets a 1D-Vector of strings.\n// The length can be queried using GxfGetParameterInfo()\ngxf_result_t GxfParameterGet1DStrVector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                            char* value[], uint64_t* count, uint64_t* min_length);\n// Gets a 2D-Vector of 64-bit floating point.\n// For 2D: rank = 2; shape = [height, width, ...]\n// The rank and shape should match with the internal registered std::vector parameter.\n// The height and width can be obtained using GxfGetParameterInfo()\ngxf_result_t GxfParameterGet2DFloat64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                            double** value, uint64_t* height, uint64_t* width);\n// Gets a 1D-Vector of signed 64-bit integers.\n// For 1D: rank = 1; shape = [length, ...]\n// The rank and shape should match with the internal registered std::vector parameter.\n// The length can be queried using GxfGetParameterInfo()\ngxf_result_t GxfParameterGet1DInt64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                          int64_t* value, uint64_t* length);\n// Gets a 2D-Vector of signed 64-bit integers.\n// For 2D: rank = 2; shape = [height, width, ...]\n// The rank and shape should match with the internal registered std::vector parameter.\n// The height and width can be obtained using GxfGetParameterInfo()\ngxf_result_t GxfParameterGet2DInt64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                          int64_t** value, uint64_t* height, uint64_t* width);\n// Gets a 1D-Vector of unsigned 32-bit integers.\n// For 1D: rank = 1; shape = [length, ...]\n// The rank and shape should match with the internal registered std::vector parameter.\n// The length can be queried using GxfGetParameterInfo()\ngxf_result_t GxfParameterGet1DUInt64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                           uint64_t* value, uint64_t* length);\n// Gets a 2D-Vector of unsigned 32-bit integers.\n// For 2D: rank = 2; shape = [height, width, ...]\n// The rank and shape should match with the internal registered std::vector parameter.\n// The height and width can be obtained using GxfGetParameterInfo()\ngxf_result_t GxfParameterGet2DUInt64Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                           uint64_t** value, uint64_t* height, uint64_t* width);\n// Gets a 1D-Vector of signed 32-bit integers.\n// For 1D: rank = 1; shape = [length, ...]\n// The rank and shape should match with the internal registered std::vector parameter.\n// The length can be queried using GxfGetParameterInfo()\ngxf_result_t GxfParameterGet1DInt32Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                          int32_t* value, uint64_t* length);\n// Gets a 2D-Vector of signed 32-bit integers.\n// For 2D: rank = 2; shape = [height, width, ...]\n// The rank and shape should match with the internal registered std::vector parameter.\n// The height and width can be obtained using GxfGetParameterInfo()\ngxf_result_t GxfParameterGet2DInt32Vector(gxf_context_t context, gxf_uid_t uid, const char* key,\n                                          int32_t** value, uint64_t* height, uint64_t* width);\n// --  Graph execution  ----------------------------------------------------------------------------\n\n// Loads a list of entities from a YAML file.\ngxf_result_t GxfGraphLoadFile(gxf_context_t context, const char* filename,\n                              const char* parameters_override[] = nullptr,\n                              const uint32_t num_overrides = 0);\n\n// Loads a list of entities from a YAML file. This API is used when shared context is created using\n// GxfGetSharedContext() and GxfContextCreateShared(). Separate instances of entities are created\n// for shared context by adding entity_prefix to the entity name.\ngxf_result_t GxfGraphLoadFileExtended(gxf_context_t context, const char* filename,\n                                      const char* entity_prefix,\n                                      const char* parameters_override[] = nullptr,\n                                      const uint32_t num_overrides = 0,\n                                      gxf_uid_t parent_eid = kNullUid,\n                                      void* prerequisites = nullptr);\n\n// Saves a list of entities to a YAML file.\ngxf_result_t GxfGraphSaveToFile(gxf_context_t context, const char* filename);\n\n// Set the root folder for searching YAML files during loading\ngxf_result_t GxfGraphSetRootPath(gxf_context_t context, const char* path);\n\n// Loads a list of entities from a YAML file.\ngxf_result_t GxfGraphParseString(gxf_context_t context, const char* text,\n                                 const char* parameters_override[] = nullptr,\n                                 const uint32_t num_overrides = 0);\n\n// Activate all System components\ngxf_result_t GxfGraphActivate(gxf_context_t context);\n\n// Deactivate all System components\ngxf_result_t GxfGraphDeactivate(gxf_context_t context);\n\n// Starts the execution of the graph asynchronously\ngxf_result_t GxfGraphRunAsync(gxf_context_t context);\n\n// Interrupt the execution of the graph\ngxf_result_t GxfGraphInterrupt(gxf_context_t context);\n\n// Waits for the graph to complete execution\ngxf_result_t GxfGraphWait(gxf_context_t context);\n\n// Runs all System components and waits for their completion\ngxf_result_t GxfGraphRun(gxf_context_t context);\n\n// --  Info queries  -------------------------------------------------------------------------------\n\n// Type to represent version of GXF Runtime and list of loaded Extensions\ntypedef struct {\n  const char* version;      // GXF Runtime Version\n  uint64_t num_extensions;  // in-out capacity of extensions/Number of extension types\n  gxf_tid_t* extensions;    // List of Extension IDs\n} gxf_runtime_info;\n\n// Gets Meta Data about the GXF Runtime\ngxf_result_t GxfRuntimeInfo(gxf_context_t context, gxf_runtime_info* info);\n\n// Type to represent description and list of components for loaded Extension\ntypedef struct {\n  gxf_tid_t id;             // (UUID) Extension ID (registered via GXF_EXT_FACTORY_SET_INFO)\n  const char* name;         // (String) Extension Name (registered via GXF_EXT_FACTORY_SET_INFO)\n  const char* description;  // (String) Description (registered via GXF_EXT_FACTORY_SET_INFO)\n  const char* version;      // (String) Extension Version (registered via GXF_EXT_FACTORY_SET_INFO)\n  const char* runtime_version;\n                            // (String) GXF Core version with which the extension was compiled with\n  const char* license;      // (String) Extension License (registered via GXF_EXT_FACTORY_SET_INFO)\n  const char* author;       // (String) Extension Author (registered via GXF_EXT_FACTORY_SET_INFO)\n  const char* display_name;\n                            // (String) Extension Display Name (registered via\n                            //  GXF_EXT_FACTORY_SET_DISPLAY_INFO, maximum 30 characters)\n  const char* category;     // (String) Extension Category (registered via\n                            //  GXF_EXT_FACTORY_SET_DISPLAY_INFO, maximum 30 characters)\n  const char* brief;        // (String) Extension Brief (registered via\n                            //  GXF_EXT_FACTORY_SET_DISPLAY_INFO, maximum 50 characters)\n  uint64_t num_components;  // in-out capacity of components/Number of components\n  gxf_tid_t* components;    // List of IDs of provided components\n} gxf_extension_info_t;\n\n// Gets description and list of components in loaded Extension\ngxf_result_t GxfExtensionInfo(gxf_context_t context, gxf_tid_t tid, gxf_extension_info_t* info);\n\n// Type to represent description and list of Parameter of Component\ntypedef struct {\n  gxf_tid_t cid;            // (UUID) Component ID (registered via GXF_EXT_FACTORY_ADD)\n  const char* base_name;    // (String) Base Class Name (registered via GXF_EXT_FACTORY_ADD)\n  int is_abstract;          // (Bool) If the Component is abstract (Can not be instantiated)\n  const char* type_name;    // (String) Component Name (registered via GXF_EXT_FACTORY_ADD)\n  const char* display_name;\n                            // (String) Extension Display Name (registered via\n                            //  GXF_EXT_FACTORY_SET_DISPLAY_INFO, maximum 30 characters)\n  const char* brief;        // (String) Extension Brief (registered via\n                            //  GXF_EXT_FACTORY_SET_DISPLAY_INFO, maximum 50 characters)\n  const char* description;  // (String) Description (registered via GXF_EXT_FACTORY_ADD)\n  uint64_t num_parameters;  // in-out capacity of parameters/Number of Parameters\n  const char** parameters;  // List of Names for Parameters\n} gxf_component_info_t;\n\n// Gets description and list of parameters of Component. Parameters are only available after\n// at least one instance is created for the Component.\ngxf_result_t GxfComponentInfo(gxf_context_t context, gxf_tid_t tid, gxf_component_info_t* info);\n\n/// @brief The type of a parameter\ntypedef enum {\n  GXF_PARAMETER_TYPE_CUSTOM = 0,   // A custom type not natively supported by GXF.\n  GXF_PARAMETER_TYPE_HANDLE = 1,   // A GXF handle. The handle type is specified separately.\n  GXF_PARAMETER_TYPE_STRING = 2,   // A null-terminated character string (const char*)\n  GXF_PARAMETER_TYPE_INT64 = 3,    // A 64-bit signed integer (int64_t)\n  GXF_PARAMETER_TYPE_UINT64 = 4,   // A 64-bit unsigned integer (uint64_t)\n  GXF_PARAMETER_TYPE_FLOAT64 = 5,  // A 64-bit floating point (double)\n  GXF_PARAMETER_TYPE_BOOL = 6,     // A boolean type (bool)\n  GXF_PARAMETER_TYPE_INT32 = 7,    // A 32-bit signed integer (int32_t)\n  GXF_PARAMETER_TYPE_FILE = 8,     // a file system path (string)\n  GXF_PARAMETER_TYPE_INT8 = 9,     // A 8-bit signed integer (int8_t)\n  GXF_PARAMETER_TYPE_INT16 = 10,   // A 16-bit signed integer (int16_t)\n  GXF_PARAMETER_TYPE_UINT8 = 11,   // A 8-bit unsigned integer (uint8_t)\n  GXF_PARAMETER_TYPE_UINT16 = 12,  // A 16-bit unsigned integer (uint16_t)\n  GXF_PARAMETER_TYPE_UINT32 = 13,  // A 32-bit unsigned integer (uint32_t)\n  GXF_PARAMETER_TYPE_FLOAT32 = 14,  // A 32-bit floating point (float)\n  GXF_PARAMETER_TYPE_COMPLEX64 = 15,  // A 64-bit complex floating point (float)\n  GXF_PARAMETER_TYPE_COMPLEX128 = 16,  // A 128-bit complex floating point (double)\n} gxf_parameter_type_t;\n\n// Holds metadata information about a parameter which was registered as part of the component\n// interface.\ntypedef struct {\n  const char* key;              // The name of the parameter as it appears in the GXF file.\n  const char* headline;         // A short headline used to display the parameter to a human.\n  const char* description;      // A longer text describing the usage of the parameter.\n  gxf_parameter_flags_t flags;  // Parameter flags for example to make a parameter optional.\n  gxf_parameter_type_t type;    // The type of the parameter\n  gxf_tid_t handle_tid;         // In case the parameter is a handle the TID of the component.\n  const void* default_value;    // Default value of parameter, N/A for handle and custom params\n  const void* numeric_min;      // Min value of range for numeric parameters, N/A for other types\n  const void* numeric_max;      // Max value of range for numeric parameters, N/A for other types\n  const void* numeric_step;     // Step value of range for numeric parameters, N/A for other types\n  const char* platform_information;\n                                // (String) Platforms separated by comma. Empty means all platforms\n  int32_t rank;                 // Rank of the parameter. 0-scalar, 1-list etc. Max rank is 8\n  int32_t shape[8];             // Sizes of multi dimensional parameters if it is of fixed\n                                // length(array). Shape of a dimension is -1 for scalar and\n                                // variable length arrays(vector)\n} gxf_parameter_info_t;\n\n// Gives a string describing the parameter type\nconst char* GxfParameterTypeStr(gxf_parameter_type_t param_type);\n\n// Gives a string describing the flag type\nconst char* GxfParameterFlagTypeStr(gxf_parameter_flags_t_ flag_type);\n\n// Gets description of specific parameter. Fail if the component is not instantiated yet.\ngxf_result_t GxfParameterInfo(gxf_context_t context, gxf_tid_t cid, const char* key,\n                              gxf_parameter_info_t* info);\n\ngxf_result_t GxfGetParameterInfo(gxf_context_t context, gxf_tid_t cid, const char* key,\n                              gxf_parameter_info_t* info);\n\ngxf_result_t GxfRedirectLog(gxf_context_t context, FILE* fp);\n\n// -------------------------------------------------------------------------------------------------\n\n#ifdef __cplusplus\n}  // extern \"C\"\n#endif\n\n#ifdef __cplusplus\n\ninline bool operator==(const gxf_tid_t& lhs, const gxf_tid_t& rhs) noexcept {\n  return lhs.hash1 == rhs.hash1 && lhs.hash2 == rhs.hash2;\n}\n\ninline bool operator!=(const gxf_tid_t& lhs, const gxf_tid_t& rhs) noexcept {\n  return lhs.hash1 != rhs.hash1 || lhs.hash2 != rhs.hash2;\n}\n\ninline bool operator<(const gxf_tid_t& lhs, const gxf_tid_t& rhs) noexcept {\n  return lhs.hash1 < rhs.hash1 || (lhs.hash1 == rhs.hash1 && lhs.hash2 < rhs.hash2);\n}\n\n#endif\n\n#endif  // NVIDIA_GXF_CORE_GXF_H_\n\n// Include the extended API functions.\n#include \"gxf_ext.h\"\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/gxf_ext.h",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CORE_GXF_EXT_H_\n#define NVIDIA_GXF_CORE_GXF_EXT_H_\n\n#ifdef __cplusplus\n#include <cstdint>\n#else\n#include <stdint.h>\n#endif\n\n#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n// -------------------------------------------------------------------------------------------------\n\n/// @brief GXF bitmasks\ntypedef uint32_t GxfFlags;\n\n// -------------------------------------------------------------------------------------------------\n\n/// @brief Structure specifying parameters for loading extensions\ntypedef struct {\n  // Optional list of extension filenames to load\n  const char* const* extension_filenames;\n  // The number of extensions to load\n  uint32_t extension_filenames_count;\n  // Optional list of manifest filenames to load\n  const char* const* manifest_filenames;\n  // The number of manifests to load\n  uint32_t manifest_filenames_count;\n  // An optional base directory which is prepended to all extensions filenames, including those\n  // loaded via manifests.\n  const char* base_directory;\n} GxfLoadExtensionsInfo;\n\n/// @brief Loads GXF extension libraries\n///\n/// Loads one or more extensions either directly by their filename or indirectly by loading\n/// manifest files. Before a component can be added to a GXF entity the GXF extension shared\n/// library providing the component must be loaded. An extensions must only be loaded once.\n///\n/// To simplify loading multiple extensions at once the developer can create a manifest file which\n/// lists all extensions he needs. This function will then load all extensions listed in the\n/// manifest file. Multiple manifest may be loaded, however each extensions may still be loaded\n/// only a single time.\n///\n/// A manifest file is a YAML file with a single top-level entry 'extensions' followed by a\n/// list of filenames of GXF extension shared libraries.\n///\n/// Example:\n/// -----  START OF FILE  -----\n/// extensions:\n/// - gxf/std/libgxf_std.so\n/// - gxf/npp/libgxf_npp.so\n/// -----   END OF FILE   -----\n///\n/// @param context is the GXF context in which to load extensions\n/// @param info is a pointer to a GxfLoadExtensionsInfo structure describing parameters for loading\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfLoadExtensions(gxf_context_t context, const GxfLoadExtensionsInfo* info);\n\n/// @brief Loads a metadata file generated by the GXF registry\n///\n/// The GXF registry tool generates a metadata file of the contents of an extension during\n/// registration. These metadata files can be used to resolve typename and TID's of components for\n/// other extensions which depend on them. metadata files do not contain the actual implementation\n/// of the extension and must be loaded only to run the extension query API's on extension\n/// libraries which have the actual implementation and only depend on the metadata for type\n/// resolution.\n///\n/// If some components of extension B depend on some components in extension A:\n/// - Load metadata file for extension A\n/// - Load extension library for extension B using 'GxfLoadExtensions'\n/// - Run extension query api's on extension B and it's components.\n///\n/// @param context A valid GXF context\n/// @param filenames absolute paths of metadata files generated by the registry during\n///                  extension registration\n/// @param count The number of metadata files to be loaded\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfLoadExtensionMetadataFiles(gxf_context_t context, const char* const* filenames,\n                                           uint32_t count);\n\n// -------------------------------------------------------------------------------------------------\n\n/// @brief Bitmask specifying storage mode for an entity\ntypedef enum GxfEntityCreateFlagBits {\n  // 'GXF_ENTITY_CREATE_PROGRAM_BIT' specifies that the entity will be added to the program\n  // entities. Program entities are kept alive for the duration of the program. They are activated\n  // automatically when the program is activated and deactivated when the program is deactivated.\n  // If the program was already activated when the entity is created the entity must still be\n  // activated manually.\n  GXF_ENTITY_CREATE_PROGRAM_BIT = 0x00000001\n} GxfEntityCreateFlagBits;\n\n/// @brief Bitmask of GxfEntityCreateFlagBits\ntypedef GxfFlags GxfEntityCreateFlags;\n\n/// @brief Structure specifying parameters for creating entities\ntypedef struct {\n  // 'entity_name' is the name of the entity which is created. If this is a nullptr an undefined\n  // unique name is chosen. The name must not start with a double underscore.\n  const char* entity_name;\n  // 'flags' is a bitmask of GxfEntityCreateFlagsBits indicating storage method and usage behavior\n  // for the created entity.\n  GxfEntityCreateFlags flags;\n} GxfEntityCreateInfo;\n\n/// @brief Create a new GXF entity\n///\n/// Entities are light-weight containers to hold components and form the basic building blocks\n/// of a GXF application. Entities are created when a GXF file is loaded, or they can be created\n/// manually using this function. Entities created with this function must be destroyed using\n/// 'GxfEntityDestroy'. After the entity was created components can be added to it with\n/// 'GxfComponentAdd'. To start execution of codelets on an entity the entity needs to be\n/// activated first. This can happen automatically using 'GXF_ENTITY_CREATE_PROGRAM_BIT' or\n/// manually using 'GxfEntityActivate'.\n///\n/// @param context is the GXF context that creates the entity.\n/// @param info is a pointer to a GxfEntityCreateInfo structure containing parameters affecting\n///             the creation of the entity.\n/// @param eid is a pointer to a gxf_uid_t handle in which the resulting entity is returned.\n/// @return On success the function returns GXF_SUCCESS.\ngxf_result_t GxfCreateEntity(gxf_context_t context, const GxfEntityCreateInfo* info,\n                             gxf_uid_t* eid);\n\n\n/// @brief Create a new GXF entity and return the entity item ptr\n///\n/// Entities are light-weight containers to hold components and form the basic building blocks\n/// of a GXF application. Entities are created when a GXF file is loaded, or they can be created\n/// manually using this function. Entities created with this function must be destroyed using\n/// 'GxfEntityDestroy'. After the entity was created components can be added to it with\n/// 'GxfComponentAdd'. To start execution of codelets on an entity the entity needs to be\n/// activated first. This can happen automatically using 'GXF_ENTITY_CREATE_PROGRAM_BIT' or\n/// manually using 'GxfEntityActivate'. This function also returns the pointer entity item\n/// which can be used to create C++ nvidia::gxf::Entity type objects.\n///\n/// @param context is the GXF context that creates the entity.\n/// @param info is a pointer to a GxfEntityCreateInfo structure containing parameters affecting\n///             the creation of the entity.\n/// @param eid is a pointer to a gxf_uid_t handle in which the resulting entity is returned.\n/// @param item_ptr is a pointer to pointer to entity item which is created\n/// @return On success the function returns GXF_SUCCESS.\ngxf_result_t GxfCreateEntityAndGetItem(gxf_context_t context, const GxfEntityCreateInfo* info,\n                                       gxf_uid_t* eid, void** item_ptr);\n// -------------------------------------------------------------------------------------------------\n/// @brief Name of default EntityGroup\n#define kDefaultEntityGroupName \"default_entity_group\"\n\n/// @brief Create a new GXF EntityGroup\n///\n/// EntityGroup is a group of EntityItems, such that these entities are bonded to some\n/// common properties. For now the common property is all kinds of resources.\n///\n/// @param context A valid GXF context\n/// @param name name to create the EntityGroup\n/// @param gid returned uid for the created EntityGroup, abbreviation for group id.\n/// @return GXF error code\ngxf_result_t GxfCreateEntityGroup(gxf_context_t context, const char* name, gxf_uid_t* gid);\n\n/// @brief Add an entity eid into an EntityGroup by EntityGroup uid\n///\n/// Each EntityItem is created with default EntityGroup uid, this API\n/// 1. updates EntityGroup uid in current EntityItem, to new group id gid;\n/// 2. Remove eid from its previous EntityGroup;\n/// 3. add eid to its new EntityGroup\n///\n/// @param context A valid GXF context\n/// @param gid UID of an existing (new) EntityGroup\n/// @param eid The unique object ID (UID) of a valid entity\n/// @return GXF error code\ngxf_result_t GxfUpdateEntityGroup(gxf_context_t context, gxf_uid_t gid, gxf_uid_t eid);\n\n/// @brief Check if an entity id is valid currently in GXF runtime\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity to check\n/// @param valid returned boolean indicating if the entity is valid in Gxf runtime(warden)\n/// @return GXF error code\ngxf_result_t GxfEntityIsValid(gxf_context_t context, gxf_uid_t eid, bool* valid);\n\n/// @brief Finds and returns all resource component cids for EntityGroup pointed by eid\n///\n/// Finds and returns all resource component cids for EntityGroup pointed by eid.\n/// If more than `max_entities` exist only `max_entities` will be returned.\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity\n/// @param num_resource_cids In/Out: the max number of components that can fit in\n/// the buffer/the number of resources in eid's EntityGroup\n/// @param resource_cids A buffer allocated by the caller for returned UIDs of all resources, with\n/// capacity for `num_resource_cids`.\n/// @return GXF_SUCCESS if the operation was successful, GXF_QUERY_NOT_ENOUGH_CAPACITY if more\n/// resources exist in eid's EntityGroup, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityGroupFindResources(gxf_context_t context, gxf_uid_t eid,\n                                         uint64_t* num_resource_cids, gxf_uid_t* resource_cids);\n\n/// @brief Find the EntityGroup gid that the entity belongs to\n///\n/// EntityGroup is a group of EntityItems, such that these entities are bonded to some\n/// common properties. For now the common property is all kinds of resources. Through\n/// life time of each entity, it always corresponds to an EntityGroup. Eg, newly created\n/// EntityItem points to Default EntityGroup, and user can update its EntityGroup to valid\n/// one only.\n///\n/// @param context A valid GXF context\n/// @param eid eid of an EntityItem, whose EntityGroup id field is used find EntityGroup name\n/// @param gid The returned id of the entity group.\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityGroupId(gxf_context_t context, gxf_uid_t eid, gxf_uid_t* gid);\n\n/// @brief Find name of EntityGroup that holds the entity\n///\n/// EntityGroup is a group of EntityItems, such that these entities are bonded to some\n/// common properties. For now the common property is all kinds of resources. Through\n/// life time of each entity, it always corresponds to an EntityGroup. Eg, newly created\n/// EntityItem points to Default EntityGroup, and user can update its EntityGroup to valid\n/// one only.\n///\n/// @param context A valid GXF context\n/// @param eid eid of an EntityItem, whose EntityGroup id field is used find EntityGroup name\n/// @param name The returned name of the EntityGroup\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityGroupName(gxf_context_t context, gxf_uid_t eid, const char** name);\n\n/// @brief Gets cid of a resource component that is grouped with the given entity\n///\n/// @param context A valid GXF context\n/// @param eid The unique object ID (UID) of a valid entity\n/// @param type The fully qualified C++ type name of the component\n/// @param resource_key the key or name of the resource\n/// @param resource_cid The returned cid of the resource component\n/// @return GXF_SUCCESS if the operation was successful, or otherwise one of the GXF error codes.\ngxf_result_t GxfEntityResourceGetHandle(gxf_context_t context, gxf_uid_t eid,\n               const char* type, const char* resource_key, gxf_uid_t* resource_cid);\n// -------------------------------------------------------------------------------------------------\n\n/// @brief Severity levels for GXF_LOG_* logging macros\ntypedef enum {\n  GXF_SEVERITY_NONE = 0,\n  GXF_SEVERITY_ERROR = 1,\n  GXF_SEVERITY_WARNING = 2,\n  GXF_SEVERITY_INFO = 3,\n  GXF_SEVERITY_DEBUG = 4,\n  GXF_SEVERITY_VERBOSE = 5,\n} gxf_severity_t;\n\n/// @brief Sets the severity level of the logs (corresponding to GXF_LOG_* logging macros) for the\n/// entire application\n///\n/// @param context a valid GXF context\n/// @param severity a valid severity level as defined in `gxf_severity_t`. Logs corresponding to\n///                 any level <= severity will be logged.\n/// @return On success the function returns GXF_SUCCESS.\n///\ngxf_result_t GxfSetSeverity(gxf_context_t context, gxf_severity_t severity);\n\n/// @brief Returns the current severity level of the logs.\n///\n/// @param context a valid GXF context\n/// @param severity a pointer to a gxf_severity_t in which the resulting severity is returned.\n/// @return On success the function returns GXF_SUCCESS.\n///\ngxf_result_t GxfGetSeverity(gxf_context_t context, gxf_severity_t* severity);\n\n// -------------------------------------------------------------------------------------------------\n\n/// @brief Simple macro to convert enum values to their string representations which can be\n/// useful for logging enum types\n#define GXF_ENUM_TO_STR(X, Y) \\\n  case X:               \\\n    return #Y;\n\n\n#ifdef __cplusplus\n}  // extern \"C\"\n#endif\n\n#endif  // NVIDIA_GXF_CORE_GXF_EXT_H_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/handle.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CORE_HANDLE_HPP\n#define NVIDIA_GXF_CORE_HANDLE_HPP\n\n#include <cinttypes>\n#include <string>\n\n#include \"common/assert.hpp\"\n#include \"common/type_name.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// A handle which gives access to a component without specifying its type.\nclass UntypedHandle {\n public:\n  UntypedHandle(const UntypedHandle& component) = default;\n  UntypedHandle(UntypedHandle&& component) = default;\n  UntypedHandle& operator=(const UntypedHandle& component) = default;\n  UntypedHandle& operator=(UntypedHandle&& component) = default;\n\n  // Creates a null untyped handle\n  static UntypedHandle Null() {\n    return UntypedHandle{kNullContext, kNullUid};\n  }\n\n  // Creates a new untyped handle\n  static Expected<UntypedHandle> Create(gxf_context_t context, gxf_uid_t cid) {\n    UntypedHandle untyped_handle{context, cid};\n    gxf_tid_t tid;\n    gxf_result_t code = GxfComponentType(context, cid, &tid);\n    if (code != GXF_SUCCESS) {\n      return Unexpected{code};\n    }\n    const auto result = untyped_handle.initialize(tid);\n    if (!result) {\n      return ForwardError(result);\n    }\n    return untyped_handle;\n  }\n\n  // Creates a new untyped handle with component pointer\n  static Expected<UntypedHandle> Create(gxf_context_t context, gxf_uid_t cid,\n                                        gxf_tid_t tid, void* ptr) {\n    return UntypedHandle{context, cid, tid, ptr};\n  }\n\n  // The context to which the component belongs\n  gxf_context_t context() const { return context_; }\n  // The ID of the component.\n  gxf_uid_t cid() const { return cid_; }\n  // The type ID describing the component type.\n  gxf_tid_t tid() const { return tid_; }\n  // Get the pointer to underlying component object\n  void* get_ptr() const {return pointer_;}\n  // Returns null if the handle is equivalent to a nullptr.\n  bool is_null() const {\n    return context_ == kNullContext || cid_ == kNullUid || pointer_ == nullptr;\n  }\n  // Same as 'is_null'.\n  explicit operator bool() const { return !is_null(); }\n  // The component name\n  const char* name() const {\n    const char* result;\n    const gxf_result_t code = GxfComponentName(context(), cid(), &result);\n    return (code == GXF_SUCCESS) ? result : \"\";\n  }\n\n protected:\n  UntypedHandle(gxf_context_t context, gxf_uid_t cid)\n    : context_{context}, cid_{cid}, tid_{GxfTidNull()}, pointer_{nullptr} { }\n\n  UntypedHandle(gxf_context_t context, gxf_uid_t cid, gxf_tid_t tid, void* pointer)\n    : context_{context}, cid_{cid}, tid_{tid}, pointer_{pointer} {}\n\n  Expected<void> initialize(gxf_tid_t tid) {\n    tid_ = tid;\n    if (pointer_ == nullptr) {\n        return ExpectedOrCode(GxfComponentPointer(context_, cid_, tid_, &pointer_));\n    }\n    return Success;\n  }\n\n  Expected<void> initialize(const char* type_name) {\n    gxf_tid_t tid;\n    gxf_result_t code = GxfComponentTypeId(context_, type_name, &tid);\n    if (code != GXF_SUCCESS) {\n      return Unexpected{code};\n    }\n    return initialize(tid);\n  }\n\n  Expected<void> verifyPointer() const {\n    if (pointer_ == nullptr) {\n      GXF_LOG_ERROR(\"Handle pointer is null for component %s - id %ld\", name(), cid());\n      return Unexpected{GXF_FAILURE};\n    }\n    void* raw_pointer;\n    auto result = GxfComponentPointer(context(), cid(), tid_, &raw_pointer);\n    if (result != GXF_SUCCESS) {\n      return Unexpected{result};\n    }\n    if (raw_pointer != pointer_) {\n      GXF_LOG_ERROR(\"Handle pointers do not match for component %s: %p vs %p\",\n                    name(), raw_pointer, pointer_);\n      return Unexpected{GXF_FAILURE};\n    }\n    return Success;\n  }\n\n  gxf_context_t context_;\n  gxf_uid_t cid_;\n  gxf_tid_t tid_;\n  void* pointer_;\n};\n\n// A handle which gives access to a component with a specific type.\ntemplate <typename T>\nclass Handle : public UntypedHandle {\n public:\n  // Creates a null handle\n  static Handle Null() {\n    return Handle{};\n  }\n\n  // An unspecified handle is a unique handle used to denote a component which\n  // will be created in the future. A parameter of Handle to a type does not consider\n  // \"Unspecified\" as a valid parameter value and hence this handle must only be used\n  // when defining a graph application across different files and the parameters are set\n  // in a delayed fashion (sub-graphs and parameter yaml files for example)\n  // Entity activation will fail if any of the mandatory parameters are \"Unspecified\"\n  static Handle Unspecified() {\n    return Handle(kNullContext, kUnspecifiedUid);\n  }\n\n  // Creates a new handle\n  static Expected<Handle> Create(gxf_context_t context, gxf_uid_t cid) {\n    Handle handle{context, cid};\n    const auto result = handle.initialize(TypenameAsString<T>());\n    if (!result) {\n      return ForwardError(result);\n    }\n    return handle;\n  }\n\n  static Expected<Handle> Create(gxf_context_t context, gxf_uid_t cid,\n                                 gxf_tid_t tid, void * ptr) {\n    if (GxfTidIsNull(tid) || ptr == nullptr) {\n      return Create(context, cid);\n    }\n    Handle handle{context, cid, tid, ptr};\n    return handle;\n  }\n\n  // Creates a new handle from an untyped handle\n  static Expected<Handle> Create(const UntypedHandle& untyped_handle) {\n    return Create(untyped_handle.context(), untyped_handle.cid(),\n                  untyped_handle.tid(), untyped_handle.get_ptr());\n  }\n\n  friend bool operator==(const Handle& lhs, const Handle& rhs) {\n    return lhs.context() == rhs.context() && lhs.cid() == rhs.cid();\n  }\n\n  friend bool operator!=(const Handle& lhs, const Handle& rhs) {\n    return lhs.context() != rhs.context() || lhs.cid() != rhs.cid();\n  }\n\n  friend bool operator<(const Handle& lhs, const Handle& rhs) {\n    return lhs.cid() < rhs.cid();\n  }\n\n  Handle(gxf_context_t context = kNullContext, gxf_uid_t uid = kNullUid)\n    : UntypedHandle{context, uid} {}\n\n  Handle(gxf_context_t context , gxf_uid_t uid , gxf_tid_t tid, void* ptr)\n    : UntypedHandle{context, uid, tid, ptr} {}\n\n  ~Handle() = default;\n\n  Handle(const Handle& component) = default;\n  Handle(Handle&& component) = default;\n  Handle& operator=(const Handle& component) = default;\n  Handle& operator=(Handle&& component) = default;\n\n  template <typename Derived>\n  Handle(const Handle<Derived>& derived) : UntypedHandle(derived) {\n    static_assert(std::is_base_of<T, Derived>::value,\n                  \"Handle conversion is only allowed from derived class to base class\");\n  }\n\n  // Allow conversion from handle to pointer\n  operator T*() const { return get(); }\n\n  T* operator->() const {\n    return get();\n  }\n\n  T* get() const {\n    // Verifying the pointer every time the handle is accessed causes a\n    // significant bottle neck in perf. This should be removed in a future\n    // release. Uncomment below line if there is any problem.\n    // GXF_ASSERT(verifyPointer(), \"Invalid Component Pointer.\");\n    GXF_ASSERT_FALSE(pointer_ == nullptr);\n    return reinterpret_cast<T*>(pointer_);\n  }\n\n  Expected<T*> try_get() const {\n    // Verifying the pointer every time the handle is accessed causes a\n    // significant bottle neck in perf. This should be removed in a future\n    // release. Uncomment below line if there is any problem.\n    // if (!verifyPointer()) { return Unexpected{GXF_FAILURE}; }\n    if (pointer_ == nullptr) { return Unexpected{GXF_FAILURE}; }\n    return reinterpret_cast<T*>(pointer_);\n  }\n\n private:\n  using UntypedHandle::UntypedHandle;\n};\n\n\n// Creates a new handle given the string representation of the handle in a yaml blob\n// If the value is of format <entity-name>/<component-name> then cid is ignored.\n// If the entity name is not specified, the value indicates just the component name, then\n// 'cid' is used to query the entity name to create the handle\ntemplate <typename T>\nstatic Expected<Handle<T>> CreateHandleFromString(gxf_context_t context, gxf_uid_t cid,\n                                                  const char* value) {\n  gxf_uid_t eid;\n  std::string component_name;\n  const std::string tag{value};\n  const size_t pos = tag.find('/');\n\n  if (pos == std::string::npos) {\n    // Get the entity of this component\n    auto result = GxfComponentEntity(context, cid, &eid);\n    if (result != GXF_SUCCESS) {\n      GXF_LOG_ERROR(\"Failed to find entity for component [C%05\" PRId64 \"] with name [%s] with\"\n                    \" error %s\", cid, value, GxfResultStr(result));\n      return Unexpected{result};\n    }\n    component_name = tag;\n  } else {\n    // Split the tag into entity and component name\n    const std::string entity_name = tag.substr(0, pos);\n    component_name = tag.substr(pos + 1);\n    // Search for the entity\n    auto result = GxfEntityFind(context, entity_name.c_str(), &eid);\n    if (result != GXF_SUCCESS) {\n      GXF_LOG_ERROR(\"Failed to find entity [E%05\" PRId64 \"] with name [%s] while parsing '%s'\",\n          eid, entity_name.c_str(), tag.c_str());\n      return Unexpected{result};\n    }\n  }\n\n  // Get the type id of the component we are are looking for.\n  gxf_tid_t tid;\n  auto result = GxfComponentTypeId(context, TypenameAsString<T>(), &tid);\n  if (result != GXF_SUCCESS) {\n    GXF_LOG_ERROR(\"Failed to find type ID of component type [%s] with error %s\",\n        TypenameAsString<T>(), GxfResultStr(result));\n    return Unexpected{result};\n  }\n\n  gxf_uid_t cid2;\n  // Find the component in the indicated entity\n  result = GxfComponentFind(context, eid, tid, component_name.c_str(), nullptr, &cid2);\n  if (result != GXF_SUCCESS) {\n    GXF_LOG_ERROR(\"Failed to find component in entity [E%05\" PRId64 \"] with name [%s]\",\n        eid, component_name.c_str());\n    return Unexpected{result};\n  }\n\n  return Handle<T>::Create(context, cid2);\n}\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/parameter.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CORE_PARAMETER_HPP_\n#define NVIDIA_GXF_CORE_PARAMETER_HPP_\n\n#include <functional>\n#include <mutex>\n#include <string>\n#include <utility>\n\n#include \"common/assert.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/core/parameter_parser.hpp\"\n#include \"gxf/core/parameter_wrapper.hpp\"\n\n\nnamespace nvidia {\nnamespace gxf {\n\n// Base class for parameters stored in ParameterStorage\nclass ParameterBackendBase {\n public:\n  virtual ~ParameterBackendBase() = default;\n\n  // The context to which this parameter backend belongs\n  gxf_context_t context() const { return context_; }\n\n  // The object to which this parameter is attached.\n  gxf_uid_t uid() const { return uid_; }\n\n  // The name of the parameter\n  const char* key() const { return key_; }\n\n  // Returns true if the parameter is guaranteed to always have a value set. Only mandatory\n  // parameters can be accessed directly with 'get' instead of using 'try_get'.\n  bool isMandatory() const { return (flags_ & GXF_PARAMETER_FLAGS_OPTIONAL) == 0; }\n\n  // Returns true if the parameter can not be changed after the component has been activated.\n  bool isConstant() const { return (flags_ & GXF_PARAMETER_FLAGS_DYNAMIC) == 0; }\n\n  // Sets the latest value from the backend to the frontend.\n  virtual void writeToFrontend() = 0;\n\n  // Parses the parameter from the given YAML object.\n  virtual Expected<void> parse(const YAML::Node& node, const std::string& prefix) = 0;\n\n  // Wrap the parameter as a YAML node.\n  virtual Expected<YAML::Node> wrap() = 0;\n\n  // Returns true if the parameter is set\n  virtual bool isAvailable() const = 0;\n\n  // Returns true if it is possible to change this parameter\n  bool isImmutable() const {\n    if (isConstant()) {\n      const bool is_active = false;  // FIXME(v1) Check that component is not active.\n      if (is_active) {\n        return true;\n      }\n    }\n    return false;\n  }\n\n  // FIXME(v1) make private\n  gxf_context_t context_{nullptr};\n  gxf_uid_t uid_{kNullUid};\n  gxf_parameter_flags_t flags_{0};\n  bool is_dynamic_{false};\n  const char* key_{nullptr};\n  const char* headline_{nullptr};\n  const char* description_{nullptr};\n};\n\ntemplate <typename T>\nclass Parameter;\n\n// This class stores a parameter of a specific type in the backend.\ntemplate <typename T>\nclass ParameterBackend : public ParameterBackendBase {\n public:\n  void writeToFrontend() override;\n  Expected<void> parse(const YAML::Node& node, const std::string& prefix) override;\n  bool isAvailable() const override { return value_.has_value(); }\n\n  // Sets the parameter to the given value.\n  Expected<void> set(T value) {\n    // Make sure that the new value passes the validator\n    if (validator_&& !validator_(value)) { return Unexpected{GXF_PARAMETER_OUT_OF_RANGE}; }\n    // Don't allow modification of a parameter which is currently immutable\n    if (isImmutable()) { return Unexpected{GXF_PARAMETER_CAN_NOT_MODIFY_CONSTANT}; }\n    // Update the parameter value\n    value_ = std::move(value);\n    return Success;\n  }\n\n  Expected<YAML::Node> wrap() {\n    if (!value_) {\n      return Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n    }\n    return ParameterWrapper<T>::Wrap(context(), value_.value());\n  }\n\n  // Gets the current value of the parameter.\n  const Expected<T>& try_get() const { return value_; }\n\n  // FIXME(v1) make private\n  nvidia::gxf::Parameter<T>* frontend_ = nullptr;\n  std::function<bool(const T&)> validator_;\n  Expected<T> value_ = Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n};\n\n// An intermediate base class for parameters which store a handle.\nclass HandleParameterBackend : public ParameterBackendBase {\n public:\n  virtual ~HandleParameterBackend() = default;\n\n  // Gets the component ID of the handle.\n  virtual Expected<gxf_uid_t> get() const = 0;\n\n  // Sets the handle using just a component ID\n  virtual Expected<void> set(gxf_uid_t cid) = 0;\n};\n\n// A specialization of ParameterBackend<T> for handle types. It derives from the intermediate base\n// class HandleParameterBackend so that parameter backends of handle types all have a common base\n// class.\ntemplate <typename T>\nclass ParameterBackend<Handle<T>> : public HandleParameterBackend {\n public:\n  void writeToFrontend() override;\n  Expected<void> parse(const YAML::Node& node, const std::string& prefix) override;\n  bool isAvailable() const override {\n    return (value_.has_value()) && (value_ != Handle<T>::Unspecified());\n  }\n\n  Expected<YAML::Node> wrap() {\n    if (!value_ || value_ == Handle<T>::Unspecified()) {\n      return Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n    }\n\n    return ParameterWrapper<Handle<T>>::Wrap(context(), value_.value());\n  }\n\n  // Sets the handle using just a component ID\n  Expected<void> set(gxf_uid_t cid) override {\n    auto expected = Handle<T>::Create(context(), cid);\n    if (expected) {\n      value_ = expected.value();\n      return Success;\n    } else {\n      return ForwardError(expected);\n    }\n  }\n\n  // Gets the component ID of the handle.\n  Expected<gxf_uid_t> get() const override {\n    if (!value_) {\n      GXF_LOG_VERBOSE(\"Handle parameter with name '%s' is not initialized\", key());\n      return Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n    } else if (value_ == Handle<T>::Unspecified()) {\n      GXF_LOG_VERBOSE(\"Handle parameter with name '%s' is unspecified\", key());\n      return Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n    }\n    return value_->cid();\n  }\n\n  // Sets the parameter to the given value.\n  Expected<void> set(Handle<T> value) {\n    if (isImmutable()) { return Unexpected{GXF_PARAMETER_CAN_NOT_MODIFY_CONSTANT}; }\n    value_ = std::move(value);\n    return Success;\n  }\n\n  // Gets the current value of the parameter, return Unexpected if not\n  // set or if it's set to Handle<T>::Unspecified()\n  const Expected<Handle<T>>& try_get() const {\n    if (value_ == Handle<T>::Unspecified()) { return unspecified_handle_; }\n    return value_;\n  }\n\n  // FIXME(v1) make private\n  const Expected<Handle<T>> unspecified_handle_ = Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n  nvidia::gxf::Parameter<Handle<T>>* frontend_ = nullptr;\n  Expected<Handle<T>> value_ = Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n};\n\n\n// Common base class for specializations of Parameter<T>.\nclass ParameterBase {\n public:\n  virtual ~ParameterBase() = default;\n};\n\n// This type represents a parameter of a component and can be used in custom components. It\n// communicates with the backend to set and get parameters as configured.\ntemplate <typename T>\nclass Parameter : public ParameterBase {\n public:\n  Parameter() = default;\n\n  // enable copying\n  Parameter(const Parameter<T>& other)\n  : mutex_() {\n    std::unique_lock<std::mutex> lock(other.mutex_);\n    value_ = other.value_;\n    backend_ = other.backend_;\n  }\n  // Gets the current parameter value. Only valid if the parameter is marked as 'mandatory' in the\n  // parameter interface. Otherwise an assert will be raised.\n  const T& get() const {\n    std::unique_lock<std::mutex> lock(mutex_);\n    GXF_ASSERT(backend_ != nullptr, \"A parameter with type '%s' was not registered.\",\n               TypenameAsString<T>());\n    GXF_ASSERT(backend_->isMandatory(), \"Only mandatory parameters can be accessed with get(). \"\n               \"'%s' is not marked as mandatory\", backend_->key());\n    GXF_ASSERT(value_, \"Mandatory parameter '%s' was not set.\", backend_->key());\n    return value_.value();\n  }\n\n  // Convenience function for accessing a mandatory parameter.\n  operator const T&() const {\n    return get();\n  }\n\n  // Tries to get the parameter value. If the parameter is not set Unexpected is returned.\n  const Expected<T>& try_get() const {\n    std::unique_lock<std::mutex> lock(mutex_);\n    return value_;\n  }\n\n  // Sets the parameter to the given value.\n  Expected<void> set(T value) {\n    GXF_ASSERT(backend_ != nullptr, \"Parameter of type '%s' was not registered.\",\n               TypenameAsString<T>());\n    const auto result = backend_->set(value);\n    if (!result) {\n      return result;\n    }\n    value_ = std::move(value);\n    return Success;\n  }\n\n  // Sets the parameter to the given value, but does not notify the backend about the change.\n  // This function shall only be used by the ParameterBackend class.\n  void setWithoutPropagate(const T& value) {\n    std::unique_lock<std::mutex> lock(mutex_);\n    value_ = value;\n  }\n\n  // Connects this parameter frontend to the corresponding backend.\n  void connect(ParameterBackend<T>* backend) {\n    backend_ = backend;\n  }\n\n  const char* key() {\n    return backend_ == nullptr ? nullptr : backend_->key();\n  }\n\n private:\n  Expected<T> value_ = Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n  ParameterBackend<T>* backend_ = nullptr;\n  mutable std::mutex mutex_;\n};\n\n// A specialization of Parameter<T> for handle types.\ntemplate <typename S>\nclass Parameter<Handle<S>> : public ParameterBase {\n public:\n  // Gets the current parameter value. Only valid if the parameter is marked as 'mandatory' in the\n  // parameter interface. Otherwise an assert will be raised.\n  const Handle<S>& get() const {\n    GXF_ASSERT(backend_ != nullptr, \"A handle parameter with type '%s' was not registered.\",\n               TypenameAsString<S>());\n    GXF_ASSERT(backend_->isMandatory(), \"Only mandatory parameters can be accessed with get(). \"\n               \"'%s' is not marked as mandatory\", backend_->key());\n    GXF_ASSERT(value_, \"Mandatory parameter '%s' was not set.\", backend_->key());\n    GXF_ASSERT(value_ != Handle<S>::Unspecified(), \"Handle was created but not assigned.\"\n               \"Unspecified handles cannot be accessed.\");\n    return value_.value();\n  }\n\n  // Convenience function for accessing a mandatory parameter.\n  operator const Handle<S>&() const {\n    return get();\n  }\n\n  // Tries to get the parameter value. If the parameter is not set or set to\n  // Handle<S>::Unspecified()  is returned.\n  const Expected<Handle<S>>& try_get() const {\n    if (value_ == Handle<S>::Unspecified()) { return unspecified_handle_; }\n    return value_;\n  }\n\n  // Only if T = Handle<S>\n  S* operator->() const {\n    return get().get();\n  }\n\n  // Sets the parameter to the given value.\n  Expected<void> set(Handle<S> value) {\n    GXF_ASSERT(backend_ != nullptr,\n               \"Parameter corresponding to Handle of `%s' was not registered.\",\n               TypenameAsString<S>());\n    const auto result = backend_->set(value);\n    if (!result) {\n      return result;\n    }\n    value_ = std::move(value);\n    return Success;\n  }\n\n  // Sets the parameter to the given value, but does not notify the backend about the change.\n  // This function shall only be used by the ParameterBackend class.\n  void setWithoutPropagate(const Handle<S>& value) {\n    value_ = value;\n  }\n\n  // Connects this parameter frontend to the corresponding backend.\n  void connect(ParameterBackend<Handle<S>>* backend) {\n    backend_ = backend;\n  }\n\n  const char* key() {\n    return backend_ == nullptr ? nullptr : backend_->key();\n  }\n\n private:\n  // Used to return an Unexpected when the handle is not specified\n  const Expected<Handle<S>> unspecified_handle_ = Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n  Expected<Handle<S>> value_ = Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n  ParameterBackend<Handle<S>>* backend_ = nullptr;\n};\n\n// -------------------------------------------------------------------------------------------------\n\ntemplate <typename T>\nvoid ParameterBackend<T>::writeToFrontend() {\n  if (frontend_ && value_) {\n    frontend_->setWithoutPropagate(*value_);\n  }\n}\n\ntemplate <typename T>\nExpected<void> ParameterBackend<T>::parse(const YAML::Node& node, const std::string& prefix) {\n  return ParameterParser<T>::Parse(context(), uid(), key(), node, prefix)\n        .map([this] (const T& value) { return set(value); })\n        .and_then([this] { writeToFrontend(); });\n}\n\ntemplate <typename T>\nvoid ParameterBackend<Handle<T>>::writeToFrontend() {\n  if (frontend_ && value_) {\n    frontend_->setWithoutPropagate(*value_);\n  }\n}\n\ntemplate <typename T>\nExpected<void> ParameterBackend<Handle<T>>::parse(const YAML::Node& node,\n                                                  const std::string& prefix) {\n  return ParameterParser<Handle<T>>::Parse(context(), uid(), key(), node, prefix)\n        .map([this] (const Handle<T>& value) { return set(value); })\n        .and_then([this] { writeToFrontend(); });\n}\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CORE_PARAMETER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/parameter_parser.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_PARAMETER_PARSER_HPP_\n#define NVIDIA_GXF_STD_PARAMETER_PARSER_HPP_\n\n#include <sstream>\n#include <string>\n#include <type_traits>\n#include <utility>\n\n#include \"common/assert.hpp\"\n#include \"common/fixed_string.hpp\"\n#include \"common/fixed_vector.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/filepath.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/core/handle.hpp\"\n#include \"yaml-cpp/yaml.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\ntemplate <typename T, typename V = void>\nstruct ParameterParser;\n\n// Parses a parameter from YAML using the default YAML parser. This class can be specialized to\n// support custom types.\ntemplate <typename T>\nstruct ParameterParser<T> {\n  static Expected<T> Parse(gxf_context_t context, gxf_uid_t component_uid, const char* key,\n                           const YAML::Node& node, const std::string& prefix) {\n    if (!strcmp(key, \"int8\") || !strcmp(key, \"vector_int8\") || !strcmp(key, \"vector_2d_int8\")) {\n      GXF_LOG_WARNING(\"type %s is not supported\", key);\n      return Unexpected{GXF_PARAMETER_INVALID_TYPE};\n    } else {\n      try {\n        return node.as<T>();\n      } catch (...) {\n        std::stringstream ss;\n        ss << node;\n        GXF_LOG_ERROR(\"Could not parse parameter '%s' from '%s'\", key, ss.str().c_str());\n        return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n      }\n    }\n  }\n};\n\n// Specialization of ParameterParser for std::string. The full node is serialized to a string, even\n// though it might contain sub children.\ntemplate <>\nstruct ParameterParser<std::string> {\n  static Expected<std::string> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                     const char* key, const YAML::Node& node,\n                                     const std::string& prefix) {\n    try {\n      std::stringstream ss;\n      ss << node;\n      return ss.str();\n    } catch (...) {\n      std::stringstream ss;\n      ss << node;\n      GXF_LOG_ERROR(\"Could not parse parameter '%s' from '%s'\", key, ss.str().c_str());\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n  }\n};\n\ntemplate <>\nstruct ParameterParser<FilePath> {\n  static Expected<FilePath> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                  const char* key, const YAML::Node& node,\n                                  const std::string& prefix) {\n    try {\n      FilePath path;\n      std::stringstream ss;\n      ss << node;\n      path.assign(ss.str());\n      return path;\n    } catch (...) {\n      std::stringstream ss;\n      ss << node;\n      GXF_LOG_ERROR(\"Could not parse parameter '%s' from '%s'\", key, ss.str().c_str());\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n  }\n};\n\n// Specialization of ParameterParser for uint8_t because it is not supported natively by yaml-cpp\ntemplate <>\nstruct ParameterParser<uint8_t> {\n  static Expected<uint8_t> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                 const char* key, const YAML::Node& node,\n                                 const std::string& prefix) {\n    try {\n      return static_cast<uint8_t>(node.as<uint32_t>());\n    } catch (...) {\n      std::stringstream ss;\n      ss << node;\n      GXF_LOG_ERROR(\"Could not parse parameter '%s' from '%s'\", key, ss.str().c_str());\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n  }\n};\n\n// Specialization of ParameterParser for FixedString.\n// Substitutes std::string for safety-critical components.\ntemplate <size_t N>\nstruct ParameterParser<FixedString<N>> {\n  static Expected<FixedString<N>> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                        const char* key, const YAML::Node& node,\n                                        const std::string& prefix) {\n    return ParameterParser<std::string>::Parse(context, component_uid, key, node, prefix)\n        .map([&](std::string str) {\n          FixedString<N> string;\n          return string.copy(str.data(), str.size())\n              .substitute_error(GXF_EXCEEDING_PREALLOCATED_SIZE)\n              .substitute(string);\n        });\n  }\n};\n\n// Specialization of ParameterParser for FixedVector with stack allocation.\n// Substitutes std::array for safety-critical components.\n// TODO(ayusmans): parsing support for FixedVector with heap allocation\ntemplate <typename T, ssize_t N>\nstruct ParameterParser<FixedVector<T, N>> {\n  static Expected<FixedVector<T, N>> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                           const char* key, const YAML::Node& node,\n                                           const std::string& prefix) {\n    if (!node.IsSequence()) {\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    if (node.size() > N) {\n      GXF_LOG_ERROR(\"Parameter size (%zu) exceeds vector capacity (%zu)\", node.size(), N);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    FixedVector<T, N> vector;\n    for (size_t i = 0; i < node.size(); i++) {\n      const auto maybe = ParameterParser<T>::Parse(context, component_uid, key, node[i], prefix);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      vector.push_back(std::move(maybe.value()));\n    }\n    return vector;\n  }\n};\n\n// Specialization of ParameterParser for gxf::Handle. It parses the parameter as a string and\n// interprets it as either a component name in the current entity, or as a composed string of the\n// form 'entity_name/component_name'.\ntemplate <typename S>\nstruct ParameterParser<Handle<S>> {\n  static Expected<Handle<S>> Parse(gxf_context_t context, gxf_uid_t component_uid, const char* key,\n                                   const YAML::Node& node, const std::string& prefix) {\n    const char* component_name = \"UNKNOWN\";\n    auto code = GxfParameterGetStr(context, component_uid, kInternalNameParameterKey,\n                                   &component_name);\n    if (code != GXF_SUCCESS) { return Unexpected{code}; }\n\n    gxf_uid_t owner_eid, param_eid;\n    const char* owner_entity_name = \"UNKNOWN\";\n    std::string param_entity_name;\n    // Get the entity of this component\n    const gxf_result_t result_1 = GxfComponentEntity(context, component_uid, &owner_eid);\n    if (result_1 != GXF_SUCCESS) {\n      return Unexpected{result_1};\n    }\n\n    code = GxfEntityGetName(context, owner_eid, &owner_entity_name);\n    if (code != GXF_SUCCESS) { return Unexpected{code}; }\n\n    // Parse string from node\n    std::string tag;\n    try {\n      tag = node.as<std::string>();\n    } catch (...) {\n      std::stringstream ss;\n      ss << node;\n      GXF_LOG_ERROR(\"Could not parse parameter '%s' from '%s'\", key, ss.str().c_str());\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n\n    std::string handle_component_name;\n    const size_t pos = tag.find('/');\n    if (pos == std::string::npos) {\n      handle_component_name = tag;\n      // target param component and owner component reside in the same entity\n      param_eid = owner_eid;\n    } else {\n      handle_component_name = tag.substr(pos + 1);\n\n      // Get the entity\n      gxf_result_t result_1_with_prefix = GXF_FAILURE;\n      // Try using entity name with prefix\n      if (!prefix.empty()) {\n        param_entity_name = prefix + tag.substr(0, pos);\n        // param eid\n        result_1_with_prefix = GxfEntityFind(context, param_entity_name.c_str(), &param_eid);\n        if (result_1_with_prefix != GXF_SUCCESS) {\n          GXF_LOG_WARNING(\"Could not find entity (with prefix) '%s' while parsing parameter '%s' \"\n                          \"of component %s with id %zu\",\n                          param_entity_name.c_str(), key, component_name, component_uid);\n        }\n      }\n      // Try using entity name without prefix, if lookup with prefix failed\n      if (result_1_with_prefix != GXF_SUCCESS) {\n        param_entity_name = tag.substr(0, pos);\n        // param eid\n        const gxf_result_t result_1_no_prefix = GxfEntityFind(context,\n          param_entity_name.c_str(), &param_eid);\n        if (result_1_no_prefix != GXF_SUCCESS) {\n          GXF_LOG_ERROR(\"Could not find entity '%s' while parsing parameter '%s' of component %s\"\n                        \" with id %zu\",\n                        param_entity_name.c_str(), key, component_name, component_uid);\n          return Unexpected{result_1_no_prefix};\n        } else if (!prefix.empty()) {\n          GXF_LOG_WARNING(\"Found entity (without prefix) '%s' while parsing parameter '%s' \"\n                          \"of component '%s' with id %zu in a subgraph, however the approach is\"\n                          \" deprecated, please use prerequisites instead\",\n                          param_entity_name.c_str(), key, component_name, component_uid);\n        }\n      }\n    }\n\n    // Get the type id of the component we are are looking for.\n    gxf_tid_t tid;\n    const gxf_result_t result_2 = GxfComponentTypeId(context, TypenameAsString<S>(), &tid);\n    if (result_2 != GXF_SUCCESS) {\n      return Unexpected{result_2};\n    }\n\n    // Find the component in the indicated entity\n    // param eid\n    gxf_uid_t cid;\n    const gxf_result_t result_3 = GxfComponentFind(context, param_eid, tid,\n      handle_component_name.c_str(), nullptr, &cid);\n    if (result_3 != GXF_SUCCESS) {\n      if (handle_component_name == \"<Unspecified>\") {\n        GXF_LOG_DEBUG(\"Using an <Unspecified> handle in entity '%s' with id %zu while parsing \"\n        \"parameter '%s' of component '%s' with id %zu. This handle must be set to a valid component\"\n        \" before graph activation\",\n        owner_entity_name, owner_eid, key, component_name, component_uid);\n        return Handle<S>::Unspecified();\n      } else {\n        GXF_LOG_WARNING(\"Cannot find target paramter component[entity name: %s, component name: %s]\"\n          \" in type[%s] \"\n          \"for owner component[entity name: %s, component name: %s, cid: %ld], \"\n          \"during parsing its parameter[key: %s, value: %s]\",\n          param_entity_name.c_str(), handle_component_name.c_str(),  // param component info\n          TypenameAsString<S>(),  // expected param component type\n          owner_entity_name, component_name, component_uid,  // owner component info\n          key, tag.c_str());  // param key:value string presentation\n        // print help info\n        for (int offset = 0; ; offset++) {\n          auto code1 = GxfComponentFind(context, param_eid, GxfTidNull(),\n            handle_component_name.c_str(), &offset, &cid);\n          if (code1 == GXF_ENTITY_COMPONENT_NOT_FOUND) {\n            GXF_LOG_DEBUG(\"No more component instance found as entity/component: %s\",\n              tag.c_str());\n            break;\n          } else if (code1 != GXF_SUCCESS) {\n            GXF_LOG_ERROR(\"Failed to execute component cid find with eid: %ld, \"\n              \"component name: %s, offset: %d\", param_eid, handle_component_name.c_str(), offset);\n            return Unexpected{code1};\n          }\n          const char* component_type_name = nullptr;\n          auto code2 = GxfComponentTypeNameFromUID(context, cid, &component_type_name);\n          if (code2 != GXF_SUCCESS) {\n            GXF_LOG_ERROR(\"Failed to find component type name from cid [%ld]\", cid);\n            return Unexpected{code2};\n          }\n          GXF_LOG_WARNING(\"Found component[%s] in type[%s]; \"\n            \"however type[%s] is expected for \"\n            \"component[entity name: %s, component name: %s, key: %s]\",\n            tag.c_str(), component_type_name,\n            TypenameAsString<S>(), owner_entity_name, component_name, key);\n        }\n        // end help info print\n      }\n\n      return Unexpected{result_3};\n    }\n\n    return Handle<S>::Create(context, cid);\n  }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_PARAMETER_PARSER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/parameter_parser_std.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_PARAMETER_PARSER_STD_HPP_\n#define NVIDIA_GXF_STD_PARAMETER_PARSER_STD_HPP_\n\n#include <array>\n#include <sstream>\n#include <string>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"yaml-cpp/yaml.h\"\n\n#include \"common/assert.hpp\"\n#include \"common/yaml_parser.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/core/parameter_parser.hpp\"\n#include \"gxf/core/parameter_registrar.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Parses unknown many of known types.\ntemplate <typename T>\nstruct ParameterParser<std::vector<T>> {\n  static Expected<std::vector<T>> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                        const char* key, const YAML::Node& node,\n                                        const std::string& prefix) {\n    if (!node.IsSequence()) {\n      const char* component_name = \"UNKNOWN\";\n      GxfParameterGetStr(context, component_uid, kInternalNameParameterKey, &component_name);\n      GXF_LOG_ERROR(\"Parameter '%s' in component '%s' must be a vector\", key, component_name);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    std::vector<T> result(node.size());\n    for (size_t i = 0; i < node.size(); i++) {\n      const auto maybe = ParameterParser<T>::Parse(context, component_uid, key, node[i], prefix);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      result[i] = std::move(maybe.value());\n    }\n    return result;\n  }\n};\n\n// Parses known many of known types.\ntemplate <typename T, std::size_t N>\nstruct ParameterParser<std::array<T, N>> {\n  static Expected<std::array<T, N>> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                                const char* key, const YAML::Node& node,\n                                                const std::string& prefix) {\n    if (!node.IsSequence()) {\n      const char* component_name = \"UNKNOWN\";\n      GxfParameterGetStr(context, component_uid, kInternalNameParameterKey, &component_name);\n      GXF_LOG_ERROR(\"Parameter '%s' in component '%s' must be an array\", key, component_name);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    if (node.size() != N) {\n      GXF_LOG_ERROR(\"Length of parameter array (%zu) does not match required length (%zu)\",\n                    node.size(), N);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    std::array<T, N> result;\n    for (size_t i = 0; i < result.size(); i++) {\n      const auto maybe = ParameterParser<T>::Parse(context, component_uid, key, node[i], prefix);\n      if (!maybe) { return ForwardError(maybe); }\n      result[i] = std::move(maybe.value());\n    }\n    return result;\n  }\n};\n\n// Parameter support for std::unordered_map<std::string, T>\ntemplate <typename T>\nstruct ParameterParser<std::unordered_map<std::string, T>> {\n  static Expected<std::unordered_map<std::string, T>> Parse(gxf_context_t context,\n                                                            gxf_uid_t component_uid,\n                                                            const char* key, const YAML::Node& node,\n                                                            const std::string& prefix) {\n    if (!node.IsMap()) {\n      const char* component_name = \"UNKNOWN\";\n      GxfParameterGetStr(context, component_uid, kInternalNameParameterKey, &component_name);\n      GXF_LOG_ERROR(\"Parameter '%s' in component '%s' must be a map\", key, component_name);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    std::unordered_map<std::string, T> map;\n    for (const auto& iter : node) {\n      const std::string& key = iter.first.as<std::string>();\n      const YAML::Node& value = iter.second;\n      auto maybe = ParameterParser<T>::Parse(context, component_uid, key.c_str(), value, prefix);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      map[key] = std::move(maybe.value());\n    }\n    return map;\n  }\n};\n\n\ntemplate <typename T>\nstruct ParameterParser<std::complex<T>> {\n  static Expected<std::complex<T>> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                  const char* key, const YAML::Node& node,\n                                  const std::string& prefix) {\n    try {\n      return static_cast<std::complex<T>>(node.as<std::complex<T>>());\n    } catch (...) {\n      std::stringstream ss;\n      ss << node;\n      GXF_LOG_ERROR(\"Could not parse parameter '%s' from '%s'\", key, ss.str().c_str());\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n  }\n};\n\n\n// Template specializations for ParameterInfo to be used during\n// parameter registration in the extension\n\n// Specialized ParameterInfoOverride for parameters of type std::vector<T>\ntemplate <typename T>\nstruct ParameterInfoOverride<std::vector<T>> {\n  Expected<void> apply(ParameterRegistrar* registrar,\n                       ParameterRegistrar::ComponentParameterInfo& info) {\n    // Get the element info\n    ParameterInfoOverride<T> override;\n    ParameterRegistrar::ComponentParameterInfo element_info;\n    const auto result = override.apply(registrar, element_info);\n    if (!result) { return ForwardError(result); }\n    info.type = element_info.type;\n    info.is_arithmetic = element_info.is_arithmetic;\n    info.handle_tid = element_info.handle_tid;\n\n    // Fetch the shape of <T> and update it to the current ComponentParameterInfo\n    for (int32_t i = 0; i < element_info.rank; ++i) {\n      info.shape[i] = element_info.shape[i];\n    }\n    // A vector increases the rank by 1 and adds shape [-1] to <T>.\n    info.shape[element_info.rank] = -1;\n    info.rank = element_info.rank + 1;\n\n    return Success;\n  }\n};\n\n// Specialized ParameterInfoOverride for parameters of type std::array<T,N>\ntemplate <typename T, std::size_t N>\nstruct ParameterInfoOverride<std::array<T, N>> {\n  Expected<void> apply(ParameterRegistrar* registrar,\n                       ParameterRegistrar::ComponentParameterInfo& info) {\n    // Get the element info\n    ParameterInfoOverride<T> override;\n    ParameterRegistrar::ComponentParameterInfo element_info;\n    const auto result = override.apply(registrar, element_info);\n    if (!result) { return ForwardError(result); }\n    info.type = element_info.type;\n    info.is_arithmetic = element_info.is_arithmetic;\n    info.handle_tid = element_info.handle_tid;\n\n    // Fetch the shape of <T> and update it to the current ComponentParameterInfo\n    for (int32_t i = 0; i < element_info.rank; ++i) {\n      info.shape[i] = element_info.shape[i];\n    }\n    // An increases the rank by 1 and adds shape [N] to <T>.\n    info.shape[element_info.rank] = N;\n    info.rank = element_info.rank + 1;\n\n    return Success;\n  }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_PARAMETER_PARSER_STD_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/parameter_registrar.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_PARAMETER_REGISTRAR_HPP_\n#define NVIDIA_GXF_STD_PARAMETER_REGISTRAR_HPP_\n\n#include <complex>\n#include <map>\n#include <memory>\n#include <shared_mutex>  // NOLINT\n#include <string>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"common/memory_utils.hpp\"\n#include \"common/type_utils.hpp\"\n#include \"gxf/core/expected_macro.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/core/parameter.hpp\"\n#include \"gxf/core/type_registry.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// @brief Struct specifying parameters for registering parameter\ntemplate <typename T>\nstruct ParameterInfo {\n  // Key used to access the parameter. Required.\n  const char* key = nullptr;\n  // Brief description. Optional.\n  const char* headline = nullptr;\n  // Detailed description. Optional.\n  const char* description = nullptr;\n  // Applicable platform list separated by comma. Optional.\n  const char* platform_information = nullptr;\n\n  // Default value if not provided otherwise. Not applicable to Handle parameter. Optional.\n  Expected<T> value_default = Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n  // Minimal value, Maximum value and Minimal step for arithmetic types. Optional.\n  Expected<std::array<T, 3>> value_range = Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n\n  // Bit flags about properties like if it is required etc.\n  gxf_parameter_flags_t flags = GXF_PARAMETER_FLAGS_NONE;\n\n  // Max allowed is rank 8\n  static constexpr const int32_t kMaxRank = 8;\n  // Parameters can be tensors and this value indicates its rank. For a \"scalar\" rank is 0.\n  int32_t rank = 0;\n  // Parameters can be tensors and this is its shape. Only values up to rank must be set. If rank\n  // is 0 no value must be set. -1 can be used to indicate a dynamic size which is not fixed at\n  // compile time.\n  int32_t shape[kMaxRank] = {1};\n};\n\nclass Component;\n\ntemplate <typename T>\nstruct ParameterTypeTrait {\n  static constexpr const char* type_name = \"(custom)\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_CUSTOM;\n  static constexpr const bool is_arithmetic = false;\n};\n\ntemplate <typename T>\nstruct ParameterTypeTrait<Handle<T>> {\n  static constexpr const char* type_name = \"(handle)\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_HANDLE;\n  static constexpr const bool is_arithmetic = false;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<int8_t> {\n  static constexpr const char* type_name = \"Int8\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_INT8;\n  static constexpr const bool is_arithmetic = true;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<int16_t> {\n  static constexpr const char* type_name = \"Int16\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_INT16;\n  static constexpr const bool is_arithmetic = true;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<int32_t> {\n  static constexpr const char* type_name = \"Int32\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_INT32;\n  static constexpr const bool is_arithmetic = true;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<int64_t> {\n  static constexpr const char* type_name = \"Int64\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_INT64;\n  static constexpr const bool is_arithmetic = true;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<uint8_t> {\n  static constexpr const char* type_name = \"UInt8\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_UINT8;\n  static constexpr const bool is_arithmetic = true;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<uint16_t> {\n  static constexpr const char* type_name = \"UInt16\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_UINT16;\n  static constexpr const bool is_arithmetic = true;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<uint32_t> {\n  static constexpr const char* type_name = \"UInt32\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_UINT32;\n  static constexpr const bool is_arithmetic = true;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<uint64_t> {\n  static constexpr const char* type_name = \"UInt64\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_UINT64;\n  static constexpr const bool is_arithmetic = true;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<float> {\n  static constexpr const char* type_name = \"Float32\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_FLOAT32;\n  static constexpr const bool is_arithmetic = true;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<double> {\n  static constexpr const char* type_name = \"Float64\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_FLOAT64;\n  static constexpr const bool is_arithmetic = true;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<char*> {\n  static constexpr const char* type_name = \"String\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_STRING;\n  static constexpr const bool is_arithmetic = false;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<std::string> {\n  static constexpr const char* type_name = \"String\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_STRING;\n  static constexpr const bool is_arithmetic = false;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<bool> {\n  static constexpr const char* type_name = \"Boolean\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_BOOL;\n  static constexpr const bool is_arithmetic = false;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<FilePath> {\n  static constexpr const char* type_name = \"File\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_FILE;\n  static constexpr const bool is_arithmetic = false;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<std::complex<float>> {\n  static constexpr const char* type_name = \"Complex64\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_COMPLEX64;\n  static constexpr const bool is_arithmetic = true;\n};\n\ntemplate <>\nstruct ParameterTypeTrait<std::complex<double>> {\n  static constexpr const char* type_name = \"Complex128\";\n  static constexpr const gxf_parameter_type_t type = GXF_PARAMETER_TYPE_COMPLEX128;\n  static constexpr const bool is_arithmetic = true;\n};\n\n// Stores parameters for an GXF application\nclass ParameterRegistrar {\n public:\n  // Container for a single object of any type\n  // Contained object is dynamically allocated on the heap and properly destructed when finished\n  class TypeEraser {\n   public:\n    TypeEraser() = default;\n    TypeEraser(const TypeEraser& other) { *this = other; }\n    TypeEraser(TypeEraser&& other) { *this = std::move(other); }\n    template <typename T, EnableIf_t<!IsSame_v<Decay_t<T>, TypeEraser>, TypeEraser*> = nullptr>\n    TypeEraser(T&& value) { *this = std::forward<T>(value); }\n    ~TypeEraser() { reset(); }\n\n    TypeEraser& operator=(const TypeEraser& other) {\n      if (other.instance_) {\n        instance_ = other.instance_->clone();\n      }\n      return *this;\n    }\n\n    TypeEraser& operator=(TypeEraser&& other) {\n      instance_ = std::move(other.instance_);\n      return *this;\n    }\n\n    template <typename T>\n    EnableIf_t<!IsSame_v<Decay_t<T>, TypeEraser>, TypeEraser&> operator=(T&& value) {\n      instance_ = MakeUniqueNoThrow<storage_impl<Decay_t<T>>>(std::forward<T>(value));\n      return *this;\n    }\n\n    bool has_value() const { return static_cast<bool>(instance_); }\n    const void* get() const { return has_value() ? instance_->get() : nullptr; }\n    void reset() { instance_.reset(); }\n\n   private:\n    struct storage_base {\n      virtual ~storage_base() = default;\n      virtual const void* get() const = 0;\n      virtual std::unique_ptr<storage_base> clone() const = 0;\n    };\n\n    template <typename T>\n    struct storage_impl final : public storage_base {\n      template <typename... Args>\n      storage_impl(Args&&... args) : value(std::forward<Args>(args)...) {}\n\n      // FIXME: return c_str() directly for string types to avoid reinterpret_cast\n      const void* get() const override { return &value; }\n\n      std::unique_ptr<storage_base> clone() const override {\n        return MakeUniqueNoThrow<storage_impl<T>>(value);\n      }\n\n      T value;\n    };\n\n    std::unique_ptr<storage_base> instance_;\n  };\n\n  // Struct to hold information about a single parameter\n  struct ComponentParameterInfo {\n    std::string key;\n    std::string headline;\n    std::string description;\n\n    std::string platform_information;\n\n    gxf_parameter_type_t type;\n    gxf_tid_t handle_tid = GxfTidNull();\n    bool is_arithmetic;\n\n    gxf_parameter_flags_t flags;\n\n    // params to be accessed only via getDefaultValue(...)\n    // and getNumericRange(...) apis\n    TypeEraser default_value;\n    // Range info as [min, max, step]\n    std::array<TypeEraser, 3> value_range;\n\n    int32_t rank = 0;\n    int32_t shape[ParameterInfo<int32_t>::kMaxRank] = {0};\n  };\n\n  // Struct to hold information about all the parameters in a component\n  struct ComponentInfo {\n    std::string type_name;\n    std::vector<std::string> parameter_keys;\n    std::unordered_map<std::string, ComponentParameterInfo> parameters;\n  };\n\n  ParameterRegistrar() = default;\n\n  template <typename T>\n  Expected<void> registerComponentParameter(gxf_tid_t tid, const std::string& type_name,\n                                            const ParameterInfo<T>& parameter_info);\n\n  template<typename T>\n  Expected<std::vector<ParameterRegistrar::ComponentParameterInfo>>\n           getParametersOfType(gxf_tid_t tid);\n\n  // Add's a parameterless component to parameter registrar. These are useful to lookup component\n  // tid using component type name when parameters of type Handle<T> are being registered\n  void addParameterlessType(const gxf_tid_t tid, std::string type_name);\n\n  // Check if parameter registrar has a component\n  bool hasComponent(const gxf_tid_t tid) const;\n\n  // Get the number of parameters in a component\n  size_t componentParameterCount(const gxf_tid_t tid) const;\n\n  // Get the list of parameter keys in a component\n  Expected<void> getParameterKeys(const gxf_tid_t tid, const char** keys, size_t& count) const;\n\n  // Check if a component has a parameter\n  Expected<bool> componentHasParameter(const gxf_tid_t tid, const char* key) const;\n\n  // Get the default value of a component\n  // Depending on the data type of the parameter, default value has to be\n  // casted into the data type specified in gxf_parameter_type_t in gxf.h\n  Expected<const void*> getDefaultValue(const gxf_tid_t tid, const char* key) const;\n\n  // Load the numeric range of a parameter into gxf_parameter_info_t\n  // Depending on the data type of the parameter, numeric ranges value have to be\n  // casted into the data type specified in gxf_parameter_type_t in gxf.h\n  Expected<bool> getNumericRange(const gxf_tid_t tid, const char* key,\n                                 gxf_parameter_info_t* info) const;\n\n  // fills the gxf_parameter_info_t struct with the info stored in the parameter registrar\n  // for that specific component and parameter key\n  Expected<void> getParameterInfo(const gxf_tid_t tid, const char* key,\n                                  gxf_parameter_info_t* info) const;\n\n  // Returns the pointer to ComponentParameterInfo object in ComponentInfo\n  Expected<ComponentParameterInfo*> getComponentParameterInfoPtr(const gxf_tid_t tid,\n                                                                 const char* key) const;\n\n  // Get the tid of a component which has already been registered with the type registry,\n  // returns Unexpected if component not found\n  Expected<gxf_tid_t> tidFromTypename(std::string type_name) {\n    for (const auto& cparam : component_parameters) {\n      if (cparam.second->type_name == type_name) {\n        return cparam.first;\n      }\n    }\n    GXF_LOG_ERROR(\"Component type not found %s\", type_name.c_str());\n    return Unexpected{GXF_ENTITY_COMPONENT_NOT_FOUND};\n  }\n\n private:\n  // Verifies the info of a new parameter being registered and fills the info\n  // in the ComponentParameterInfo struct which is returned\n  Expected<void> registerComponentParameterImpl(gxf_tid_t tid, const std::string& type_name,\n                                                ComponentParameterInfo& info);\n\n  // Maintains a map of {tid : ComponentInfo}\n  // component type name in ComponentInfo is used to look up the corresponding tid when a parameter\n  // of handle<T> is used for that component\n  std::map<gxf_tid_t, std::unique_ptr<ComponentInfo>> component_parameters;\n};\n\n// loads numeric range info from ComponentParameterInfo to gxf_parameter_info_t\ntemplate <typename T>\nstatic bool getNumericRangeImpl(ParameterRegistrar::ComponentParameterInfo* ptr,\n                                gxf_parameter_info_t* info) {\n  static_assert(ParameterTypeTrait<T>::is_arithmetic);\n\n  if (!ptr || !info) {\n    return false;\n  }\n\n  info->numeric_min = nullptr;\n  info->numeric_max = nullptr;\n  info->numeric_step = nullptr;\n\n  if (std::get<0>(ptr->value_range).has_value()) {\n    info->numeric_min = std::get<0>(ptr->value_range).get();\n  }\n  if (std::get<1>(ptr->value_range).has_value()) {\n    info->numeric_max = std::get<1>(ptr->value_range).get();\n  }\n  if (std::get<2>(ptr->value_range).has_value()) {\n    info->numeric_step = std::get<2>(ptr->value_range).get();\n  }\n\n  return true;\n}\n\n// Provides a customizable step which can be used during parameter registration to modify\n// parameter info given to registry before it is written to the parameter info storage.\ntemplate <typename T>\nstruct ParameterInfoOverride {\n  Expected<void> apply(ParameterRegistrar* /*registrar*/,\n                       ParameterRegistrar::ComponentParameterInfo& info) {\n    info.type = ParameterTypeTrait<T>::type;\n    info.is_arithmetic = ParameterTypeTrait<T>::is_arithmetic;\n    info.handle_tid = GxfTidNull();\n    return Success;\n  }\n};\n\n// Specialized ParameterInfoOverride for parameters of type handle<T>\ntemplate <typename T>\nstruct ParameterInfoOverride<Handle<T>> {\n  Expected<void> apply(ParameterRegistrar* registrar,\n                       ParameterRegistrar::ComponentParameterInfo& info) {\n    info.type = GXF_PARAMETER_TYPE_HANDLE;\n    info.is_arithmetic = false;\n    auto handle = registrar->tidFromTypename(TypenameAsString<T>());\n    if (!handle) { return ForwardError(handle); }\n    info.handle_tid = handle.value();\n    return Success;\n  }\n};\n\ntemplate <typename T>\nExpected<void> ParameterRegistrar::registerComponentParameter(\n    gxf_tid_t tid, const std::string& type_name, const ParameterInfo<T>& registrar_info) {\n\n  ComponentParameterInfo info;\n\n  if (registrar_info.key == nullptr) {\n    return Unexpected{GXF_ARGUMENT_NULL};\n  }\n  info.key = std::string{registrar_info.key};\n\n  if (registrar_info.headline == nullptr) {\n    return Unexpected{GXF_ARGUMENT_NULL};\n  }\n  info.headline = std::string{registrar_info.headline};\n\n  if (registrar_info.description == nullptr) {\n    return Unexpected{GXF_ARGUMENT_NULL};\n  }\n  info.description = std::string{registrar_info.description};\n\n  // Handles platform information if present\n  if (registrar_info.platform_information != nullptr) {\n    info.platform_information = std::string{registrar_info.platform_information};\n  }\n\n  if (registrar_info.value_default) {\n    info.default_value = *registrar_info.value_default;\n  } else {\n    info.default_value.reset();\n  }\n\n  if (registrar_info.value_range) {\n    std::get<0>(info.value_range) = std::get<0>(*registrar_info.value_range);\n    std::get<1>(info.value_range) = std::get<1>(*registrar_info.value_range);\n    std::get<2>(info.value_range) = std::get<2>(*registrar_info.value_range);\n  } else {\n    std::get<0>(info.value_range).reset();\n    std::get<1>(info.value_range).reset();\n    std::get<2>(info.value_range).reset();\n  }\n\n  info.flags = registrar_info.flags;\n\n  // Set the rank and make sure the shape is correct.\n  info.rank = registrar_info.rank;\n  if (info.rank > ParameterInfo<T>::kMaxRank) {\n    return Unexpected{GXF_ARGUMENT_OUT_OF_RANGE};\n  }\n\n  for (int32_t i = 0; i < info.rank; i++) {\n    info.shape[i] = registrar_info.shape[i];\n  }\n  for (int32_t i = info.rank; i < ParameterInfo<T>::kMaxRank; i++) {\n    info.shape[i] = 1;\n  }\n\n  // Apply custom overrides\n  ParameterInfoOverride<T> custom_override;\n  const auto result = custom_override.apply(this, info);\n  if (!result) {\n    GXF_LOG_ERROR(\"Parameter Override failed for Component \\\"%s\\\" and Parameter \\\"%s\\\"\",\n                  type_name.c_str(), registrar_info.key);\n    return result;\n  }\n\n  // register the parameter\n  return registerComponentParameterImpl(tid, type_name, info);\n}\n\n// Provides a customizable step which can be used to query parameter info\ntemplate <typename T>\nstruct ParameterQueryOverride {\n  Expected<std::vector<ParameterRegistrar::ComponentParameterInfo>> query(\n                       ParameterRegistrar* registrar, gxf_tid_t tid) {\n    std::string type_name{TypenameAsString<T>()};\n    size_t count = registrar->componentParameterCount(tid);\n    const char* keys[count];\n    GXF_RETURN_IF_ERROR(registrar->getParameterKeys(tid, keys, count));\n    std::vector<ParameterRegistrar::ComponentParameterInfo> result;\n    for (size_t i = 0; i < count; ++i) {\n      auto info = GXF_UNWRAP_OR_RETURN(registrar->getComponentParameterInfoPtr(tid, keys[i]));\n      if (info->type == ParameterTypeTrait<T>::type) {\n        result.push_back(*info);\n      }\n    }\n    return result;\n  }\n};\n\n// Specialized ParameterInfoOverride for parameters of type handle<T>\ntemplate <typename T>\nstruct ParameterQueryOverride<Handle<T>> {\n  Expected<std::vector<ParameterRegistrar::ComponentParameterInfo>> query(\n                       ParameterRegistrar* registrar, gxf_tid_t tid) {\n    std::string type_name{TypenameAsString<T>()};\n    auto query_tid = GXF_UNWRAP_OR_RETURN(registrar->tidFromTypename(type_name));\n    size_t count = registrar->componentParameterCount(tid);\n    const char* keys[count];\n    GXF_RETURN_IF_ERROR(registrar->getParameterKeys(tid, keys, count));\n    std::vector<ParameterRegistrar::ComponentParameterInfo> result;\n    for (size_t i = 0; i < count; ++i) {\n      auto info = GXF_UNWRAP_OR_RETURN(registrar->getComponentParameterInfoPtr(tid, keys[i]));\n      if (info->type != GXF_PARAMETER_TYPE_HANDLE ) {continue;}\n      if (info->handle_tid == query_tid) {\n        result.push_back(*info);\n      }\n    }\n    return result;\n  }\n};\n\ntemplate<typename T>\nExpected<std::vector<ParameterRegistrar::ComponentParameterInfo>>\n  ParameterRegistrar::getParametersOfType(gxf_tid_t tid) {\n    ParameterQueryOverride<T> override;\n    return override.query(this, tid);\n  }\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_PARAMETER_REGISTRAR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/parameter_storage.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_PARAMETER_STORAGE_HPP_\n#define NVIDIA_GXF_STD_PARAMETER_STORAGE_HPP_\n\n#include <cinttypes>\n#include <map>\n#include <memory>\n#include <mutex>\n#include <shared_mutex>  // NOLINT\n#include <string>\n#include <utility>\n\n#include \"gxf/core/parameter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Stores all parameters for an GXF application\n//\n// Parameters are stored in a dictionary style class. For each parameter the actual parameter value\n// and various helper classes to query and set parameters are provided.\nclass ParameterStorage {\n public:\n  ParameterStorage(gxf_context_t context);\n\n  // Registers a parameter with the backend. This is used by components when they register their\n  // parameter interface to connect parameters in components to the backend.\n  template <typename T>\n  Expected<void> registerParameter(nvidia::gxf::Parameter<T>* frontend, gxf_uid_t uid,\n                                   const char* key, const char* headline, const char* description,\n                                   Expected<T> default_value, gxf_parameter_flags_t flags) {\n    if (frontend    == nullptr) { return Unexpected{GXF_ARGUMENT_NULL}; }\n    if (key         == nullptr) { return Unexpected{GXF_ARGUMENT_NULL}; }\n    if (headline    == nullptr) { return Unexpected{GXF_ARGUMENT_NULL}; }\n    if (description == nullptr) { return Unexpected{GXF_ARGUMENT_NULL}; }\n\n    std::unique_lock<std::shared_timed_mutex> lock(mutex_);\n\n    auto it = parameters_.find(uid);\n    if (it == parameters_.end()) {\n      std::map<std::string, std::unique_ptr<ParameterBackendBase>> tmp;\n      it = parameters_.insert({uid, std::move(tmp)}).first;\n    }\n\n    const auto jt = it->second.find(key);\n    if (jt != it->second.end()) { return Unexpected{GXF_PARAMETER_ALREADY_REGISTERED}; }\n\n    auto backend = std::make_unique<ParameterBackend<T>>();\n    backend->context_ = context_;\n    backend->uid_ = uid;\n    backend->flags_ = flags;\n    backend->is_dynamic_ = false;\n    backend->key_ = key;\n    backend->headline_ = headline;\n    backend->description_ = description;\n    backend->frontend_ = frontend;\n    // FIXME(v1) validator\n\n    frontend->connect(backend.get());\n\n    if (default_value) {\n      const auto code = backend->set(std::move(*default_value));\n      if (!code) { return ForwardError(code); }\n      backend->writeToFrontend();\n    }\n\n    it->second.insert({key, std::move(backend)}).first;\n\n    return Success;\n  }\n\n  Expected<void> parse(gxf_uid_t uid, const char* key, const YAML::Node& node,\n                       const std::string& prefix);\n\n  Expected<YAML::Node> wrap(gxf_uid_t uid, const char* key);\n\n  // Sets a parameter. If the parameter is not yet present a new dynamic parameter will be created.\n  // This function fails if a parameter already exists, but with the wrong type.\n  template <typename T>\n  Expected<void> set(gxf_uid_t uid, const char* key, T value) {\n    std::unique_lock<std::shared_timed_mutex> lock(mutex_);\n\n    GXF_LOG_VERBOSE(\"Setting parameter [%s] of type [%s] on uid [%\" PRId64 \"]\", key,\n                    TypenameAsString<T>(), uid);\n    auto it = parameters_.find(uid);\n    if (it == parameters_.end()) {\n      std::map<std::string, std::unique_ptr<ParameterBackendBase>> tmp;\n      it = parameters_.insert({uid, std::move(tmp)}).first;\n    }\n\n    auto jt = it->second.find(key);\n    if (jt == it->second.end()) {\n      auto ptr = std::make_unique<ParameterBackend<T>>();\n      ptr->context_ = context_;\n      ptr->uid_ = uid;\n      ptr->flags_ = GXF_PARAMETER_FLAGS_OPTIONAL | GXF_PARAMETER_FLAGS_DYNAMIC;\n      ptr->is_dynamic_ = true;\n      ptr->key_ = key;\n      ptr->headline_ = key;\n      ptr->description_ = \"N/A\";\n      jt = it->second.insert({key, std::move(ptr)}).first;\n    }\n\n    auto* ptr = dynamic_cast<ParameterBackend<T>*>(jt->second.get());\n    if (ptr == nullptr) {\n      GXF_LOG_ERROR(\"Attempting to set invalid parameter type for [%s] with type [%s]\",\n                    key, TypenameAsString<T>());\n      return Unexpected{GXF_PARAMETER_INVALID_TYPE};\n    }\n\n    const auto code = ptr->set(std::move(value));\n    if (!code) { return ForwardError(code); }\n\n    ptr->writeToFrontend();  // FIXME(v1) Special treatment for codelet parameters\n    return Success;\n  }\n\n  // Gets a parameter\n  template <typename T>\n  Expected<T> get(gxf_uid_t uid, const char* key) const {\n    std::shared_lock<std::shared_timed_mutex> lock(mutex_);\n    return getValuePointer<T>(uid, key).map([](const T* pointer) { return *pointer; });\n  }\n\n  // Sets a string parameter. The storage creates it's own internal copy and does not take ownership\n  // of the given pointer.\n  Expected<void> setStr(gxf_uid_t uid, const char* key, const char* value);\n\n  // Gets a string parameter. A pointer to the internal storage is returned whose contents may\n  // change if a call to setStr happens in the meantime.\n  Expected<const char*> getStr(gxf_uid_t uid, const char* key) const;\n\n  // Sets a file path parameter.\n  Expected<void> setPath(gxf_uid_t, const char* key, const char* value);\n\n  // Gets a file path parameter. A pointer to the internal storage is returned whose contents may\n  // change if a call to setPath happens in the meantime.\n  Expected<const char*> getPath(gxf_uid_t uid, const char* key) const;\n\n  // Sets a handle parameter.\n  Expected<void> setHandle(gxf_uid_t, const char* key, gxf_uid_t value);\n\n  // Gets a handle parameter.\n  Expected<gxf_uid_t> getHandle(gxf_uid_t uid, const char* key) const;\n\n  // Sets a vector of string parameter. The storage creates it's own internal copy and does not take\n  // ownership of the given pointer.\n  Expected<void> setStrVector(gxf_uid_t uid, const char* key, const char** value, uint64_t length);\n\n  // Adds the given value to a parameter and returns the result. The parameter is initialized to\n  // 0 in case it does not exist.\n  Expected<int64_t> addGetInt64(gxf_uid_t uid, const char* key, int64_t delta);\n\n  // Returns true if all mandatory parameters are available\n  Expected<void> isAvailable() const;\n\n  // Returns true if all mandatory parameters of a component are available\n  Expected<void> isAvailable(gxf_uid_t uid) const;\n\n  // Clean up data for specific Entity upon Entity destruction\n  Expected<void> clearEntityParameters(gxf_uid_t eid);\n\n private:\n  friend class Runtime;\n\n  // Finds a parameter and returns a pointer to its value.\n  template <typename T>\n  Expected<const T*> getValuePointer(gxf_uid_t uid, const char* key) const {\n    std::shared_lock<std::shared_timed_mutex> lock(mutex_);\n    auto backend = getBackendPointerImpl<ParameterBackend<T>>(uid, key);\n    if (!backend) { return ForwardError(backend); }\n    const auto& maybe = backend.value()->try_get();\n    if (!maybe) { return Unexpected{GXF_PARAMETER_NOT_INITIALIZED}; }\n    return &(*maybe);\n  }\n\n  // Finds a parameter backend and returns a pointer to it.\n  template <typename T>\n  Expected<const T*> getBackendPointerImpl(gxf_uid_t uid, const char* key) const {\n    const auto it = parameters_.find(uid);\n    if (it == parameters_.end()) { return Unexpected{GXF_PARAMETER_NOT_FOUND}; }\n    const auto jt = it->second.find(key);\n    if (jt == it->second.end()) { return Unexpected{GXF_PARAMETER_NOT_FOUND}; }\n\n    const T* ptr = dynamic_cast<const T*>(jt->second.get());\n    if (ptr == nullptr) { return Unexpected{GXF_PARAMETER_INVALID_TYPE}; }\n    return ptr;\n  }\n\n  // Finds a parameter backend and returns a pointer to it.\n  template <typename T>\n  Expected<T*> getBackendPointerImpl(gxf_uid_t uid, const char* key) {\n    const auto it = parameters_.find(uid);\n    if (it == parameters_.end()) { return Unexpected{GXF_PARAMETER_NOT_FOUND}; }\n    const auto jt = it->second.find(key);\n    if (jt == it->second.end()) { return Unexpected{GXF_PARAMETER_NOT_FOUND}; }\n\n    T* ptr = dynamic_cast<T*>(jt->second.get());\n    if (ptr == nullptr) { return Unexpected{GXF_PARAMETER_INVALID_TYPE}; }\n    return ptr;\n  }\n\n  mutable std::shared_timed_mutex mutex_;\n  gxf_context_t context_;\n  std::map<gxf_uid_t, std::map<std::string, std::unique_ptr<ParameterBackendBase>>> parameters_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_PARAMETER_STORAGE_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/parameter_wrapper.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_PARAMETER_WRAPPER_HPP_\n#define NVIDIA_GXF_STD_PARAMETER_WRAPPER_HPP_\n\n#include <complex>\n#include <string>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"common/yaml_parser.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/core/handle.hpp\"\n#include \"yaml-cpp/yaml.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\ntemplate <typename T, typename V = void>\nstruct ParameterWrapper;\n\ntemplate <typename T>\nstruct ParameterWrapper<T> {\n  // Wrap the value to a YAML::Node instance\n  static Expected<YAML::Node> Wrap(gxf_context_t context, const T& value) {\n    return YAML::Node(value);\n  }\n};\n\ntemplate <typename T>\nstruct ParameterWrapper<Handle<T>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context, const Handle<T>& value) {\n    std::string c_name = value.name();\n    const char *entity_name;\n    gxf_uid_t eid = kNullUid;\n    auto result = GxfComponentEntity(context, value.cid(), &eid);\n    if (result != GXF_SUCCESS) {\n      GXF_LOG_ERROR(\"Unable to find the entity for %s\", c_name.c_str());\n      return Unexpected{result};\n    }\n    result = GxfEntityGetName(context, eid, &entity_name);\n    if (result != GXF_SUCCESS) {\n      GXF_LOG_ERROR(\"Unable to get the entity name\");\n      return Unexpected{result};\n    }\n\n    std::string full_name = std::string(entity_name) + \"/\" + c_name;\n    return YAML::Node(full_name);\n  }\n};\n\ntemplate<typename T>\nstruct ParameterWrapper<std::vector<nvidia::gxf::Handle<T>>> {\n  static Expected<YAML::Node> Wrap(\n    gxf_context_t context,\n    const std::vector<nvidia::gxf::Handle<T>>& value) {\n      YAML::Node node(YAML::NodeType::Sequence);\n      for (auto &h : value) {\n        auto maybe = ParameterWrapper<Handle<T>>::Wrap(context, h);\n        if (!maybe) {\n          return Unexpected{maybe.error()};\n        }\n        node.push_back(maybe.value());\n      }\n    return node;\n  }\n};\n\ntemplate<typename T, size_t N>\nstruct ParameterWrapper<std::array<nvidia::gxf::Handle<T>, N>> {\n  static Expected<YAML::Node> Wrap(\n    gxf_context_t context,\n    const std::array<nvidia::gxf::Handle<T>, N>& value) {\n      YAML::Node node(YAML::NodeType::Sequence);\n      for (auto &h : value) {\n        auto maybe = ParameterWrapper<Handle<T>>::Wrap(context, h);\n        if (!maybe) {\n          return Unexpected{maybe.error()};\n        }\n        node.push_back(maybe.value());\n      }\n    return node;\n  }\n};\n\ntemplate<typename T>\nstruct ParameterWrapper<std::unordered_map<std::string, T>> {\n  static Expected<YAML::Node> Wrap(\n    gxf_context_t context,\n    const std::unordered_map<std::string, T>& map) {\n    YAML::Node node(YAML::NodeType::Map);\n    for (const auto& iter : map) {\n      const std::string& key = iter.first;\n      const T& value = iter.second;\n      auto maybe = ParameterWrapper<T>::Wrap(context, value);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node[key] = std::move(maybe.value());\n    }\n    return node;\n  }\n};\n\ntemplate<typename T, ssize_t N>\nstruct ParameterWrapper<nvidia::FixedVector<T, N>> {\n  static Expected<YAML::Node> Wrap(\n    gxf_context_t context,\n    const nvidia::FixedVector<T, N>& value) {\n      YAML::Node node(YAML::NodeType::Sequence);\n      for (size_t i = 0; i < value.size(); i++) {\n        auto v = value.at(i).value();\n        auto maybe = ParameterWrapper<T>::Wrap(context, v);\n        if (!maybe) {\n          return Unexpected{maybe.error()};\n        }\n        node.push_back(maybe.value());\n      }\n    return node;\n  }\n};\n\ntemplate<size_t N>\nstruct ParameterWrapper<nvidia::FixedString<N>> {\n  static Expected<YAML::Node> Wrap(\n    gxf_context_t context,\n    const nvidia::FixedString<N>& value) {\n      return ParameterWrapper<std::string>::Wrap(context, std::string(value.c_str()));\n  }\n};\n\ntemplate<typename T, ssize_t N>\nstruct ParameterWrapper<nvidia::FixedVector<nvidia::gxf::Handle<T>, N>> {\n  static Expected<YAML::Node> Wrap(\n    gxf_context_t context,\n    const nvidia::FixedVector<nvidia::gxf::Handle<T>, N>& value) {\n      YAML::Node node(YAML::NodeType::Sequence);\n      for (size_t i = 0; i < value.size(); i++) {\n        auto h = value.at(i).value();\n        auto maybe = ParameterWrapper<Handle<T>>::Wrap(context, h);\n        if (!maybe) {\n          return Unexpected{maybe.error()};\n        }\n        node.push_back(maybe.value());\n      }\n    return node;\n  }\n};\n\ntemplate<>\nstruct ParameterWrapper<nvidia::gxf::FilePath> {\n  static Expected<YAML::Node> Wrap(\n    gxf_context_t context,\n    const nvidia::gxf::FilePath& value) {\n    return ParameterWrapper<std::string>::Wrap(context, value);\n  }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/registrar.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CORE_REGISTRAR_HPP_\n#define NVIDIA_GXF_CORE_REGISTRAR_HPP_\n\n#include <map>\n#include <memory>\n#include <shared_mutex>  // NOLINT\n#include <string>\n#include <tuple>\n#include <utility>\n\n#include \"gxf/core/parameter.hpp\"\n#include \"gxf/core/parameter_registrar.hpp\"\n#include \"gxf/core/parameter_storage.hpp\"\n#include \"gxf/core/resource.hpp\"\n#include \"gxf/core/resource_manager.hpp\"\n#include \"gxf/core/resource_registrar.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Registers parameters and other information of components\nclass Registrar {\n public:\n  // Constant for registering an optional parameter with no default value\n  static constexpr Unexpected NoDefaultParameter() {\n    return Unexpected{GXF_PARAMETER_NOT_INITIALIZED};\n  }\n\n  Registrar() = default;\n\n  // Simplified version of 'parameter'.\n  template <typename T>\n  Expected<void> parameter(Parameter<T>& parameter, const char* key) {\n    return parameterImpl<T>(parameter, {key, key, \"N/A\"});\n  }\n\n  // Simplified version of 'parameter'.\n  template <typename T>\n  Expected<void> parameter(Parameter<T>& parameter, const char* key, const char* headline) {\n    return parameterImpl<T>(parameter, {key, headline, \"N/A\"});\n  }\n\n  // Simplified version of 'parameter'.\n  template <typename T>\n  Expected<void> parameter(Parameter<T>& parameter, const char* key, const char* headline,\n                           const char* description) {\n    return parameterImpl<T>(parameter, {key, headline, description});\n  }\n\n  // Simplified version of 'parameter'.\n  template <typename T>\n  Expected<void> parameter(Parameter<T>& parameter, const char* key, const char* headline,\n                           const char* description, const T& default_value) {\n    ParameterInfo<T> info;\n    info.key = key;\n    info.headline = headline;\n    info.description = description;\n    info.value_default = default_value;\n    return parameterImpl<T>(parameter, info);\n  }\n\n  // Register a component parameter. Every parameter must be registered in the function\n  // 'registerInterface'.\n  template <typename T>\n  Expected<void> parameter(Parameter<T>& parameter, const char* key, const char* headline,\n                           const char* description, const T& default_value,\n                           gxf_parameter_flags_t flags) {\n    ParameterInfo<T> info;\n    info.key = key;\n    info.headline = headline;\n    info.description = description;\n    info.value_default = default_value;\n    info.flags = flags;\n    return parameterImpl<T>(parameter, info);\n  }\n\n  // Register a component parameter. Every parameter must be registered in the function\n  // 'registerInterface'.\n  template <typename T>\n  Expected<void> parameter(Parameter<T>& parameter, const char* key, const char* headline,\n                           const char* description, Unexpected, gxf_parameter_flags_t flags) {\n    ParameterInfo<T> info;\n    info.key = key;\n    info.headline = headline;\n    info.description = description;\n    info.flags = flags;\n    return parameterImpl<T>(parameter, info);\n  }\n\n  // Registers with a struct of all info.\n  template <typename T>\n  Expected<void> parameter(nvidia::gxf::Parameter<T>& parameter,\n                           const ParameterInfo<T>& parameter_info) {\n    return parameterImpl<T>(parameter, parameter_info);\n  }\n\n  // Implementation for parameter() above that registers parameter in ParameterRegistrar and\n  // ParameterStorage.\n  // register component's Parameter class members. Two usages:\n  // 1. Register, maintain a map { comp_tid : parameter_info{tid} }\n  //    when Runtime call Component::registerInterface() in GxfRegisterComponent()\n  //    set parameter_registrar != nullptr && a mock parameter_storage\n  // 2. Populate ParameterStorage, maintain a map { comp_cid : backend_parameters }\n  //    create each backend_parameter, connect corresponding frontend_parameter\n  //    when Runtime call Component::registerInterface() in GxfComponentAdd()\n  //    set parameter_registrar == nullptr && parameter_storage != nullptr\n  template <typename T>\n  Expected<void> parameterImpl(nvidia::gxf::Parameter<T>& parameter,\n                               const ParameterInfo<T>& parameter_info) {\n    if (parameter_registrar != nullptr) {\n      Expected<void> result =\n          parameter_registrar->registerComponentParameter(tid, type_name, parameter_info);\n      if (!result) { return ForwardError(result); }\n    }\n\n    if (parameter_storage == nullptr) { return Unexpected{GXF_CONTEXT_INVALID}; }\n    return parameter_storage->registerParameter<T>(\n        &parameter, cid, parameter_info.key, parameter_info.headline, parameter_info.description,\n        parameter_info.value_default, parameter_info.flags);\n  }\n\n  // Registers a component with no parameters\n  Expected<void> registerParameterlessComponent() {\n    if (parameter_registrar != nullptr) parameter_registrar->addParameterlessType(tid, type_name);\n    return Success;\n  }\n\n  // register component's Resource class members. Two usages:\n  // 1. Register, maintain a map { comp_tid : resources_info{tid} }\n  //    when Runtime call Component::registerInterface() in GxfRegisterComponent()\n  //    set resource_registrar != nullptr && resource_manager = nullptr\n  // 2. Connect, assign ResourceManager ptr to each resource class member\n  //    when Runtime call Component::registerInterface() in GxfComponentAdd()\n  //    set resource_registrar == nullptr && resource_manager != nullptr\n  template <typename T>\n  Expected<void> resource(Resource<Handle<T>>& resource, const char* description) {\n    // only for first time Runtime call Component::registerInterface(),\n    // i.e. register by GxfRegisterComponent()\n    if (resource_registrar != nullptr) {\n      Expected<void> result =\n          resource_registrar->registerComponentResource<T>(tid, type_name.c_str(), description);\n      if (!result) { return ForwardError(result); }\n    }\n    // only for second time Runtime call Component::registerInterface(),\n    // i.e. allocate by GxfComponentAdd()\n    if (resource_manager != nullptr) {\n      // resource_manager is empty at this stage,\n      // but should be populated before resource value query\n      Expected<void> result1 = resource.connect(resource_manager, cid);\n      if (!result1) { return ForwardError(result1); }\n    }\n    return Success;\n  }\n\n  // Sets the mandatory parameter storage where parameters loaded from YAML are stored.\n  void setParameterStorage(std::shared_ptr<ParameterStorage> param_storage) {\n    parameter_storage = param_storage;\n  }\n\n  // Gets the mandatory parameter storage where parameters loaded from YAML are stored.\n  ParameterStorage* getParameterStorage() { return parameter_storage.get(); }\n\n  // Sets parameter registrar\n  void setParameterRegistrar(ParameterRegistrar* param_registrar) {\n    parameter_registrar = param_registrar;\n  }\n\n  // Gets the parameter registrar used to register the component interface\n  ParameterRegistrar* getParameterRegistrar() { return parameter_registrar; }\n\n  // Sets resource registrar\n  void setResourceRegistrar(std::shared_ptr<ResourceRegistrar> resource_registrar) {\n    this->resource_registrar = resource_registrar;\n  }\n\n  // Sets resource manager\n  void setResourceManager(std::shared_ptr<ResourceManager> resource_manager) {\n    this->resource_manager = resource_manager;\n  }\n\n  std::shared_ptr<ParameterStorage> parameter_storage = nullptr;\n\n  // Stores information about parameter for query\n  ParameterRegistrar* parameter_registrar = nullptr;\n\n  std::shared_ptr<ResourceRegistrar> resource_registrar = nullptr;\n\n  // have to stick to shared_ptr to avoid nullptr dereferencing risk,\n  // as Codelet's Resource try_get() are lazy call\n  std::shared_ptr<ResourceManager> resource_manager;\n\n  // The type id of registering component\n  gxf_tid_t tid;\n  // The instance uid of registering component\n  gxf_uid_t cid = 0;\n  // The typename of the registering component\n  std::string type_name;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CORE_REGISTRAR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/resource.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_CORE_RESOURCE_HPP_\n#define NVIDIA_GXF_CORE_RESOURCE_HPP_\n\n#include <memory>\n\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/core/resource_manager.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\ntemplate <typename T>\nclass Resource;\n\ntemplate <typename T>\nclass Resource<Handle<T>> {\n public:\n  // Tries to get the Resource value. If first time call, query ResourceManager and save\n  // the value for subsequent calls.\n  // If invalid ResourceManager ptr, Handle<S>::Unspecified() keeps returning every time\n  // If ResourceManager cannot find the target, GXF_ENTITY_COMPONENT_NOT_FOUND\n  // will be saved into value_. No repeat query.\n  const Expected<Handle<T>>& try_get(const char* name = nullptr) const {\n    if (value_ == Handle<T>::Unspecified()) {\n      if (resource_manager_ == nullptr) {\n        GXF_LOG_WARNING(\"Resource [type: %s] from component [cid: %ld] cannot get \"\n                        \"its value because of nullptr ResourceManager\",\n                        TypenameAsString<T>(), owner_cid_);\n        return unspecified_handle_;\n      } else {\n        Expected<Handle<T>> maybe_value =\n          resource_manager_->findComponentResource<T>(owner_cid_, name);\n        if (!maybe_value) {\n          GXF_LOG_DEBUG(\"Resource [type: %s] from component [cid: %ld] \"\n                          \"cannot find its value from ResourceManager\",\n                          TypenameAsString<T>(), owner_cid_);\n          value_ = ForwardError(maybe_value);\n          return value_;\n        }\n        value_ = maybe_value.value();\n      }\n    }\n    return value_;\n  }\n\n  Expected<void> connect(std::shared_ptr<ResourceManager> resource_manager, gxf_uid_t owner_cid) {\n    if (resource_manager == nullptr) {\n      GXF_LOG_WARNING(\"nullptr ResourceManager ptr passed to connect Resource: %s from cid: %ld\",\n                      TypenameAsString<T>(), owner_cid);\n    }\n    resource_manager_ = resource_manager;\n    owner_cid_ = owner_cid;\n    return Success;\n  }\n\n private:\n  gxf_uid_t owner_cid_ = 0;\n  std::shared_ptr<ResourceManager> resource_manager_;\n  mutable Expected<Handle<T>> value_ = Handle<T>::Unspecified();\n  // Used to return an Unexpected when the handle is not specified\n  const Expected<Handle<T>> unspecified_handle_ = Unexpected{GXF_RESOURCE_NOT_INITIALIZED};\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CORE_RESOURCE_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/resource_manager.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_GXF_STD_RESOURCE_MANAGER_HPP_\n#define NVIDIA_GXF_GXF_STD_RESOURCE_MANAGER_HPP_\n\n#include <utility>\n#include <vector>\n\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/core/handle.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/// @brief Provide lookup from eid & resource type to resource_cids\nclass ResourceManager {\n public:\n  ResourceManager(gxf_context_t context);\n  virtual ~ResourceManager() = default;\n\n  // Static public interface\n  static Expected<gxf_uid_t> findEntityResourceByTypeName(gxf_context_t context,\n                               gxf_uid_t eid, const char* type_name,\n                               const char* target_resource_name = nullptr);\n\n  // Static public interface\n  static Expected<gxf_uid_t> findComponentResourceByTypeName(gxf_context_t context,\n                               gxf_uid_t cid, const char* type_name,\n                               const char* target_resource_name = nullptr);\n\n\n  // Static public interface\n  // Find Resource of type Handle<T> belong to entity eid\n  template <typename T>\n  static Expected<Handle<T>> findEntityResource(gxf_context_t context, gxf_uid_t eid,\n                                            const char* target_resource_name = nullptr) {\n    const char* type_name = TypenameAsString<T>();\n    Expected<gxf_uid_t> maybe_cid = ResourceManager::findEntityResourceByTypeName(context,\n                                                       eid, type_name, target_resource_name);\n    if (!maybe_cid) { return ForwardError(maybe_cid); }\n    return Handle<T>::Create(context, maybe_cid.value());\n  }\n\n\n  // Find Resource of type Handle<T> belong to entity eid\n  template <typename T>\n  Expected<Handle<T>> findEntityResource(gxf_uid_t eid,\n                                         const char* target_resource_name = nullptr) {\n    // object member function calls class static function\n    return ResourceManager::findEntityResource<T>(context_, eid, target_resource_name);\n  }\n\n\n  // Find Resource of type Handle<T> belong to component cid\n  template <typename T>\n  Expected<Handle<T>> findComponentResource(gxf_uid_t cid,\n                                            const char* target_resource_name = nullptr) {\n    const char* type_name = TypenameAsString<T>();\n    Expected<gxf_uid_t> maybe_cid = ResourceManager::findComponentResourceByTypeName(context_,\n                                                       cid, type_name, target_resource_name);\n    if (!maybe_cid) { return ForwardError(maybe_cid); }\n    return Handle<T>::Create(context_, maybe_cid.value());\n  }\n\n private:\n  gxf_context_t context_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GXF_STD_RESOURCE_MANAGER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/resource_registrar.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_GXF_STD_RESOURCE_REGISTRAR_HPP_\n#define NVIDIA_GXF_GXF_STD_RESOURCE_REGISTRAR_HPP_\n\n#include <map>\n#include <memory>\n#include <string>\n\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/// @brief Maintains a map of {component_tid : ComponentInfo(Resources)}\nclass ResourceRegistrar {\n public:\n  ResourceRegistrar(gxf_context_t context) : context_(context) {}\n  // Struct to hold information about a single Resource\n  struct ComponentResourceInfo {\n    gxf_tid_t handle_tid = GxfTidNull();\n    std::string description;\n  };\n\n  // Struct to hold information about all the Resources in a component\n  struct ComponentInfo {\n    std::string type_name;\n    std::map<gxf_tid_t, ComponentResourceInfo> resources;\n  };\n\n  template <typename T>\n  Expected<void> registerComponentResource(gxf_tid_t comp_tid,\n                                           const char* comp_type_name,\n                                           const char* resource_description) {\n    auto it = component_resources_.find(comp_tid);\n    if (it == component_resources_.end()) {\n      component_resources_[comp_tid] = std::make_unique<ComponentInfo>();\n      it = component_resources_.find(comp_tid);\n    }\n    gxf_tid_t resource_tid;\n    gxf_result_t result = GxfComponentTypeId(context_, TypenameAsString<T>(), &resource_tid);\n    if (result != GXF_SUCCESS) {\n      GXF_LOG_ERROR(\"ResourceRegistrar: Runtime cannot \"\n                    \"find tid of resource [type name: %s]\", TypenameAsString<T>());\n      return Unexpected { result };\n    }\n    const auto jt = it->second->resources.find(resource_tid);\n    if (jt != it->second->resources.end()) {\n      return Unexpected{GXF_PARAMETER_ALREADY_REGISTERED};\n    }\n    ComponentResourceInfo info;\n    info.handle_tid = resource_tid;\n    info.description = std::string(resource_description);\n    it->second->type_name = comp_type_name;\n    it->second->resources[info.handle_tid] = info;\n\n    return Success;\n  }\n\n private:\n  gxf_context_t context_;\n  std::map<gxf_tid_t, std::unique_ptr<ComponentInfo>> component_resources_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GXF_STD_RESOURCE_REGISTRAR_HPP_\n\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/core/type_registry.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_TYPE_REGISTRY_HPP\n#define NVIDIA_GXF_STD_TYPE_REGISTRY_HPP\n\n#include <map>\n#include <set>\n#include <shared_mutex>  // NOLINT\n#include <string>\n\n#include \"gxf/core/expected.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// A type registry which maps a C++ type to it's name and which can be used to\n// track base classes.\nclass TypeRegistry {\n public:\n  Expected<void> add(gxf_tid_t tid, const char* component_type_name);\n\n  Expected<void> add_base(const char* component_type_name, const char* base_type_name);\n\n  Expected<gxf_tid_t> id_from_name(const char* component_type_name) const;\n\n  Expected<bool> is_base(gxf_tid_t derived, gxf_tid_t base) const;\n\n  Expected<const char*> name(gxf_tid_t tid) const;\n\n private:\n  std::map<gxf_tid_t, std::string> names_;\n  std::map<std::string, gxf_tid_t> tids_;\n  std::map<gxf_tid_t, std::set<gxf_tid_t>> bases_;\n  mutable std::shared_timed_mutex mutex_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/cuda_allocator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CUDA_ALLOCATOR_HPP\n#define NVIDIA_GXF_CUDA_ALLOCATOR_HPP\n\n#include <cuda_runtime.h>\n#include <string>\n\n#include \"gxf/std/allocator.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief Provides both synchronous and asynchronous allocation and deallocation of memory.\n */\nclass CudaAllocator : public Allocator {\n public:\n  virtual Expected<byte*> allocate_async(uint64_t size, cudaStream_t stream);\n  virtual Expected<void> free_async(byte* pointer, cudaStream_t stream);\n  virtual gxf_result_t allocate_async_abi(uint64_t, void**, cudaStream_t) = 0;\n  virtual gxf_result_t free_async_abi(void*, cudaStream_t) = 0;\n  virtual Expected<size_t> get_pool_size(MemoryStorageType type) const = 0;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CUDA_ALLOCATOR_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/cuda_buffer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CUDA_CUDA_BUFFER_HPP_\n#define NVIDIA_GXF_CUDA_CUDA_BUFFER_HPP_\n\n#include <atomic>\n#include <utility>\n\n#include \"common/byte.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/cuda/cuda_allocator.hpp\"\n#include \"gxf/cuda/cuda_common.hpp\"\n#include \"gxf/cuda/cuda_stream.hpp\"\n#include \"gxf/cuda/cuda_stream_pool.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/memory_buffer.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nconstexpr const char* kDefaultCudaBufferName = \"gxf_cuda_buffer\";\n\n/**\n * @brief A container for a single block of cuda memory with support for async memory allocations.\n *\n * Each CudaBuffer is type of MemoryBuffer associated with a CudaAllocator, CudaStreamPool &\n * CudaStream. Memory is allocated by resizing the buffer. Both cuda memory and the cuda stream is\n * released back into the pool when a cuda buffer object is destroyed.\n */\nclass CudaBuffer : public MemoryBuffer {\n public:\n  // state of the buffer\n  enum class State : int8_t {\n    UNSET = 0,                // Initial state not waiting for any cuda event callback\n    CALLBACK_REGISTERED,      // Indicates a call back function is registered\n    DATA_AVAILABLE,           // Indicates data is ready to be consumed\n  };\n\n  CudaBuffer() = default;\n  CudaBuffer(const CudaBuffer&) = delete;\n  CudaBuffer& operator=(const CudaBuffer&) = delete;\n\n  CudaBuffer(CudaBuffer&& other) noexcept { *this = std::move(other); }\n\n  CudaBuffer& operator=(CudaBuffer&& other) noexcept {\n    if (this == &other) {\n      return *this;\n    }\n    state_ = other.state_.load();\n    release_func_ =  std::move(other.release_func_);\n    stream_ = std::move(other.stream_);\n    stream_owner_ = other.stream_owner_.load();\n    stream_pool_ = std::move(other.stream_pool_);\n\n    other.state_ = State::UNSET;\n    other.stream_owner_ = false;\n    other.release_func_ = nullptr;\n    return *this;\n  }\n\n  // callback function to release memory passed to the CudaBuffer\n  using release_function_t = std::function<Expected<void> (void* pointer)>;\n\n  // Host callback function prototype\n  using callback_function_t = void(*)(void*);\n\n  static void CUDART_CB cudaHostCallback(void* buffer_ptr) {\n    auto buffer = reinterpret_cast<CudaBuffer*>(buffer_ptr);\n    GXF_LOG_VERBOSE(\"Received host callback from cuda stream for cuda buffer\");\n    auto cb_registered = State::CALLBACK_REGISTERED;\n    GXF_ASSERT_TRUE(buffer->state_.compare_exchange_strong(cb_registered, State::DATA_AVAILABLE));\n  }\n\n  Expected<void> freeBuffer() {\n    auto state = state_.load();\n    // If another callback is already registered, buffer should not be freed\n    GXF_ASSERT_NE(static_cast<int>(state), static_cast<int>(State::CALLBACK_REGISTERED));\n    state_ = State::UNSET;\n    if (release_func_ && pointer_) {\n      const Expected<void> result = release_func_(pointer_);\n      if (!result) {\n        return ForwardError(result);\n      }\n      release_func_ = nullptr;\n    }\n\n    return Success;\n  }\n\n  ~CudaBuffer() { freeBuffer(); }\n\n  // Resizes the cuda buffer to given size on custom cuda stream. A new stream is allocated\n  // from the stream pool if a pre allocated stream is not provided.\n  Expected<void> resizeAsync(Handle<CudaAllocator> allocator,\n                             Handle<CudaStreamPool> stream_pool,\n                             uint64_t size,\n                             Handle<CudaStream> stream_handle = Handle<CudaStream>::Null()) {\n    if (!allocator) {\n      GXF_LOG_ERROR(\"Invalid cuda allocator provided while resizing a cuda buffer\");\n      return Unexpected{GXF_ARGUMENT_INVALID};\n    }\n\n    if (!stream_pool) {\n      GXF_LOG_ERROR(\"Invalid cuda stream pool provided while resizing a cuda buffer\");\n      return Unexpected{GXF_ARGUMENT_INVALID};\n    }\n\n    const auto code = freeBuffer();\n    if (!code) {\n      GXF_LOG_ERROR(\"Failed to free memory. Error code: %s\", GxfResultStr(code.error()));\n      return ForwardError(code);\n    }\n\n    // Allocate new stream if its not pre-allocated\n    if (!stream_handle) {\n      stream_handle = GXF_UNWRAP_OR_RETURN(stream_pool->allocateStream());\n    }\n    auto stream = GXF_UNWRAP_OR_RETURN(stream_handle->stream());\n    auto result = GXF_UNWRAP_OR_RETURN(allocator->allocate_async(size, stream));\n\n    GXF_LOG_VERBOSE(\"Registering callback for cuda buffer\");\n    auto unset = State::UNSET;\n    GXF_ASSERT_TRUE(state_.compare_exchange_strong(unset, State::CALLBACK_REGISTERED));\n    cudaError_t cuda_result = cudaLaunchHostFunc(stream, cudaHostCallback, this);\n    CHECK_CUDA_ERROR(cuda_result,\n      \"Unable to register host function using cudaLaunchHostFunc\");\n\n    storage_type_ = MemoryStorageType::kDevice;\n    pointer_ = result;\n    size_ = size;\n\n    // TODO(chandrahasj) should allow custom stream to release the data ?\n    release_func_ = [allocator, stream_pool, stream_handle, this] (void *data) {\n      auto stream = stream_handle->stream().value();\n      allocator->free_async(reinterpret_cast<byte*>(data), stream);\n      if (this->stream_owner_.load()) {\n        // synchronize the stream before releasing it back into the pool\n        auto code = cudaStreamSynchronize(stream_handle->stream().value());\n        if (code != cudaSuccess) {\n          GXF_LOG_ERROR(\"Failed to synchronize cuda stream\");\n          return ExpectedOrCode(GXF_FAILURE);\n        }\n        return stream_pool->releaseStream(stream_handle);\n      }\n      return Success;\n    };\n\n    stream_ = stream_handle;\n    stream_pool_ = stream_pool;\n    return Success;\n  }\n\n  // Retrieves cuda buffer state\n  State state() const { return state_.load(); }\n\n  // Retrieves CudaStream\n  Handle<CudaStream> stream() {\n    return stream_;\n  }\n\n  // Retrieves CudaStreamPool\n  Handle<CudaStreamPool> streamPool() {\n    return stream_pool_;\n  }\n\n  // Ownership of CudaStream is transferred to the caller.\n  // Stream will not be released when this object is destroyed.\n  Handle<CudaStream> transferStreamOwnership() {\n    stream_owner_ = false;\n    return stream_;\n  }\n\n  // Register a host function callback on the cuda stream associated with the buffer\n  Expected<void> registerCallbackOnStream(callback_function_t func, void* data) {\n    if (!stream_) {\n      GXF_LOG_ERROR(\"CudaStream is not set for the CudaBuffer\");\n      return Unexpected{GXF_FAILURE};\n    }\n\n    auto stream = GXF_UNWRAP_OR_RETURN(stream_->stream());\n    GXF_LOG_VERBOSE(\"Registering callback for cuda buffer\");\n    cudaError_t cuda_result = cudaLaunchHostFunc(stream, func, data);\n    CHECK_CUDA_ERROR(cuda_result, \"Unable to register host function using cudaLaunchHostFunc\");\n    return Success;\n  }\n\n private:\n  // State of the buffer\n  std::atomic<State> state_{State::UNSET};\n  // Flag to keep track of shared ownership of the cuda stream\n  std::atomic<bool> stream_owner_{true};\n  // cuda host callback function on completion of all the queued work\n  release_function_t release_func_ = nullptr;\n  // CudaStream used to allocate memory for the cuda buffer\n  Handle<CudaStream> stream_ = Handle<CudaStream>::Null();\n  // CudaStreamPool used to allocate the CudaStream used by the buffer\n  Handle<CudaStreamPool> stream_pool_ = Handle<CudaStreamPool>::Null();\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CUDA_CUDA_BUFFER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/cuda_common.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CUDA_CUDA_COMMON_HPP_\n#define NVIDIA_GXF_CUDA_CUDA_COMMON_HPP_\n\n#include <cuda_runtime.h>\n\n#include \"common/assert.hpp\"\n#include \"common/logger.hpp\"\n\n#define CHECK_CUDA_ERROR(cu_result, fmt, ...)                                  \\\n    do {                                                                       \\\n        cudaError_t err = (cu_result);                                          \\\n        if (err != cudaSuccess) {                                               \\\n            GXF_LOG_ERROR(fmt \", cuda_error: %s, error_str: %s\", ##__VA_ARGS__, \\\n                cudaGetErrorName(err), cudaGetErrorString(err));                \\\n            return Unexpected{GXF_FAILURE};                                     \\\n        }                                                                       \\\n    } while (0)\n\n#define CHECK_CUDA_ERROR_RESULT(cu_result, fmt, ...)                      \\\n  do {                                                                    \\\n    cudaError_t err = (cu_result);                                        \\\n    if (err != cudaSuccess) {                                             \\\n      GXF_LOG_ERROR(fmt \", cuda_error: %s, error_str: %s\", ##__VA_ARGS__, \\\n                    cudaGetErrorName(err), cudaGetErrorString(err));      \\\n      return GXF_FAILURE;                                                 \\\n    }                                                                     \\\n  } while (0)\n\n#define CONTINUE_CUDA_ERROR(cu_result, fmt, ...)                               \\\n    do {                                                                       \\\n        cudaError_t err = (cu_result);                                          \\\n        if (err != cudaSuccess) {                                               \\\n            GXF_LOG_ERROR(fmt \", cuda_error: %s, error_str: %s\", ##__VA_ARGS__, \\\n                cudaGetErrorName(err), cudaGetErrorString(err));                \\\n        }                                                                       \\\n    } while (0)\n\n#endif  // NVIDIA_GXF_CUDA_CUDA_COMMON_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/cuda_event.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CUDA_CUDA_EVENT_HPP_\n#define NVIDIA_GXF_CUDA_CUDA_EVENT_HPP_\n\n#include <cuda_runtime.h>\n\n#include <memory>\n#include <utility>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/handle.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nclass CudaStream;\n\n/**\n * @brief Holds and provides access to cudaEvent_t. The event could be set via initWithEvent() or\n * via created by init(flags, dev_id).\n */\nclass CudaEvent {\n public:\n  CudaEvent() = default;\n  ~CudaEvent();\n\n  CudaEvent(const CudaEvent&) = delete;\n  void operator=(const CudaEvent&) = delete;\n  CudaEvent(CudaEvent&& other) {\n    resetInternal();\n    dev_id_ = other.dev_id_;\n    other.dev_id_ = -1;\n    event_ = std::move(other.event_);\n  }\n  void operator=(CudaEvent&& other) {\n    resetInternal();\n    dev_id_ = other.dev_id_;\n    other.dev_id_ = -1;\n    event_ = std::move(other.event_);\n  }\n\n  friend class CudaStream;\n\n  using EventDestroy = std::function<void(cudaEvent_t)>;\n\n  // Initialize an external event that shall be used\n  Expected<void> initWithEvent(cudaEvent_t event, int dev_id = -1, EventDestroy free_fnc = nullptr);\n  // Initialize a new event internally\n  Expected<void> init(uint32_t flags = 0, int dev_id = -1);\n  // Deinitialize cudaevent. In case that user does not called it explicitly,\n  // cuda event could be freed in destructor.\n  Expected<void> deinit();\n\n  // Retrieves cuda event\n  Expected<cudaEvent_t> event() const;\n  // Get device id which owns this stream\n  int dev_id() const { return dev_id_; }\n\n private:\n  template<class T>\n  using PtrT = std::unique_ptr<T, std::function<void(T*)>>;\n  using EventPtr = PtrT<cudaEvent_t>;\n\n  void resetInternal();\n\n  static Expected<CudaEvent::EventPtr> createEventInternal(uint32_t flags, int gpuid);\n  static Expected<CudaEvent::EventPtr> createEventInternal(\n    cudaEvent_t event, EventDestroy free_event);\n\n  int32_t dev_id_ = -1;\n  EventPtr event_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CUDA_CUDA_EVENT_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/cuda_scheduling_terms.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CUDA_CUDA_SCHEDULING_TERMS_HPP_\n#define NVIDIA_GXF_CUDA_CUDA_SCHEDULING_TERMS_HPP_\n\n#include <cuda_runtime.h>\n\n#include <atomic>\n#include <map>\n#include <string>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"gxf/core/parameter_registrar.hpp\"\n#include \"gxf/cuda/cuda_common.hpp\"\n#include \"gxf/cuda/cuda_event.hpp\"\n#include \"gxf/cuda/cuda_stream_id.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/scheduling_term.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief Scheduling term to monitor activity on a cuda stream\n *\n * A component which specifies the availability of data at the receiver on completion of\n * the work on the provided cuda stream with the help of callback function to host.\n * This scheduling term will register a call back function which will be called once the work on the\n * specified cuda stream completes indicating that the data is available for consumption\n */\nclass CudaStreamSchedulingTerm : public SchedulingTerm {\n public:\n  // state of the scheduling term\n  enum class State : int8_t {\n    UNSET = 0,                // Initial state not waiting for any cuda host callback\n    CALLBACK_REGISTERED,      // Indicates a call back function is registered\n    DATA_AVAILABLE,           // Indicates data is ready to be consumed\n  };\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t timestamp) override;\n  gxf_result_t update_state_abi(int64_t timestamp) override;\n\n private:\n  // Receiver to monitor cuda stream activity\n  Parameter<Handle<Receiver>> receiver_;\n  // The current state of the scheduling term\n  SchedulingConditionType current_state_;\n  // Current message entity being monitored\n  std::atomic<gxf_uid_t> message_eid_ = kNullUid;\n  // State of the scheduling term\n  std::atomic<State> state_{State::UNSET};\n  // cuda host callback function on completion of all the queued work\n  static void CUDART_CB cudaHostCallback(void* term_ptr);\n};\n\n/**\n * @brief Scheduling term based on cuda event\n *\n * A component which specifies the availability of data at the receiver on completion of the\n * work on the provided cuda stream with the help of cuda event.\n * This scheduling term will keep polling on the event provided to check for data availability for\n * consumption.\n */\nclass CudaEventSchedulingTerm : public SchedulingTerm {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t timestamp) override;\n  gxf_result_t update_state_abi(int64_t timestamp) override;\n\n private:\n  Parameter<Handle<Receiver>> receiver_;\n  Parameter<std::string> event_name_{};\n  SchedulingConditionType current_state_;   // The current state of the scheduling term\n  int64_t last_state_change_;               // timestamp when the state changed the last time\n};\n\n\n/**\n * @brief Scheduling term based on data availability in a cuda buffer\n *\n * A component which specifies the availability of data at the receiver based on\n * the cuda buffers present in incoming messages.\n */\nclass CudaBufferAvailableSchedulingTerm : public SchedulingTerm {\n public:\n  // state of the scheduling term\n  enum class State : int8_t {\n    UNSET = 0,                // Initial state not waiting for any cuda host callback\n    CALLBACK_REGISTERED,      // Indicates a call back function is registered\n    DATA_AVAILABLE,           // Indicates data is ready to be consumed\n  };\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t timestamp) override;\n  gxf_result_t update_state_abi(int64_t timestamp) override;\n\n private:\n  // Receiver to monitor cuda stream activity\n  Parameter<Handle<Receiver>> receiver_;\n  // The current state of the scheduling term\n  SchedulingConditionType current_state_;\n  // Current message entity being monitored\n  std::atomic<gxf_uid_t> message_eid_ = kNullUid;\n  // State of the scheduling term\n  std::atomic<State> state_{State::UNSET};\n  // timestamp when the state changed the last time\n  int64_t last_state_change_;\n  // cuda host callback function on completion of all the queued work\n  static void CUDART_CB cudaHostCallback(void* term_ptr);\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CUDA_CUDA_SCHEDULING_TERMS_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/cuda_stream.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CUDA_CUDA_STREAM_HPP_\n#define NVIDIA_GXF_CUDA_CUDA_STREAM_HPP_\n\n#include \"cuda.h\"\n#include <cuda_runtime.h>\n\n#include <functional>\n#include <mutex>\n#include <queue>\n#include <string>\n#include <vector>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/cuda/cuda_event.hpp\"\n\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief Holds and provides access to cudaStream_t. CudaStream is allocated and\n * recycled by CudaStreamPool\n */\nclass CudaStream {\n public:\n  CudaStream() = default;\n  ~CudaStream();\n\n  CudaStream(const CudaStream&) = delete;\n  CudaStream(CudaStream&&) = delete;\n  void operator=(const CudaStream&) = delete;\n\n  using EventDestroy = std::function<void(cudaEvent_t)>;\n  using SyncedCallback = std::function<void()>;\n\n  // Retrieves cudaSteam_t\n  Expected<cudaStream_t> stream() const;\n  // Get device id which owns this stream\n  int dev_id() const { return dev_id_; }\n\n  // Record event to extend Entity life until event synchronized.\n  Expected<void> record(Handle<CudaEvent> event, Entity input_entity,\n                        SyncedCallback synced_cb = nullptr);\n  // Record event on the stream for an async callback.\n  // The callback would be delayed until CudaStreamSync ticks.\n  // The Callback usually is used to recycle dependent resources.\n  // If record failed, callback would not be called. User need to check return results.\n  Expected<void> record(cudaEvent_t event, EventDestroy cb);\n\n  // Reset all events and callback all the hook functions to release resource.\n  Expected<void> resetEvents();\n\n  // Sync all streams, meanwhile clean all recorded events and callback recycle functions\n  Expected<void> syncStream();\n\n private:\n  friend class CudaStreamPool;\n  // Initialize new cuda stream if was not set by external\n  Expected<void> initialize(const std::string& nvtxIdentifier, uint32_t flags = 0, int dev_id = -1,\n                            int32_t priority = 0, CUgreenCtx green_ctx = nullptr);\n  Expected<void> deinitialize();\n\n  Expected<void> prepareResourceInternal(int dev_id);\n\n  Expected<void> recordEventInternal(cudaEvent_t e);\n  Expected<void> syncEventInternal(cudaEvent_t e);\n\n  Expected<void> resetEventsInternal(std::queue<CudaEvent::EventPtr>& q);\n\n  gxf_result_t loadDriverFunctions();\n\n  mutable std::shared_timed_mutex mutex_;\n  int dev_id_ = 0;\n  cudaStream_t stream_ = 0;\n\n  // store all recorded event with destroy functions.\n  std::queue<CudaEvent::EventPtr> recorded_event_queue_;\n  // event is defined for for synchronization of stream\n  CudaEvent::EventPtr sync_event_;\n\n  // Function to create green context stream\n  CUresult (*fnCuGreenCtxStreamCreate)(CUstream*, CUgreenCtx, unsigned int, int32_t) = nullptr;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CUDA_CUDA_STREAM_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/cuda_stream_id.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CUDA_CUDA_STREAM_ID_HPP_\n#define NVIDIA_GXF_CUDA_CUDA_STREAM_ID_HPP_\n\n#include \"gxf/core/gxf.h\"\n\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief The Structure indicates cuda stream component ID.\n *\n * Message entity carrying CudaStreamId indicates that Tensors will be or\n * has been processed by corresponding cuda stream. The handle could\n * be deduced by Handle<CudaStream>::Create(context, stream_cid).\n */\nstruct CudaStreamId {\n  // component id of CudaStream\n  gxf_uid_t stream_cid;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CUDA_CUDA_STREAM_ID_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/cuda_stream_pool.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CUDA_CUDA_STREAM_POOL_HPP_\n#define NVIDIA_GXF_CUDA_CUDA_STREAM_POOL_HPP_\n\n#include <cuda_runtime.h>\n\n#include <atomic>\n#include <memory>\n#include <mutex>\n#include <queue>\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/cuda/cuda_stream.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/cuda_green_context.hpp\"\n#include \"gxf/std/resources.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief A stream pool containing a specified number of CUDA streams on a single device.\n */\nclass CudaStreamPool : public Allocator {\n public:\n  CudaStreamPool() = default;\n  ~CudaStreamPool();\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t is_available_abi(uint64_t size) override;\n  gxf_result_t allocate_abi(uint64_t size, int32_t type, void** pointer) override;\n  gxf_result_t free_abi(void* pointer) override;\n  gxf_result_t deinitialize() override;\n\n  // Allocate a cudastream for other components\n  Expected<Handle<CudaStream>> allocateStream();\n  // Free a cudastream\n  Expected<void> releaseStream(Handle<CudaStream> stream);\n\n private:\n  Expected<Entity> createNewStreamEntity(const std::string& nvtx_identifier);\n  Expected<void> reserveStreams();\n\n  Resource<Handle<GPUDevice>> gpu_device_;\n  Parameter<uint32_t> stream_flags_;\n  Parameter<int32_t> stream_priority_;\n  Parameter<uint32_t> reserved_size_;\n  Parameter<uint32_t> max_size_;\n  Parameter<std::string> nvtx_identifier_;\n  Parameter<Handle<CudaGreenContext>> cuda_green_context_;\n\n  std::mutex mutex_;\n  // map of <entity_id, Entity>\n  std::unordered_map<gxf_uid_t, std::unique_ptr<Entity>> streams_;\n  std::queue<Entity> reserved_streams_;\n  CUgreenCtx green_ctx_ = nullptr;\n  std::atomic<AllocatorStage> stage_{AllocatorStage::kUninitialized};\n  int32_t dev_id_ = -1;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CUDA_CUDA_STREAM_POOL_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/cuda_stream_sync.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CUDA_CUDA_STREAM_SYNC_HPP_\n#define NVIDIA_GXF_CUDA_CUDA_STREAM_SYNC_HPP_\n\n#include <cuda_runtime.h>\n\n#include \"gxf/cuda/cuda_stream.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief Get all cuda streams from messages of receiver, synchronize these streams,\n * and forward messages to transmitter.\n */\nclass CudaStreamSync  : public Codelet {\n public:\n  virtual ~CudaStreamSync() = default;\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t tick() override;\n\n private:\n  Parameter<Handle<Receiver>> rx_;\n  Parameter<Handle<Transmitter>> tx_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CUDA_CUDA_STREAM_SYNC_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/stream_ordered_allocator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STREAM_ORDERED_ALLOCATOR_HPP_\n#define NVIDIA_GXF_STREAM_ORDERED_ALLOCATOR_HPP_\n\n#include <cuda_runtime.h>\n#include <cuda_runtime_api.h>\n\n#include <shared_mutex>\n#include <string>\n#include <unordered_map>\n\n#include \"common/assert.hpp\"\n#include \"common/logger.hpp\"\n#include \"gxf/core/parameter.hpp\"\n#include \"gxf/cuda/cuda_allocator.hpp\"\n#include \"gxf/std/resources.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief An allocator which allocates and frees device memory using a CUDA memory pool.\n *\n * It uses CUDA's Stream Ordered Memory Allocator (cudaMallocFromPoolAsync/cudaFreeAsync).\n * For synchronous allocate and free, it uses its own internally created CUDA stream.\n * For asynchronous allocate and free, the user provided stream is used.\n */\nclass StreamOrderedAllocator : public CudaAllocator {\n public:\n  StreamOrderedAllocator() = default;\n  ~StreamOrderedAllocator() = default;\n\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t is_available_abi(uint64_t size) override;\n  gxf_result_t allocate_abi(uint64_t size, int32_t storage_type, void** pointer) override;\n  gxf_result_t allocate_async_abi(uint64_t size, void** pointer,\n                                  cudaStream_t stream) override;\n  gxf_result_t free_async_abi(void* pointer, cudaStream_t stream) override;\n  gxf_result_t free_abi(void* pointer) override;\n\n  Expected<size_t> get_pool_size(MemoryStorageType type) const override;\n\n private:\n  Resource<Handle<GPUDevice>> gpu_device_;\n  Parameter<std::string> release_threshold_;\n  Parameter<std::string> device_memory_initial_size_;\n  Parameter<std::string> device_memory_max_size_;\n\n  mutable std::shared_mutex shared_mutex_;\n  std::unordered_map<void*, size_t> pool_map_;\n\n  AllocatorStage stage_{AllocatorStage::kUninitialized};\n  cudaStream_t stream_;\n  cudaMemPool_t memory_pool_;\n  cudaMemPoolProps pool_props_ = {};\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STREAM_ORDERED_ALLOCATOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/tests/convolution.h",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CUDA_TESTS_CONVOLUTION_HPP\n#define NVIDIA_GXF_CUDA_TESTS_CONVOLUTION_HPP\n\n#include <cuda.h>\n\n#define THREADS_PER_BLOCK 32\n#define THREADS_PER_BLOCK_1 (THREADS_PER_BLOCK - 1)\n\n// A simple GPU based 2d convolution operation\nextern \"C\" void convolveKernel(float *input, float *kernel,\n                               float *output, int width, int height,\n                               int kernelSize, cudaStream_t stream);\n\n#endif // NVIDIA_GXF_CUDA_TESTS_CONVOLUTION_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/tests/green_context_with_smid.h",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_CUDA_TESTS_GREEN_CONTEXT_WITH_SMID_HPP\n#define NVIDIA_GXF_CUDA_TESTS_GREEN_CONTEXT_WITH_SMID_HPP\n\n#include <cuda.h>\n#include <cuda_runtime.h>\n\n#include <vector>\n#include <set>\n\n#define THREADS_PER_BLOCK 32\n#define THREADS_PER_BLOCK_1 (THREADS_PER_BLOCK - 1)\n\n// Structure to hold thread information including SMID\nstruct SmidThreadInfo {\n    int thread_id;           // Global thread index\n    int block_id;            // Block index\n    int thread_in_block;     // Thread index within block\n    uint32_t smid;           // Streaming Multiprocessor ID\n    int block_dim_x;         // Block dimension x\n    int grid_dim_x;          // Grid dimension x\n};\n\n// CUDA kernel function declarations\nextern \"C\" {\n    // Main function to test SMID for every thread\n    void test_every_thread_smid(int num_blocks, int threads_per_block, cudaStream_t stream, SmidThreadInfo* h_thread_info);\n\n    // Function to analyze SMID results\n    int32_t analyze_smid_results(std::vector<SmidThreadInfo*> ptr_thread_info, int threads_per_block, int num_blocks, int num_streams);\n\n    // Function to test SMID with different configurations\n    int32_t test_smid_with_different_configurations(cudaStream_t* streams, int num_streams);\n\n    // Function to collect SM IDs from a single kernel launch\n    int32_t collect_smid_from_kernel(int num_blocks, int threads_per_block, cudaStream_t stream, std::set<uint32_t>& unique_smids);\n}\n\n#endif // NVIDIA_GXF_CUDA_TESTS_GREEN_CONTEXT_WITH_SMID_HPP"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/cuda/tests/test_cuda_helper.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CUDA_TESTS_TEST_CUDA_HELPER_HPP\n#define NVIDIA_GXF_CUDA_TESTS_TEST_CUDA_HELPER_HPP\n\n#include <cublas_v2.h>\n#include <algorithm>\n#include <limits>\n#include <mutex>\n#include <numeric>\n#include <queue>\n#include <random>\n#include <set>\n#include <string>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"convolution.h\"\n#include \"green_context_with_smid.h\"\n\n#include \"common/assert.hpp\"\n#include \"gxf/cuda/cuda_buffer.hpp\"\n#include \"gxf/cuda/cuda_common.hpp\"\n#include \"gxf/cuda/cuda_event.hpp\"\n#include \"gxf/cuda/cuda_stream.hpp\"\n#include \"gxf/cuda/cuda_stream_id.hpp\"\n#include \"gxf/cuda/cuda_stream_pool.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\n#define CHECK_CUBLUS_ERROR(cu_result, fmt, ...)                         \\\n  do {                                                                  \\\n    cublasStatus_t err = (cu_result);                                   \\\n    if (err != CUBLAS_STATUS_SUCCESS) {                                 \\\n      GXF_LOG_ERROR(fmt \", cublas_error: %d\", ##__VA_ARGS__, (int)err); \\\n      return Unexpected{GXF_FAILURE};                                   \\\n    }                                                                   \\\n  } while (0)\n\nnamespace nvidia {\nnamespace gxf {\nnamespace test {\nnamespace cuda {\n\nconstexpr static const char* kStreamName0 = \"CudaStream0\";\nconstexpr static int kDefaultDevId = 0;\nstatic const Shape kInitTensorShape{1024, 2048};\n\n// The base class with cuda stream utils\nclass StreamBasedOps : public Codelet {\n public:\n  static Expected<Handle<CudaStream>> getStream(Entity& message) {\n    auto stream_id = message.get<CudaStreamId>();\n    GXF_ASSERT(stream_id, \"failed to find cudastreamid\");\n    auto stream =\n        Handle<CudaStream>::Create(stream_id.value().context(), stream_id.value()->stream_cid);\n    GXF_ASSERT(stream, \"create cudastream from cid failed\");\n    GXF_ASSERT(stream.value(), \"cudastream handle is null\");\n    return stream;\n  }\n\n  static Expected<void> addStream(Entity& message, Handle<CudaStream>& stream,\n                                  const char* name = nullptr) {\n    auto stream_id = message.add<CudaStreamId>(name);\n    GXF_ASSERT(stream_id, \"failed to add cudastreamid\");\n    stream_id.value()->stream_cid = stream.cid();\n    GXF_ASSERT(stream_id.value()->stream_cid != kNullUid, \"stream_cid is null\");\n    return Success;\n  }\n\n  static Expected<Handle<Tensor>> addTensor(Entity& message, Handle<Allocator> pool,\n                                            const TensorDescription& description) {\n    GXF_ASSERT(pool, \"pool is not set\");\n    auto tensor = message.add<Tensor>(description.name.c_str());\n    GXF_ASSERT(tensor, \"failed to add message tensor\");\n    const uint64_t bytes_per_element = description.element_type == PrimitiveType::kCustom\n                                           ? description.bytes_per_element\n                                           : PrimitiveTypeSize(description.element_type);\n\n    auto result = tensor.value()->reshapeCustom(description.shape, description.element_type,\n                                                bytes_per_element, description.strides,\n                                                description.storage_type, pool);\n    GXF_ASSERT(result, \"reshape tensor:%s failed\", description.name.c_str());\n    return tensor;\n  }\n\n  Expected<Handle<CudaEvent>> addNewEvent(Entity& message, const char* name = nullptr) const {\n    GXF_ASSERT(opsEvent(), \"cuda stream ops event is not initialized\");\n\n    auto mabe_event = message.add<CudaEvent>(name);\n    GXF_ASSERT(mabe_event, \"failed to add cudaevent\");\n    auto& event = mabe_event.value();\n    auto ret = event->initWithEvent(ops_event_->event().value(), ops_event_->dev_id());\n    GXF_ASSERT(ret, \"failed to init with event into message\");\n    GXF_ASSERT(event->event(), \"stream_cid is null\");\n    return mabe_event;\n  }\n\n  Expected<void> initOpsEvent() {\n    if (ops_event_) { return Success; }\n    auto ops_event_entity = Entity::New(context());\n    GXF_ASSERT(ops_event_entity, \"New event entity failed\");\n    ops_event_holder_ = ops_event_entity.value();\n    auto maybe_event = ops_event_holder_.add<CudaEvent>(\"ops_event\");\n    GXF_ASSERT(maybe_event, \"failed to init ops cudaevent\");\n    ops_event_ = maybe_event.value();\n    auto ret = ops_event_->init(0, kDefaultDevId);\n    if (!ret) {\n      GXF_LOG_ERROR(\"failed to init Ops event\");\n      return ForwardError(ret);\n    }\n    return ret;\n  }\n\n  const Handle<CudaEvent>& opsEvent() const { return ops_event_; }\n\n private:\n  Handle<CudaEvent> ops_event_;\n  Entity ops_event_holder_;\n};\n\n// Generate cuda tensor map with cudastreams for transmitter cuda_tx,\n// Generate host tensor map for transmitter host_tx\nclass StreamTensorGenerator : public StreamBasedOps {\n public:\n  gxf_result_t initialize() override {\n    GXF_ASSERT(stream_pool_.get(), \"stream pool is not set\");\n    auto stream = stream_pool_->allocateStream();\n    GXF_ASSERT(stream, \"allocating stream failed\");\n    stream_ = std::move(stream.value());\n    GXF_ASSERT(stream_->stream(), \"allocated stream is not initialized.\");\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t deinitialize() override {\n    auto result = stream_pool_->releaseStream(stream_);\n    return ToResultCode(result);\n  }\n\n  gxf_result_t tick() override {\n    Expected<Entity> maybe_dev_msg = Entity::New(context());\n    GXF_ASSERT(maybe_dev_msg, \"New dev message failed\");\n    auto& dev_msg = maybe_dev_msg.value();\n\n    Expected<Entity> maybe_host_msg = Entity::New(context());\n    GXF_ASSERT(maybe_host_msg, \"New host message failed\");\n    auto& host_msg = maybe_host_msg.value();\n\n    auto ret = createTensors(dev_msg, host_msg);\n    GXF_ASSERT(ret, \"creating tensors failed\");\n    GXF_ASSERT(stream_, \"stream is not allocated\");\n\n    ret = addStream(dev_msg, stream_, kStreamName0);\n    GXF_ASSERT(ret, \"stream tensor generator adding stream failed\");\n\n    ret = cuda_tx_->publish(dev_msg);\n    if (!ret) {\n      GXF_LOG_ERROR(\"stream tensor generator publishing cuda tensors failed\");\n      return ToResultCode(ret);\n    }\n\n    if (host_tx_.get()) {\n      ret = host_tx_->publish(host_msg);\n      if (!ret) {\n        GXF_LOG_ERROR(\"stream tensor generator publishing cuda tensors failed\");\n        return ToResultCode(ret);\n      }\n    }\n\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(cuda_tx_, \"cuda_tx\", \"transmitter of cuda tensors\", \"\");\n    result &= registrar->parameter(host_tx_, \"host_tx\", \"transmitter of host tensors\", \"\",\n                                   Handle<Transmitter>());\n    result &= registrar->parameter(cuda_tensor_pool_, \"cuda_tensor_pool\", \"Cuda Tensor Pool\", \"\");\n    result &= registrar->parameter(host_tensor_pool_, \"host_tensor_pool\", \"Host Tensor Pool\", \"\");\n    result &= registrar->parameter(stream_pool_, \"stream_pool\", \"Cuda Stream Pool\", \"\");\n    return ToResultCode(result);\n  }\n\n private:\n  Expected<void> createTensors(Entity& dev_msg, Entity& host_msg) {\n    Shape shape = kInitTensorShape;\n    for (size_t i = 0; i < 2; ++i) {\n      TensorDescription dev_desc{\"cuda_tensor\", MemoryStorageType::kDevice, shape,\n                                 PrimitiveType::kFloat32};\n      auto cuda_tensor_ret = addTensor(dev_msg, cuda_tensor_pool_, dev_desc);\n      GXF_ASSERT(cuda_tensor_ret, \"Generator dev message adding tensor failed.\");\n      auto& cuda_tensor = cuda_tensor_ret.value();\n      Expected<float*> cuda_data = cuda_tensor->data<float>();\n      if (!cuda_data) { return ForwardError(cuda_data); }\n\n      TensorDescription host_desc{\"host_tensor\", MemoryStorageType::kHost, shape,\n                                  PrimitiveType::kFloat32};\n      auto host_tensor_ret = addTensor(host_msg, host_tensor_pool_, host_desc);\n      GXF_ASSERT(host_tensor_ret, \"Generator host message adding tensor failed.\");\n      auto& host_tensor = host_tensor_ret.value();\n      Expected<float*> host_data = host_tensor->data<float>();\n      if (!host_data) { return ForwardError(host_data); }\n\n      for (size_t j = 0; j < shape.size(); ++j) {\n        host_data.value()[j] = (j % 100) + 1.0f;\n      }\n\n      cudaError_t error = cudaMemcpy(cuda_data.value(), host_data.value(), cuda_tensor->size(),\n                                     cudaMemcpyHostToDevice);\n      CHECK_CUDA_ERROR(error, \"StreamTensorGenerator cuda memory cpy H2D failed.\");\n    }\n\n    return Success;\n  }\n\n  Parameter<Handle<Transmitter>> cuda_tx_;\n  Parameter<Handle<Transmitter>> host_tx_;\n  Parameter<Handle<Allocator>> cuda_tensor_pool_;\n  Parameter<Handle<Allocator>> host_tensor_pool_;\n  Parameter<Handle<CudaStreamPool>> stream_pool_;\n\n  Handle<CudaStream> stream_;\n};\n\n// Dot product execution base class\nclass DotProductExe {\n private:\n  Handle<Receiver> rx_;\n  Handle<Transmitter> tx_;\n  Handle<Allocator> tensor_pool_;\n\n public:\n  DotProductExe() = default;\n  virtual ~DotProductExe() = default;\n  void setEnv(const Handle<Receiver>& rx, const Handle<Transmitter>& tx,\n              const Handle<Allocator>& pool) {\n    rx_ = rx;\n    tx_ = tx;\n    tensor_pool_ = pool;\n  }\n\n  virtual Expected<void> dotproduct_i(float* in0, float* in1, float* out, int32_t row,\n                                      int32_t column, Entity& in_msg, Entity& out_msg) = 0;\n\n  Expected<void> execute(const char* out_tensor_name = \"\") {\n    GXF_ASSERT(rx_ && tx_ && tensor_pool_, \"dotproduct received empty in_msg\");\n\n    Expected<Entity> in_msg = rx_->receive();\n    GXF_ASSERT(in_msg, \"dotproduct received empty in_msg\");\n\n    // get tensors\n    auto in_tensors = in_msg->findAll<Tensor>();\n    GXF_ASSERT(in_tensors, \"failed to find Tensors in in_msg\");\n    GXF_ASSERT(in_tensors->size() == 2, \"doesn't find Tensors in in_msg\");\n    GXF_ASSERT(in_tensors->at(0).value()->rank() == 2, \"Input tensor rank is not 2\");\n    int32_t column = in_tensors->at(0).value()->shape().dimension(1);\n    int32_t row = in_tensors->at(0).value()->shape().dimension(0);\n    MemoryStorageType mem_type = in_tensors->at(0).value()->storage_type();\n    PrimitiveType data_type = in_tensors->at(0).value()->element_type();\n    float* in_data[2] = {nullptr};\n    for (size_t i = 0; i < in_tensors->size(); ++i) {\n      GXF_ASSERT(in_tensors->at(i).value(), \"Input Tensor Handle is empty\");\n      GXF_ASSERT(in_tensors->at(i).value()->rank() == 2, \"Input tensor rank is not 2\");\n      in_data[i] = ValuePointer<float>(in_tensors->at(i).value()->pointer());\n    }\n\n    Expected<Entity> output = Entity::New(in_msg->context());\n    GXF_ASSERT(output, \"Creating dotproduct output tensor failed.\");\n    Shape out_shape{row};\n    GXF_ASSERT(out_shape.rank() == 1 && out_shape.size() == static_cast<uint64_t>(row),\n               \"output_shape is not correct\");\n    TensorDescription outDesc{out_tensor_name, mem_type, out_shape, data_type};\n    auto out_tensor = StreamBasedOps::addTensor(output.value(), tensor_pool_, outDesc);\n    GXF_ASSERT(out_tensor && out_tensor.value(), \"cuda dotproduct output tensor is not found\");\n    float* out_data = ValuePointer<float>(out_tensor.value()->pointer());\n\n    auto ret =\n        dotproduct_i(in_data[0], in_data[1], out_data, row, column, in_msg.value(), output.value());\n    GXF_ASSERT(ret, \"dotproduct execute with implementation failed\");\n\n    ret = tx_->publish(output.value());\n    GXF_ASSERT(ret, \"dotproduct publishing tensors failed\");\n    return ret;\n  }\n};\n\n// Cublas Dot product Operators\nclass CublasDotProduct : public StreamBasedOps {\n public:\n  // Culblas dot production execution class\n  class CublasDotProductExe : public DotProductExe {\n   private:\n    cublasHandle_t handle_ = nullptr;\n    CublasDotProduct* codelet_ = nullptr;\n\n   public:\n    CublasDotProductExe(CublasDotProduct* codelet) : codelet_(codelet) {}\n    ~CublasDotProductExe() {\n      if (handle_) { cublasDestroy(handle_); }\n    }\n    Expected<void> dotproduct_i(float* in0, float* in1, float* out, int32_t row, int32_t column,\n                                Entity& in_msg, Entity& out_msg) override {\n      // locate stream\n      auto maybe_stream = StreamBasedOps::getStream(in_msg);\n      GXF_ASSERT(maybe_stream && maybe_stream.value(), \"get stream from in_msg failed\");\n      auto& stream = maybe_stream.value();\n      auto ret = StreamBasedOps::addStream(out_msg, stream);\n      GXF_ASSERT(ret, \"adding cudastream into dotproduct output message failed.\");\n\n      int gpu_id = stream->dev_id();\n      if (gpu_id >= 0) {\n        CHECK_CUDA_ERROR(cudaSetDevice(gpu_id), \"failed to set deviceid: %d\", gpu_id);\n      }\n\n      if (!handle_) {\n        CHECK_CUBLUS_ERROR(cublasCreate(&handle_), \"failed to create cublas handle\");\n      }\n      auto custream_id = stream->stream();\n      GXF_ASSERT(custream_id, \"cudastream id is invalid\");\n      CHECK_CUBLUS_ERROR(cublasSetStream(handle_, custream_id.value()), \"cublas set stream failed\");\n\n      for (int i = 0; i < row; ++i) {\n        CHECK_CUBLUS_ERROR(\n            cublasSdot(handle_, column, in0 + column * i, 1, in1 + column * i, 1, out + i),\n            \"cublasSdot failed on row :%d\", i);\n      }\n\n      auto maybe_event = codelet_->addNewEvent(out_msg, \"cudotproduct_event\");\n      GXF_ASSERT(maybe_event, \"failed to add cublas dot product event\");\n      // Recording this event here causes a leak\n      // ret = stream->record(maybe_event.value(), in_msg,\n      //                      []() { GXF_LOG_ERROR(\"cublas dotproduct event synced\"); });\n      // GXF_ASSERT(ret, \"cublas dotproduct record event failed\");\n      return ret;\n    }\n  };\n\n  CublasDotProduct() : exec_(this) {}\n\n  gxf_result_t initialize() override {\n    GXF_ASSERT(tensor_pool_.get() && rx_.get() && tx_.get(), \"params not set\");\n    exec_.setEnv(rx_.get(), tx_.get(), tensor_pool_.get());\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t start() override { return ToResultCode(initOpsEvent()); }\n\n  gxf_result_t tick() override {\n    auto ret = exec_.execute(\"cublasdotproduct_tensor\");\n    return ToResultCode(ret);\n  }\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(tx_, \"tx\", \"transmitter of tensors\", \"\");\n    result &= registrar->parameter(rx_, \"rx\", \"receiver of tensors\", \"\");\n    result &= registrar->parameter(tensor_pool_, \"tensor_pool\", \"Tensor Pool\", \"\");\n    return ToResultCode(result);\n  }\n\n private:\n  Parameter<Handle<Transmitter>> tx_;\n  Parameter<Handle<Receiver>> rx_;\n  Parameter<Handle<Allocator>> tensor_pool_;\n\n  CublasDotProductExe exec_;\n};\n\n// CPU Dot product Operators\nclass HostDotProduct : public Codelet {\n public:\n  // CPU dot production execution class\n  class HostDotProductExe : public DotProductExe {\n   public:\n    HostDotProductExe() = default;\n    ~HostDotProductExe() = default;\n    Expected<void> dotproduct_i(float* in0, float* in1, float* out, int32_t row, int32_t column,\n                                Entity& in_msg, Entity& out_msg) override {\n      for (int i = 0; i < row; ++i) {\n        float sum = 0.0;\n        float* x = in0 + column * i;\n        float* y = in1 + column * i;\n        for (int j = 0; j < column; ++j) { sum += x[j] * y[j]; }\n        out[i] = sum;\n      }\n      return Success;\n    }\n  };\n\n  gxf_result_t initialize() override {\n    GXF_ASSERT(tensor_pool_.get() && rx_.get() && tx_.get(), \"params not set\");\n    exec_.setEnv(rx_.get(), tx_.get(), tensor_pool_.get());\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t tick() override {\n    auto ret = exec_.execute(\"host_dotproduct_tensor\");\n    return ToResultCode(ret);\n  }\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(tx_, \"tx\", \"transmitter of tensors\", \"\");\n    result &= registrar->parameter(rx_, \"rx\", \"receiver of tensors\", \"\");\n    result &= registrar->parameter(tensor_pool_, \"tensor_pool\", \"Tensor Pool\", \"\");\n    return ToResultCode(result);\n  }\n\n private:\n  Parameter<Handle<Transmitter>> tx_;\n  Parameter<Handle<Receiver>> rx_;\n  Parameter<Handle<Allocator>> tensor_pool_;\n\n  HostDotProductExe exec_;\n};\n\n// Stream based Memory copy from device to host\nclass MemCpy2Host : public StreamBasedOps {\n public:\n  gxf_result_t start() override { return ToResultCode(initOpsEvent()); }\n\n  gxf_result_t tick() override {\n    auto in = rx_->receive();\n    GXF_ASSERT(in, \"rx received empty message\");\n    auto maybe_tensor = in.value().get<Tensor>();\n    GXF_ASSERT(maybe_tensor, \"tensor not found\");\n    auto& in_tensor = maybe_tensor.value();\n    byte* in_data = in_tensor->pointer();\n\n    Expected<Entity> out_msg = Entity::New(context());\n    TensorDescription out_desc{in_tensor.name(), MemoryStorageType::kHost, in_tensor->shape(),\n                               in_tensor->element_type()};\n    auto maybe_out_tensor = addTensor(out_msg.value(), tensor_pool_, out_desc);\n    GXF_ASSERT(maybe_out_tensor, \"Memcpy host message adding tensor failed.\");\n    auto& out_tensor = maybe_out_tensor.value();\n    byte* out_data = out_tensor->pointer();\n\n    auto maybe_stream = getStream(in.value());\n    GXF_ASSERT(maybe_stream && maybe_stream.value(), \"get stream from in failed\");\n    auto& stream = maybe_stream.value();\n    auto ret = addStream(out_msg.value(), stream);\n    GXF_ASSERT(ret, \"adding cudastream into memcpy output message failed.\");\n\n    // wrap cuda operations since CHECK_CUDA_ERROR return Expected<void>\n    ret = [&, this]() -> Expected<void> {\n      int gpu_id = stream->dev_id();\n      if (gpu_id >= 0) {\n        CHECK_CUDA_ERROR(cudaSetDevice(gpu_id), \"failed to set deviceid: %d\", gpu_id);\n      }\n      cudaError_t error = cudaMemcpyAsync(out_data, in_data, in_tensor->size(),\n                                          cudaMemcpyDeviceToHost, stream->stream().value());\n      CHECK_CUDA_ERROR(error, \"CUDA memory cpy to host failed.\");\n      return Success;\n    }();\n    GXF_ASSERT(ret, \"CUDA memory cpy to host failed.\");\n\n    auto maybe_event = addNewEvent(out_msg.value(), \"memcpy_event\");\n    GXF_ASSERT(maybe_event, \"failed to add memcpy_event\");\n    // Recording this event here causes a leak\n    // ret = stream->record(maybe_event.value(), in.value(),\n    //                      []() { GXF_LOG_ERROR(\"memcpy_to_host event synced\"); });\n    // GXF_ASSERT(ret, \"memcpy_to_host record event failed\");\n\n    ret = tx_->publish(out_msg.value());\n    GXF_ASSERT(ret, \"memcpy_to_host publishing tensors failed\");\n\n    return ToResultCode(ret);\n  }\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(tx_, \"tx\", \"transmitter of tensors\", \"\");\n    result &= registrar->parameter(rx_, \"rx\", \"receiver of tensors\", \"\");\n    result &= registrar->parameter(tensor_pool_, \"tensor_pool\", \"Tensor output pool\", \"\");\n    return ToResultCode(result);\n  }\n\n private:\n  Parameter<Handle<Transmitter>> tx_;\n  Parameter<Handle<Receiver>> rx_;\n  Parameter<Handle<Allocator>> tensor_pool_;\n};\n\n// Equal verification\nclass VerifyEqual : public Codelet {\n public:\n  gxf_result_t tick() override {\n    GXF_LOG_DEBUG(\"verifying frame: %d\", count_++);\n    auto in0 = rx0_->receive();\n    GXF_ASSERT(in0, \"rx0 received empty message\");\n    auto in1 = rx1_->receive();\n    GXF_ASSERT(in1, \"rx1 received empty message\");\n\n    // get tensors\n    auto maybe_tensor0 = in0.value().get<Tensor>();\n    GXF_ASSERT(maybe_tensor0, \"tensor0 not found\");\n    auto maybe_tensor1 = in1.value().get<Tensor>();\n    GXF_ASSERT(maybe_tensor1, \"tensor1 not found\");\n    auto& tensor0 = maybe_tensor0.value();\n    auto& tensor1 = maybe_tensor1.value();\n    GXF_ASSERT(std::string(tensor0.name()) != std::string(tensor1.name()),\n               \"2 tensor name should not same\");\n    GXF_ASSERT(tensor0->shape() == tensor1->shape(), \"2 tensors' shape not matched\");\n    GXF_ASSERT(tensor0->element_type() == tensor1->element_type(),\n               \"2 tensor's element type not matched\");\n    GXF_ASSERT(tensor0->storage_type() == tensor1->storage_type(),\n               \"2 tensor's storage_type not matched\");\n    GXF_ASSERT(tensor0->storage_type() == MemoryStorageType::kHost,\n               \"very tensor storage_type is not from host\");\n\n    float* data0 = tensor0->data<float>().value();\n    float* data1 = tensor1->data<float>().value();\n    uint64_t count = tensor0->element_count();\n    for (uint64_t i = 0; i < count; ++i) {\n      // printf(\"i:%d, [%f], [%f]\\n\", static_cast<int>(i), data0[i], data1[i]);\n      GXF_ASSERT(fequal(data0[i], data1[i]), \"data0[%d]: %f but data1: %f.\", static_cast<int>(i),\n                 data0[i], data1[i]);\n    }\n\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(rx0_, \"rx0\", \"receiver0 of tensors\", \"\");\n    result &= registrar->parameter(rx1_, \"rx1\", \"receiver1 of tensors\", \"\");\n    return ToResultCode(result);\n  }\n\n private:\n  bool fequal(float a, float b) {\n    if (fabs(a - b) <= std::numeric_limits<float>::epsilon()) return true;\n    return false;\n  }\n  Parameter<Handle<Receiver>> rx0_;\n  Parameter<Handle<Receiver>> rx1_;\n  int32_t count_ = 0;\n};\n\n// Creates a CudaBuffer object on each iteration with memory allocated in an\n// asynchronous manner. Downstream receivers can either enqueue more work on to\n// CudaStream associated with the CudaBuffer or sync on the stream before accessing the memory\nclass CudaAsyncBufferGenerator : public Codelet {\n public:\n  virtual ~CudaAsyncBufferGenerator() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(signal_, \"signal\", \"Signal\",\n              \"Transmitter channel publishing messages to other graph entities\");\n    result &= registrar->parameter(allocator_, \"allocator\", \"Allocator\",\n              \"A cuda allocator component that can serve device memory asynchronously\");\n    result &= registrar->parameter(stream_pool_, \"stream_pool\", \"StreamPool\",\n              \"A cuda stream pool component\");\n    result &= registrar->parameter(size_, \"size\", \"Size\",\n              \"Size of the CudaBuffer to be allocated\", 1UL);\n    return ToResultCode(result);\n  }\n\n  gxf_result_t tick() override {\n    auto maybe_message = Entity::New(context());\n    if (!maybe_message) {\n      GXF_LOG_ERROR(\"Failure creating message entity.\");\n      return maybe_message.error();\n    }\n\n    auto message = maybe_message.value();\n    auto maybe_buffer = message.add<CudaBuffer>(kDefaultCudaBufferName);\n    if (!maybe_buffer) {\n      GXF_LOG_ERROR(\"Failure creating cuda buffer\");\n      return maybe_buffer.error();\n    }\n\n    auto buffer = maybe_buffer.value();\n    auto code = buffer->resizeAsync(allocator_, stream_pool_, size_);\n\n    auto min = std::numeric_limits<float>::min();\n    auto max = std::numeric_limits<float>::max();\n    std::uniform_real_distribution<float> distribution(min, max);\n    std::vector<float> elements;\n    auto element_count = size_ / sizeof(float);\n\n    for (size_t idx = 0; idx < element_count; idx++) {\n      elements.push_back(distribution(generator_));\n    }\n\n    auto stream_component = buffer->stream();\n    auto stream = stream_component->stream().value();\n\n    const cudaError_t error = cudaMemcpyAsync(buffer->pointer(), elements.data(),\n                                              size_, cudaMemcpyHostToDevice, stream);\n    CHECK_CUDA_ERROR_RESULT(error, \"cudaMemcpyAsync failed\");\n\n    auto result = signal_->publish(message);\n    GXF_LOG_INFO(\"Message Sent\");\n    return GXF_SUCCESS;\n  }\n\n private:\n  Parameter<Handle<Transmitter>> signal_;\n  Parameter<Handle<CudaAllocator>> allocator_;\n  Parameter<Handle<CudaStreamPool>> stream_pool_;\n  Parameter<size_t> size_;\n  std::default_random_engine generator_;\n};\n\n// Performs a green context test with detection of different SMIDs\nclass GreenContextWithSmid: public Codelet {\n public:\n  virtual ~GreenContextWithSmid() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(stream_pools_, \"stream_pools\", \"StreamPools\",\n              \"A cuda stream pool component\");\n    result &= registrar->parameter(num_stream_pools_, \"num_stream_pools\", \"NumStreamPools\",\n              \"Number of streams to allocate\", 1);\n    result &= registrar->parameter(input_, \"input\", \"Input\",\n              \"Receiver channel receiving messages from other graph entities\");\n    return ToResultCode(result);\n  }\n  gxf_result_t start() override {\n    cudaStream_t streams[10];\n    for (int i = 0; i < num_stream_pools_.get(); i++) {\n      auto stream = stream_pools_.get()[i].value()->allocateStream();\n      streams_.push_back(stream.value());\n      streams[i] = stream.value()->stream().value();\n    }\n    test_smid_with_different_configurations(&streams[0], num_stream_pools_.get());\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t stop() override {\n    for (int i = 0; i < num_stream_pools_.get(); i++) {\n      stream_pools_.get()[i].value()->releaseStream(streams_[i]);\n    }\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t tick() override {\n    // This test is done in start()\n    return GXF_SUCCESS;\n  }\n\n private:\n  Parameter<FixedVector<Handle<nvidia::gxf::CudaStreamPool>, kMaxComponents> > stream_pools_;\n  Parameter<int> num_stream_pools_;\n  Parameter<Handle<Receiver>> input_;\n\n  std::vector<Handle<CudaStream>> streams_;\n};\n\n// Performs an async 2D convolution operation on\n// incoming CudaBuffer objects.\nclass Convolve2D: public Codelet {\n public:\n  virtual ~Convolve2D() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(kernel_, \"kernel\", \"Kernel\",\n              \"2d matrix of float values represented as a vector of vectors\");\n    result &= registrar->parameter(output_, \"output\", \"Output\",\n              \"Transmitter channel publishing messages to other graph entities\");\n    result &= registrar->parameter(input_, \"input\", \"Input\",\n              \"Receiver channel receiving messages from other graph entities\");\n    result &= registrar->parameter(allocator_, \"allocator\", \"Allocator\",\n              \"A cuda allocator component that can serve device memory asynchronously\");\n    return ToResultCode(result);\n  }\n\n  gxf_result_t start() override {\n    auto& kernel = kernel_.get();\n    size_t kernel_size = kernel.size();\n\n    size_t index = 0;\n    float kernel_flat_host[kernel_size * kernel_size]; // NOLINT\n    for (const auto& k : kernel) {\n      GXF_ASSERT_EQ(k.size(), kernel_size);\n      for (const auto& value : k) {\n        kernel_flat_host[index] = value;\n        ++index;\n      }\n    }\n\n    auto maybe_kernel = allocator_->allocate(kernel_size * kernel_size * sizeof(float),\n                                             MemoryStorageType::kDevice);\n    if (!maybe_kernel) {\n      return maybe_kernel.error();\n    }\n\n    kernel_flat_ = maybe_kernel.value();\n    const cudaError_t error =\n        cudaMemcpy(kernel_flat_, kernel_flat_host, kernel_size * kernel_size * sizeof(float),\n                   cudaMemcpyHostToDevice);\n    if (error != cudaSuccess) {\n        GXF_LOG_ERROR(\"CUDA Mem copy Error: %s\\n\", cudaGetErrorString(error));\n        return GXF_FAILURE;\n    }\n    return GXF_SUCCESS;\n  }\n\n\n  gxf_result_t stop() override {\n    entity_queue_map_.clear();\n    return ToResultCode(allocator_->free(kernel_flat_));\n  }\n\n  gxf_result_t tick() override {\n    return ToResultCode(_tick());\n  }\n\n  // payload struct to access on the callback used to\n  // dereference the underlying message entity\n  typedef struct callBackData {\n    Entity data;\n    Convolve2D* ptr;\n  } callBackData;\n\n  static void CUDART_CB cudaEntityFreeCallback(void* data_ptr) {\n    auto data = reinterpret_cast<callBackData*>(data_ptr);\n    auto convolve = data->ptr;\n    GXF_LOG_VERBOSE(\"Received entity free callback from cuda stream for \"\n    \"cuda buffer in message entity E[%05ld]\", data->data.eid());\n    // Entities consumed until now cannot be freed just yet in the host callback function.\n    // Host callback function gets executed from a cuda driver thread which cannot be used\n    // to call another cuda runtime api like cudaFreeAsync. Hence keep track of the\n    // entities to be freed the next time this codelet is executed.\n    std::unique_lock<std::shared_mutex> lock(convolve->mutex_);\n    convolve->entity_free_list_.push(data->data.eid());\n  }\n\n  Expected<void> _tick() {\n    // Dequeue any previously consumed message entities\n    {\n      std::unique_lock<std::shared_mutex> lock(mutex_);\n      while (!entity_free_list_.empty()) {\n        auto element = entity_free_list_.front();\n        entity_queue_map_.erase(element);\n        entity_free_list_.pop();\n      }\n    }\n\n    auto in_message = GXF_UNWRAP_OR_RETURN(input_->receive());\n    GXF_LOG_INFO(\"Message Received: %d\", this->count_);\n    this->count_ = this->count_ + 1;\n    if (in_message.is_null()) {\n      return Unexpected{GXF_CONTRACT_MESSAGE_NOT_AVAILABLE};\n    }\n\n    auto in_buffer = GXF_UNWRAP_OR_RETURN(in_message.get<CudaBuffer>());\n    auto out_message = GXF_UNWRAP_OR_RETURN(Entity::New(context()));\n    auto out_buffer = GXF_UNWRAP_OR_RETURN(out_message.add<CudaBuffer>(kDefaultCudaBufferName));\n\n    auto stream_pool = in_buffer->streamPool();\n    auto stream_handle = in_buffer->transferStreamOwnership();\n    auto code = out_buffer->resizeAsync(allocator_, stream_pool, in_buffer->size(), stream_handle);\n\n    const int width = 32, height = 32;\n    auto cu_stream = GXF_UNWRAP_OR_RETURN(stream_handle->stream());\n\n    convolveKernel(reinterpret_cast<float*>(in_buffer->pointer()),\n                   reinterpret_cast<float*>(kernel_flat_),\n                   reinterpret_cast<float*>(out_buffer->pointer()),\n                   width, height, kernel_.get().size(), cu_stream);\n\n    cudaError_t err = cudaGetLastError();\n    if (err != cudaSuccess) {\n        GXF_LOG_ERROR(\"CUDA convolveKernel Error: %s\\n\", cudaGetErrorString(err));\n        return Unexpected{GXF_FAILURE};\n    }\n\n    // store the incoming message entity to prevent the cuda buffer getting released\n    // before it is consumed. The in_message entity is only released once the kernel\n    // has finished its execution\n    auto eid = in_message.eid();\n    {\n      std::unique_lock<std::shared_mutex> lock(mutex_);\n      entity_queue_map_.insert({eid, callBackData{std::move(in_message), this}});\n    }\n    auto it = entity_queue_map_.find(eid);\n\n\n    GXF_RETURN_IF_ERROR(in_buffer->registerCallbackOnStream(cudaEntityFreeCallback,\n                                                    reinterpret_cast<void*>(&it->second)));\n\n    auto stream_id = GXF_UNWRAP_OR_RETURN(out_message.add<CudaStreamId>());\n    stream_id->stream_cid = stream_handle.cid();\n    return output_->publish(out_message);\n  }\n\n private:\n  Parameter<Handle<CudaAllocator>> allocator_;\n  Parameter<std::vector<std::vector<float>>> kernel_;\n  Parameter<Handle<Transmitter>> output_;\n  Parameter<Handle<Receiver>> input_;\n  byte* kernel_flat_;\n  std::unordered_map<gxf_uid_t, callBackData> entity_queue_map_;\n  std::queue<gxf_uid_t> entity_free_list_;\n  std::shared_mutex mutex_;\n  int count_ = 1;\n};\n\n// Dequeue a message entity with a cuda buffer and ensures\n// the data is available\nclass CudaBufferRx: public Codelet {\n public:\n  virtual ~CudaBufferRx() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(signal_, \"signal\", \"Signal\",\n                        \"Channel to receive messages from another graph entity\");\n    return ToResultCode(result);\n  }\n\n  gxf_result_t tick() override {\n    auto maybe_message = signal_->receive();\n    GXF_LOG_INFO(\"Message Received: %d\", this->count);\n    this->count = this->count + 1;\n    if (!maybe_message || maybe_message.value().is_null()) {\n      return GXF_CONTRACT_MESSAGE_NOT_AVAILABLE;\n    }\n\n    auto message = maybe_message.value();\n    auto maybe_cuda_buffer = message.get<CudaBuffer>();\n    if (!maybe_cuda_buffer) { return ToResultCode(maybe_cuda_buffer); }\n\n    auto cuda_buffer = maybe_cuda_buffer.value();\n    auto state = cuda_buffer->state();\n    if (state != CudaBuffer::State::DATA_AVAILABLE) {\n      GXF_LOG_ERROR(\"CudaBuffer found in an invalid state %d\", static_cast<int>(state));\n      return GXF_FAILURE;\n    }\n\n    return GXF_SUCCESS;\n  }\n\n private:\n  Parameter<Handle<Receiver>> signal_;\n  int count = 1;\n};\n\n// Tracks unique SM IDs used during CUDA kernel executions and validates SM count\nclass CudaSMCount : public Codelet {\n public:\n  gxf_result_t initialize() override {\n    unique_smids_.clear();\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t tick() override {\n    std::lock_guard<std::mutex> lock(smids_mutex_);\n\n    // Default kernel configuration\n    int num_blocks = 64;\n    int threads_per_block = 1024;\n\n    int32_t result = 0;\n\n    // Check if we have a stream pool\n    const auto maybe_stream_pool = stream_pool_.try_get();\n    if (maybe_stream_pool) {\n      // Allocate a stream from the pool\n      auto stream = maybe_stream_pool.value()->allocateStream();\n      if (!stream) {\n        GXF_LOG_ERROR(\"Failed to allocate stream from pool\");\n        return GXF_FAILURE;\n      }\n\n      // Launch kernel with stream\n      result = collect_smid_from_kernel(num_blocks, threads_per_block,\n                                       stream.value()->stream().value(), unique_smids_);\n\n      // Release the stream back to the pool\n      auto release_result = maybe_stream_pool.value()->releaseStream(stream.value());\n      if (!release_result) {\n        GXF_LOG_ERROR(\"Failed to release stream back to pool\");\n        return GXF_FAILURE;\n      }\n    } else {\n      // Launch kernel without stream\n      result = collect_smid_from_kernel(num_blocks, threads_per_block, nullptr, unique_smids_);\n    }\n\n    if (result != 0) {\n      GXF_LOG_ERROR(\"Failed to collect SM IDs from kernel\");\n      return GXF_FAILURE;\n    }\n\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t deinitialize() override {\n    std::lock_guard<std::mutex> lock(smids_mutex_);\n    if (unique_smids_.size() > max_smid_count_.get()) {\n      GXF_LOG_ERROR(\"SM count check failed: used %zu SMs, expected no more than %zu\",\n                    unique_smids_.size(), max_smid_count_.get());\n      return GXF_FAILURE;\n    }\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(\n      max_smid_count_, \"max_smid_count\", \"Maximum number of SMs\",\n      \"Maximum number of unique SMs expected to be used by CUDA kernels\", 1UL);\n    result &= registrar->parameter(stream_pool_, \"stream_pool\", \"Cuda Stream Pool\",\n                                   \"Optional CUDA stream pool for kernel execution\",\n                                   Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL);\n    return ToResultCode(result);\n  }\n\n private:\n  Parameter<uint64_t> max_smid_count_;\n  Parameter<Handle<CudaStreamPool>> stream_pool_;\n  std::set<uint32_t> unique_smids_;\n  mutable std::mutex smids_mutex_;\n};\n\n}  // namespace cuda\n\n}  // namespace test\n\n}  // namespace gxf\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_CUDA_TESTS_TEST_CUDA_HELPER_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/logger/gxf_logger.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef COMMON_LOGGER_GXF_LOGGER_HPP\n#define COMMON_LOGGER_GXF_LOGGER_HPP\n\n#include <cstdio>\n#include <memory>\n\n#include \"gxf/logger/logger.hpp\"\n\nnamespace nvidia {\n\n// Environment variable for log level override\nconstexpr const char* kGxfLogEnvName = \"GXF_LOG_LEVEL\";\n\n// Indicates the level of severity for a log message\nenum class Severity {\n  // A utility case which can be used for 'SetSeverity' to disable all severity levels.\n  NONE = -2,\n  // A utility case which can be used for 'Redirect' and 'SetSeverity'\n  ALL = -1,\n  // The five different log severities in use from most severe to least severe.\n  PANIC = 0,  // Need to start at 0\n  ERROR,\n  WARNING,\n  INFO,\n  DEBUG,\n  VERBOSE,\n  // A utility case representing the number of log levels used internally.\n  COUNT\n};\n\n// The output severity which might limit certain levels of severity\nstruct SeverityContainer {\n  Severity r = Severity::INFO;\n\n  // Sets the default severity from the environment variable if it is set.\n  SeverityContainer();\n};\n\n/// Namespace for the NVIDIA logger functionality.\nnamespace logger {\n\n/// A logger that utilizes the GXF logging system.\n///\n/// The GXF logger supports six log levels:\n///   PANIC(0), ERROR(1), WARNING(2), INFO(3), DEBUG(4), and VERBOSE(5).\n///\n/// If no logger or log function is provided, a default logger implementation (DefaultGxfLogger) is\n/// used. The default logger leverages the existing functionality of the GXF logging system, which\n/// employs a global/singleton SeverityContainer to control the logging level, and a global\n/// `nvidia::LoggingFunction` to log messages.\n/// The initial logging level is determined by the environment variable `GXF_LOG_LEVEL` if it is\n/// set. Otherwise, the default logging level is INFO(3).\nclass GxfLogger : public Logger {\n public:\n  /// Constructs a logger with the provided logger or log function.\n  ///\n  /// If neither a logger nor a log function is provided, a default GXF logger will be instantiated.\n  ///\n  /// @param logger The logger to use (default: nullptr).\n  /// @param func The log function to use (default: nullptr).\n  explicit GxfLogger(const std::shared_ptr<ILogger>& logger = nullptr,\n                     const LogFunction& func = nullptr);\n};\n\n/// A logger that facilitates access to the global GXF logger.\n///\n/// The singleton GXF logger can be accessed via the instance() method.\n/// This class also provides a method to set the log severity from an environment variable.\n/// The log severity can be retrieved from the environment variable using GetSeverityFromEnv().\nclass GlobalGxfLogger {\n public:\n  /// Retrieve the global/singleton GXF logger instance.\n  static GxfLogger& instance();\n\n  /// Set the log severity based on the value of an environment variable.\n  ///\n  /// If the environment variable is not set or invalid, the severity remains unchanged.\n  /// @param env_name The name of the environment variable to use.\n  /// @return true if the environment variable is accessible and the severity was updated from it.\n  static bool SetSeverityFromEnv(const char* env_name);\n\n  /// Return the log severity based on the value of an environment variable.\n  ///\n  /// If the environment variable is not set or invalid, returns Severity::COUNT.\n  /// If 'error_code' is not null, it will be set to 1 if the environment variable is set but\n  /// invalid, or to 0 otherwise.\n  ///\n  /// @param env_name The name of the environment variable to use.\n  /// @param error_code The error code to set if the environment variable is invalid.\n  /// @return The log severity from the environment variable.\n  static Severity GetSeverityFromEnv(const char* env_name, int* error_code);\n};\n\n}  // namespace logger\n\n}  // namespace nvidia\n\n#endif /* COMMON_LOGGER_GXF_LOGGER_HPP */\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/logger/logger.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef COMMON_LOGGER_LOGGER_HPP\n#define COMMON_LOGGER_LOGGER_HPP\n\n#include <cstdarg>\n#include <cstdio>\n#include <functional>\n#include <memory>\n#include <string>\n#include <vector>\n\nnamespace nvidia {\n\n/// Namespace for the NVIDIA logger functionality.\nnamespace logger {\n\nusing LogFunction = std::function<void(const char* file, int line, const char* function_name,\n                                       int level, const char* message, void* arg)>;\n\n/// Interface for custom logger implementations.\n///\n/// ILogger provides an interface for implementing custom logging mechanisms.\n/// This interface allows the integration of various logging systems with the Logger class.\n/// Users can define their own logging behavior by implementing these methods.\nclass ILogger {\n public:\n  /// Destructor for ILogger.\n  virtual ~ILogger() = default;\n\n  /// Log a message.\n  ///\n  /// This method logs a message based on the provided details such as file name, line number,\n  /// function/module name, log level, message, and an optional argument.\n  ///\n  /// @param file The name of the source file where the log request originated.\n  /// @param line The line number in the source file.\n  /// @param name The name of the function/module making the log request.\n  /// @param level The log level.\n  /// @param message The log message.\n  /// @param arg An optional argument for additional log information (default: nullptr).\n  virtual void log(const char* file, int line, const char* name, int level, const char* message,\n                   void* arg = nullptr) = 0;\n\n  /// Set the log message pattern.\n  ///\n  /// Set the pattern used for formatting log messages.\n  ///\n  /// @param pattern The pattern string for formatting log messages.\n  virtual void pattern(const char* pattern) = 0;\n\n  /// Get the current log message pattern.\n  ///\n  /// Retrieve the currently set pattern used for formatting log messages.\n  ///\n  /// @return The currently set pattern string.\n  virtual const char* pattern() const = 0;\n\n  /// Set the log level.\n  ///\n  /// Set the log level, determining the priority of messages that should be logged.\n  ///\n  /// @param level The log level.\n  virtual void level(int level) = 0;\n\n  /// Get the current log level.\n  ///\n  /// Retrieve the currently set log level.\n  ///\n  /// @return The currently set log level.\n  virtual int level() const = 0;\n\n  /// Redirect log output.\n  ///\n  /// Redirect log output to the specified file or other output for the given log level.\n  ///\n  /// @param level The log level for which output redirection is applied.\n  /// @param output A pointer to the file or other output to redirect log output.\n  virtual void redirect(int level, void* output) = 0;\n\n  /// Get the current redirection for a specified log level.\n  ///\n  /// Retrieve the file/output pointer to which log output is currently being redirected for the\n  /// specified log level. If no redirection is set for the given level, returns nullptr.\n  ///\n  /// @param level The log level for which the redirection is queried.\n  /// @return A pointer to the file/output where log output is redirected, or nullptr if no\n  /// redirection is set.\n  virtual void* redirect(int level) const = 0;\n};\n\n/// Logger class for logging messages.\n///\n/// The Logger class provides a flexible and customizable logging system. It supports a variety of\n/// output targets, such as strings, standard error streams, files, or even network endpoints, and\n/// allows for logging at different levels of severity. This versatility makes it suitable for a\n/// wide range of applications and use cases.\n///\n/// A key feature of the Logger is its ability to be specialized. Users can create a custom logger\n/// specialization (for example, targeting a string, standard error, a file, or a network endpoint)\n/// according to their specific needs. Once created, this specialized logger can be configured and\n/// used consistently across various components of an application.\n///\n/// In addition to its flexibility in terms of output and configuration, the Logger class supports\n/// both singleton and regular class usage patterns. This provides further versatility in how\n/// logging functionality can be integrated into an application.\n///\n/// The following examples illustrate how to use the Logger in both singleton and regular class\n/// contexts.\n///\n/// ```cpp\n/// class SingletonLogger : public Logger {\n///  public:\n///   static SingletonLogger& instance() {\n///     static SingletonLogger instance;\n///     return instance;\n///   }\n///\n///  private:\n///   SingletonLogger() {\n///     // Set default logger\n///     logger_ = std::make_shared<MyLogger>();\n///     func_ = nullptr;\n///\n///     // Set level\n///     level(logger_->level());\n///\n///     // Set pattern\n///     pattern(logger_->pattern());\n///\n///     // Set sinks\n///     for (int severity = kNumSeverity - 1; severity >= 0; --severity) {\n///       redirect(severity, s_sinks[severity]);\n///     }\n///   }\n/// };\n/// // ...\n/// SingletonLogger::instance().log(__FILE__, __LINE__, __FUNCTION__, 0, \"Hello World!\");\n/// ```\n///\n/// The following example shows how to use Logger as a regular class:\n///\n/// ```cpp\n/// class TestLogger : public Logger {\n///  public:\n///   static std::shared_ptr<TestLogger> create() { return std::make_shared<TestLogger>(); }\n/// };\n/// // ...\n/// std::shared_ptr<TestLogger> logger = TestLogger::create();\n/// logger->log(__FILE__, __LINE__, __FUNCTION__, 0, \"Hello World!\");\n/// ```\nclass Logger {\n public:\n  /// Construct a logger instance.\n  Logger() = default;\n\n  /// Construct a logger instance with the custom logger.\n  ///\n  /// @param logger A shared pointer to the custom logger.\n  explicit Logger(const std::shared_ptr<ILogger>& logger);\n\n  /// Construct a logger instance with the custom log function.\n  ///\n  /// @param func The custom log function.\n  explicit Logger(const LogFunction& func);\n\n  /// Construct a logger instance with the custom logger or log function.\n  ///\n  /// @param logger A shared pointer to the custom logger.\n  /// @param func The custom log function.\n  Logger(const std::shared_ptr<ILogger> logger, const LogFunction& func);\n\n  /// Log a message.\n  ///\n  /// Log a message using either a custom logger set via logger() or func().\n  /// The custom logger set via func() has priority over the one set via logger(). If neither is\n  /// set, no logging occurs.\n  ///\n  /// @param file The name of the source file where the log request originated.\n  /// @param line The line number in the source file.\n  /// @param name The name of the function/module making the log request.\n  /// @param level The log level.\n  /// @param message The log message.\n  void log(const char* file, int line, const char* name, int level, const char* message);\n\n  /// Set a custom logger.\n  ///\n  /// Register a custom logger to be used for logging messages. The custom logger must implement\n  /// the ILogger interface.\n  ///\n  /// @param custom_logger A shared pointer to the custom logger.\n  void logger(std::shared_ptr<ILogger> custom_logger);\n\n  /// Get the current custom logger.\n  ///\n  /// Retrieve the currently set custom logger.\n  ///\n  /// @return A shared pointer to the currently set custom logger.\n  std::shared_ptr<ILogger> logger() const;\n\n  /// Set a custom log function.\n  ///\n  /// Register a custom log function and an argument to be used for logging messages. This function\n  /// has higher priority over the custom logger set via logger().\n  /// If func_arg is not nullptr, func_arg will be passed to log_func as an argument ('arg'\n  /// parameter).\n  ///\n  /// @param log_func The custom log function.\n  /// @param func_arg An opaque pointer to auxiliary data needed by log_func.\n  void func(LogFunction log_func, void* func_arg = nullptr);\n\n  /// Get the current custom log function.\n  ///\n  /// Retrieve the currently set custom log function.\n  ///\n  /// @return The currently set custom log function.\n  LogFunction func() const;\n\n  /// Get the current custom log function argument.\n  ///\n  /// Retrieve the currently set custom log function argument.\n  ///\n  /// @return The currently set custom log function argument.\n  void* arg() const;\n\n  /// Set the log message pattern.\n  ///\n  /// Set the pattern used for formatting log messages.\n  ///\n  /// @param pattern The pattern string for formatting log messages.\n  void pattern(const char* pattern);\n\n  /// Get the current log message pattern.\n  ///\n  /// Retrieve the currently set pattern used for formatting log messages.\n  ///\n  /// @return The currently set pattern string.\n  const char* pattern() const;\n\n  /// Set the log level.\n  ///\n  /// Set the log level, determining the priority of messages that should be logged.\n  ///\n  /// @param level The log level.\n  void level(int level);\n\n  /// Get the current log level.\n  ///\n  /// Retrieve the currently set log level.\n  ///\n  /// @return The currently set log level.\n  int level() const;\n\n  /// Redirect log output.\n  ///\n  /// Redirect log output to the specified file or other output for the given log level.\n  ///\n  /// @param level The log level for which output redirection is applied.\n  /// @param output A pointer to the file or other output to redirect log output.\n  void redirect(int level, void* output);\n\n  /// Get the current redirection for a specified log level.\n  ///\n  /// Retrieve the file/output pointer to which log output is currently being redirected for the\n  /// specified log level. If no redirection is set for the given level, returns nullptr.\n  ///\n  /// @param level The log level for which the redirection is queried.\n  /// @return A pointer to the file/output where log output is redirected, or nullptr if no\n  /// redirection is set.\n  void* redirect(int level) const;\n\n protected:\n  std::shared_ptr<ILogger> logger_;  ///< shared pointer to the logger\n  LogFunction func_;                 ///< log function\n  void* func_arg_ = nullptr;         ///< log function argument\n\n  std::string pattern_;       ///< log pattern\n  int level_ = 0;             ///< log level\n  std::vector<void*> sinks_;  ///< log sinks\n};\n\n}  // namespace logger\n\n}  // namespace nvidia\n\n#endif /* COMMON_LOGGER_LOGGER_HPP */\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/multimedia/audio.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_MULTIMEDIA_AUDIO_HPP_\n#define NVIDIA_GXF_MULTIMEDIA_AUDIO_HPP_\n\n#include <cstdint>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/memory_buffer.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Supported raw audio media types\nenum class AudioFormat {\n  GXF_AUDIO_FORMAT_CUSTOM = 0,\n  GXF_AUDIO_FORMAT_S16LE,  // 16-bit signed PCM audio\n  GXF_AUDIO_FORMAT_F32LE,  // 32-bit floating-point audio\n};\n\n// Supported channel layouts\nenum class AudioLayout {\n  GXF_AUDIO_LAYOUT_CUSTOM = 0,\n  GXF_AUDIO_LAYOUT_INTERLEAVED,      // Data from all the channels to be interleaved LRLRLR\n  GXF_AUDIO_LAYOUT_NON_INTERLEAVED,  // Data from all the channels not to be interleaved LLLRRR\n};\n\ntemplate <AudioFormat>\nstruct AudioTypeTraits;\n\n#define GXF_AUDIO_TYPE_TRAITS(ENUM, WIDTH)                        \\\n  template <>                                                     \\\n  struct AudioTypeTraits<AudioFormat::ENUM> {                     \\\n    static constexpr const char* name = #ENUM;                    \\\n    static constexpr const AudioFormat value = AudioFormat::ENUM; \\\n    static constexpr const int8_t width = WIDTH;                  \\\n  };\n\nGXF_AUDIO_TYPE_TRAITS(GXF_AUDIO_FORMAT_CUSTOM, 0);\nGXF_AUDIO_TYPE_TRAITS(GXF_AUDIO_FORMAT_S16LE, 16);\nGXF_AUDIO_TYPE_TRAITS(GXF_AUDIO_FORMAT_F32LE, 32);\n\n// Descriptor for an AudioBuffer\nstruct AudioBufferInfo {\n  // Number of channels in an audio frame\n  uint32_t channels;\n  // Number of samples in an audio frame\n  uint32_t samples;\n  // sampling rate in Hz\n  uint32_t sampling_rate;\n  // Number of bytes required per sample\n  uint32_t bytes_per_sample;\n  // AudioFormat of an audio frame\n  AudioFormat audio_format;\n  // AudioLayout of an audio frame\n  AudioLayout audio_layout;\n};\n\n// A media data type which stores information corresponding to an audio frame\n// resize(...) function is used to allocate memory for the audio frame based on\n// the audio frame info\nclass AudioBuffer {\n public:\n  AudioBuffer() = default;\n\n  ~AudioBuffer() { memory_buffer_.freeBuffer(); }\n\n  AudioBuffer(const AudioBuffer&) = delete;\n\n  AudioBuffer(AudioBuffer&& other) { *this = std::move(other); }\n\n  AudioBuffer& operator=(const AudioBuffer&) = delete;\n\n  AudioBuffer& operator=(AudioBuffer&& other) {\n    buffer_info_ = other.buffer_info_;\n    memory_buffer_ = std::move(other.memory_buffer_);\n\n    return *this;\n  }\n\n  template <AudioFormat A>\n  Expected<void> resize(uint32_t channels, uint32_t samples, uint32_t sampling_rate,\n                        AudioLayout layout, MemoryStorageType storage_type,\n                        Handle<Allocator> allocator) {\n    AudioTypeTraits<A> audio_type;\n    uint32_t bytes_per_sample = std::ceil(audio_type.width / 8);\n    AudioBufferInfo buffer_info{channels,         samples,          sampling_rate,\n                                bytes_per_sample, audio_type.value, layout};\n    return resizeCustom(buffer_info, storage_type, allocator);\n  }\n\n  // Type of the callback function to release memory passed to the AudioFrame using the\n  // wrapMemory method\n  using release_function_t = MemoryBuffer::release_function_t;\n\n  // Wrap existing memory inside the AudioBuffer. A callback function of type release_function_t\n  // may be passed that will be called when the AudioBuffer wants to release the memory.\n  Expected<void> wrapMemory(AudioBufferInfo buffer_info, uint64_t size,\n                            MemoryStorageType storage_type, void* pointer,\n                            release_function_t release_func);\n\n  // AudioBufferInfo of the AudioBuffer\n  AudioBufferInfo audio_buffer_info() const { return buffer_info_; }\n\n  // The type of memory where the frame data is stored.\n  MemoryStorageType storage_type() const { return memory_buffer_.storage_type(); }\n\n  // Size of the audio frame in bytes\n  uint64_t size() const { return memory_buffer_.size(); }\n\n  // Raw pointer to the first byte of the audio frame\n  byte* pointer() const { return memory_buffer_.pointer(); }\n\n  // Resizes the audio frame and allocates the corresponding memory with the allocator provided\n  // Any data previously stored in the frame would be freed\n  Expected<void> resizeCustom(AudioBufferInfo buffer_info, MemoryStorageType storage_type,\n                              Handle<Allocator> allocator);\n\n private:\n  AudioBufferInfo buffer_info_;\n  MemoryBuffer memory_buffer_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_MULTIMEDIA_AUDIO_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/multimedia/camera.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_MULTIMEDIA_CAMERA_HPP_\n#define NVIDIA_GXF_MULTIMEDIA_CAMERA_HPP_\n\n#include <array>\n#include <cstdint>\n#include \"gxf/multimedia/video.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\ntemplate <class T>\nstruct Vector2 {\n  T x; /**< point x coordinate. */\n  T y; /**< point y coordinate. */\n};\n\nusing Vector2u = Vector2<uint32_t>;\nusing Vector2f = Vector2<float>;\nusing Vector2d = Vector2<double>;\n\nenum class DistortionType {\n  /**\n   * No distortion\n   */\n  Perspective,\n  /**\n   * Brown model with 3 radial and 2 tangential distortion coefficients.\n   */\n  Brown,\n  /**\n   * Polynomial model with 6 radial and 2 tangential distortion coefficients.\n   */\n  Polynomial,\n  /**\n   * Fisheye model with 4 radial distortion coefficients.\n   * Specifies the equidistant fisheye mapping.\n   * Mapping is defined by:\n   * \\f[r = f\\theta\\f]\n   * where:\n   * - \\f$\\theta\\f$ is the angle from the optical axis.\n   * - \\f$f\\f$ is the focal length.\n   * - \\f$r\\f$ is the distance of a pixel from the image center.\n   */\n  FisheyeEquidistant,\n  /**\n   * Fisheye model with 4 radial distortion coefficients.\n   * Specifies the equisolid fisheye mapping.\n   * Mapping is defined by:\n   * \\f[r = 2f\\sin\\left(\\frac{\\theta}{2}\\right)\\f]\n   * where:\n   * - \\f$\\theta\\f$ is the angle from the optical axis.\n   * - \\f$f\\f$ is the focal length.\n   * - \\f$r\\f$ is the distance of a pixel from the image center.\n   */\n  FisheyeEquisolid,\n  /**\n   * Fisheye model with 4 radial distortion coefficients.\n   * Specifies the orthographic fisheye mapping.\n   * Mapping is defined by:\n   * \\f[r = f\\sin(\\theta)\\f]\n   * where:\n   * - \\f$\\theta\\f$ is the angle from the optical axis.\n   * - \\f$f\\f$ is the focal length.\n   * - \\f$r\\f$ is the distance of a pixel from the image center.\n   */\n  FisheyeOrthoGraphic,\n  /**\n   * Fisheye model with 4 radial distortion coefficients.\n   * Specifies the stereographic fisheye mapping.\n   * Mapping is defined by:\n   * \\f[r = 2f\\tan\\left(\\frac{\\theta}{2}\\right)\\f]\n   * where:\n   * - \\f$\\theta\\f$ is the angle from the optical axis.\n   * - \\f$f\\f$ is the focal length.\n   * - \\f$r\\f$ is the distance of a pixel from the image center.\n   */\n  FisheyeStereographic,\n};\n\n/**\n * Camera intrinsics model.\n * Intrinsic camera matrix is defined as follow:\n *     [fx  s cx]\n * K = [ 0 fy cy]\n *     [ 0  0  1]\n * where:\n * - fx, fy represent focal length.\n * - cx, cy represent principle point.\n * - s represents the skew value.\n */\ntemplate <typename T>\nstruct CameraModelBase {\n  // The maximum number of distortion coefficients.\n  static constexpr int kMaxDistortionCoefficients = 8;\n  // The dimensions of the camera image plane in pixels in the order x, y.\n  Vector2u dimensions;\n  // Focal length of the projection (in pixels) in the order x, y.\n  Vector2<T> focal_length;\n  // Optical center of the projection (in pixels) in the order x, y.\n  Vector2<T> principal_point;\n  // The skew coefficient between the x and the y axis (usually 0).\n  T skew_value;\n  // Distortion type of the camera.\n  DistortionType distortion_type;\n  // Distortion coefficients of the camera.\n  std::array<T, kMaxDistortionCoefficients> distortion_coefficients;\n};\n\nusing CameraModel = CameraModelBase<float>;\n\n/**\n * Camera extrinsics model.\n * Extrinsic camera matrix is defined as follow:\n * T = [R | t]\n * where:\n * - R is the rotation matrix.\n * - t is the translation vector.\n */\ntemplate <typename T>\nstruct Pose3DBase {\n  // The 3x3 rotation matrix.\n  std::array<T, 9> rotation;\n  // The translation vector.\n  std::array<T, 3> translation;\n};\n\nusing Pose3D = Pose3DBase<float>;\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_MULTIMEDIA_CAMERA_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/multimedia/video.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_MULTIMEDIA_VIDEO_HPP_\n#define NVIDIA_GXF_MULTIMEDIA_VIDEO_HPP_\n\n#include <cstdint>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/memory_buffer.hpp\"\n#include \"gxf/std/tensor.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n#ifdef __aarch64__\nstatic constexpr const uint16_t kGxfAlignValue = 256;\n#else\nstatic constexpr const uint16_t kGxfAlignValue = 256;\n#endif\n\n// Align frame stride to kGxfAlignValue\nstatic constexpr uint32_t StrideAlign(uint32_t stride) {\n  return (stride % kGxfAlignValue == 0) ? stride : ((stride / kGxfAlignValue + 1) * kGxfAlignValue);\n}\n\n// Compute strides for video buffer\n//\n// If stride is non zero, use it as it is\n// If stride is not set and align is enabled (default), use align(w * bytes)\n// If stride is not set and align is disabled, use (w * bytes)\nstatic constexpr uint32_t ComputeStrides(bool stride_align, uint32_t width,\n                                         uint32_t bytes_per_pixel,\n                                         uint32_t stride) {\n  if (stride != 0) {\n    // Use existing stride without alignment\n    return stride;\n  } else if (stride_align) {\n    // unset stride, align based on width\n    return StrideAlign(width * bytes_per_pixel);\n  } else {\n    // return unaligned buffer\n    return width * bytes_per_pixel;\n  }\n}\n\n// Round up to the closest even number\nstatic constexpr uint32_t AlignToEvenDimension(uint32_t value) {\n  return value + (value & 1);\n}\n\n// Supported raw video media types\nenum class VideoFormat : std::int64_t {\n  GXF_VIDEO_FORMAT_CUSTOM = 0,\n  GXF_VIDEO_FORMAT_YUV420,         // BT.601 multi planar 4:2:0 YUV\n  GXF_VIDEO_FORMAT_YUV420_ER,      // BT.601 multi planar 4:2:0 YUV ER\n  GXF_VIDEO_FORMAT_YUV420_709,     // BT.709 multi planar 4:2:0 YUV\n  GXF_VIDEO_FORMAT_YUV420_709_ER,  // BT.709 multi planar 4:2:0 YUV\n  GXF_VIDEO_FORMAT_NV12,           // BT.601 multi planar 4:2:0 YUV with interleaved UV\n  GXF_VIDEO_FORMAT_NV12_ER,        // BT.601 multi planar 4:2:0 YUV ER with interleaved UV\n  GXF_VIDEO_FORMAT_NV12_709,       // BT.709 multi planar 4:2:0 YUV with interleaved UV\n  GXF_VIDEO_FORMAT_NV12_709_ER,    // BT.709 multi planar 4:2:0 YUV ER with interleaved UV\n  GXF_VIDEO_FORMAT_RGBA,           // RGBA-8-8-8-8 single plane\n  GXF_VIDEO_FORMAT_BGRA,           // BGRA-8-8-8-8 single plane\n  GXF_VIDEO_FORMAT_ARGB,           // ARGB-8-8-8-8 single plane\n  GXF_VIDEO_FORMAT_ABGR,           // ABGR-8-8-8-8 single plane\n  GXF_VIDEO_FORMAT_RGBX,           // RGBX-8-8-8-8 single plane\n  GXF_VIDEO_FORMAT_BGRX,           // BGRX-8-8-8-8 single plane\n  GXF_VIDEO_FORMAT_XRGB,           // XRGB-8-8-8-8 single plane\n  GXF_VIDEO_FORMAT_XBGR,           // XBGR-8-8-8-8 single plane\n  GXF_VIDEO_FORMAT_RGB,            // RGB-8-8-8 single plane\n  GXF_VIDEO_FORMAT_BGR,            // BGR-8-8-8 single plane\n  GXF_VIDEO_FORMAT_R8_G8_B8,       // RGB - unsigned 8 bit multiplanar\n  GXF_VIDEO_FORMAT_B8_G8_R8,       // BGR - unsigned 8 bit multiplanar\n  GXF_VIDEO_FORMAT_GRAY,           // 8 bit GRAY scale single plane\n  GXF_VIDEO_FORMAT_GRAY16,         // 16 bit GRAY scale single plane\n  GXF_VIDEO_FORMAT_GRAY32,         // 32 bit GRAY scale single plane\n  GXF_VIDEO_FORMAT_GRAY32F,        // float 32 bit GRAY scale single plane\n  GXF_VIDEO_FORMAT_RGB16,          // RGB-16-16-16 single plane\n  GXF_VIDEO_FORMAT_BGR16,          // BGR-16-16-16 single plane\n  GXF_VIDEO_FORMAT_RGB32,          // RGB-32-32-32 single plane\n  GXF_VIDEO_FORMAT_BGR32,          // BGR-32-32-32 single plane\n  GXF_VIDEO_FORMAT_R16_G16_B16,    // RGB - signed 16 bit multiplanar\n  GXF_VIDEO_FORMAT_B16_G16_R16,    // BGR - signed 16 bit multiplanar\n  GXF_VIDEO_FORMAT_R32_G32_B32,    // RGB - signed 32 bit multiplanar\n  GXF_VIDEO_FORMAT_B32_G32_R32,    // BGR - signed 32 bit multiplanar\n  GXF_VIDEO_FORMAT_NV24,           // multi planar 4:4:4 YUV with interleaved UV\n  GXF_VIDEO_FORMAT_NV24_ER,        // multi planar 4:4:4 YUV ER with interleaved UV\n  GXF_VIDEO_FORMAT_R8_G8_B8_D8,    // RGBD unsigned 8 bit multiplanar\n  GXF_VIDEO_FORMAT_R16_G16_B16_D16,  // RGBD unsigned 16 bit multiplanar\n  GXF_VIDEO_FORMAT_R32_G32_B32_D32,  // RGBD unsigned 32 bit multiplanar\n  GXF_VIDEO_FORMAT_RGBD8,         // RGBD 8 bit unsigned single plane\n  GXF_VIDEO_FORMAT_RGBD16,        // RGBD 16 bit unsigned single plane\n  GXF_VIDEO_FORMAT_RGBD32,        // RGBD 32 bit unsigned single plane\n  GXF_VIDEO_FORMAT_D32F,          // Depth 32 bit float single plane\n  GXF_VIDEO_FORMAT_D64F,          // Depth 64 bit float single plane\n  GXF_VIDEO_FORMAT_RAW16_RGGB,    // RGGB-16-16-16-16 single plane\n  GXF_VIDEO_FORMAT_RAW16_BGGR,    // BGGR-16-16-16-16 single plane\n  GXF_VIDEO_FORMAT_RAW16_GRBG,    // GRBG-16-16-16-16 single plane\n  GXF_VIDEO_FORMAT_RAW16_GBRG,    // GBRG-16-16-16-16 single plane\n};\n\n// Supported surface memory types\nenum class SurfaceLayout : std::int32_t {\n  GXF_SURFACE_LAYOUT_INVALID = 0,\n  GXF_SURFACE_LAYOUT_PITCH_LINEAR,  // pitch linear surface memory\n  GXF_SURFACE_LAYOUT_BLOCK_LINEAR,  // block linear surface memory\n};\n\ntemplate <VideoFormat>\nstruct VideoTypeTraits;\n\n#define GXF_VIDEO_TYPE_TRAITS(ENUM)                               \\\n  template <>                                                     \\\n  struct VideoTypeTraits<VideoFormat::ENUM> {                     \\\n    static constexpr const char* name = #ENUM;                    \\\n    static constexpr const VideoFormat value = VideoFormat::ENUM; \\\n  };\n\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_CUSTOM);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_YUV420);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_YUV420_ER);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_YUV420_709);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_YUV420_709_ER);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_NV12);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_NV12_ER);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_NV12_709);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_NV12_709_ER);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RGBA);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_BGRA);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_ARGB);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_ABGR);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RGBX);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_BGRX);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_XRGB);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_XBGR);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RGB);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_BGR);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_R8_G8_B8);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_B8_G8_R8);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_GRAY);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_GRAY16);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_GRAY32);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_GRAY32F);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RGB16);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_BGR16);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RGB32);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_BGR32);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_R16_G16_B16);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_B16_G16_R16);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_R32_G32_B32);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_B32_G32_R32);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_NV24);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_NV24_ER);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_R8_G8_B8_D8);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_R16_G16_B16_D16);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_R32_G32_B32_D32);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RGBD8);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RGBD16);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RGBD32);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_D32F);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_D64F);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RAW16_RGGB);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RAW16_BGGR);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RAW16_GRBG);\nGXF_VIDEO_TYPE_TRAITS(GXF_VIDEO_FORMAT_RAW16_GBRG);\n\n\n// Struct to hold the information regarding a single color plane\nstruct ColorPlane {\n  std::string color_space;\n  uint8_t bytes_per_pixel = 0;\n  uint32_t stride = 0;\n  uint32_t offset = 0;\n  uint32_t width = 0;\n  uint32_t height = 0;\n  uint64_t size = 0;\n\n  ColorPlane() {}\n  ColorPlane(const char* c_space, uint8_t c_depth, uint32_t c_stride = 0)\n      : color_space(c_space), bytes_per_pixel(c_depth), stride(c_stride) {}\n};\n\n// Template to compute the memory size of an image\n// for different color formats\ntemplate <VideoFormat T, typename Enabler = void>\nstruct VideoFormatSize {\n  uint64_t size(uint32_t width, uint32_t height,\n                bool stride_align = true) { return 0; }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) { return {}; }\n};\n\n//  Specifies YUV420 multi-planar variants\ntemplate <VideoFormat T>\nstruct VideoFormatSize<T, std::enable_if_t<T == VideoFormat::GXF_VIDEO_FORMAT_YUV420 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_YUV420_ER ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_YUV420_709 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_YUV420_709_ER>> {\n  std::array<ColorPlane, 3> default_yuv{ColorPlane(\"Y\", 1), ColorPlane(\"U\", 1), ColorPlane(\"V\", 1)};\n\n  uint64_t fillColorPlanes(uint32_t width, uint32_t height,\n                           std::array<ColorPlane, 3>& color_planes,\n                           bool stride_align = true) {\n    uint32_t widthEven = AlignToEvenDimension(width);\n    uint32_t heightEven = AlignToEvenDimension(height);\n    color_planes[0].width = widthEven;\n    color_planes[1].width = widthEven / 2;\n    color_planes[2].width = widthEven / 2;\n    color_planes[0].height = heightEven;\n    color_planes[1].height = heightEven / 2;\n    color_planes[2].height = heightEven / 2;\n\n    uint64_t size = 0;\n    for (size_t i = 0; i < color_planes.size(); ++i) {\n      if (i == 0) {\n        color_planes[i].stride = ComputeStrides(stride_align, color_planes[i].width,\n                                color_planes[i].bytes_per_pixel, color_planes[i].stride);\n      } else if (color_planes[i].stride == 0) {\n          // YUV formats require UV planes stride to be half of Y plane\n          color_planes[i].stride = color_planes[0].stride / 2;\n      }\n      color_planes[i].size = color_planes[i].stride * color_planes[i].height;\n      color_planes[i].offset = size;\n      size += color_planes[i].size;\n    }\n    return size;\n  }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) {\n    auto yuv = default_yuv;\n    fillColorPlanes(width, height, yuv, stride_align);\n    std::vector<ColorPlane> result(yuv.begin(), yuv.end());\n    return result;\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, std::array<ColorPlane, 3>& color_planes,\n                bool stride_align = true) {\n    return fillColorPlanes(width, height, color_planes, stride_align);\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, bool stride_align = true) {\n    return fillColorPlanes(width, height, default_yuv, stride_align);\n  }\n};\n\n// Specifies YUV444 multi-planar variants\ntemplate <VideoFormat T>\nstruct VideoFormatSize<T, std::enable_if_t<T == VideoFormat::GXF_VIDEO_FORMAT_NV24 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_NV24_ER>> {\n  std::array<ColorPlane, 2> default_yuv{ColorPlane(\"Y\", 1), ColorPlane(\"UV\", 2)};\n\n  uint64_t fillColorPlanes(uint32_t width, uint32_t height,\n                           std::array<ColorPlane, 2>& color_planes,\n                           bool stride_align = true) {\n    uint32_t widthEven = AlignToEvenDimension(width);\n    uint32_t heightEven = AlignToEvenDimension(height);\n\n    uint64_t size = 0;\n    for (size_t i = 0; i < color_planes.size(); ++i) {\n      color_planes[i].width = widthEven;\n      color_planes[i].height = heightEven;\n      color_planes[i].stride = ComputeStrides(stride_align, color_planes[i].width,\n                                color_planes[i].bytes_per_pixel, color_planes[i].stride);\n      color_planes[i].size = color_planes[i].stride * color_planes[i].height;\n      color_planes[i].offset = size;\n      size += color_planes[i].size;\n    }\n    return size;\n  }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) {\n    auto yuv = default_yuv;\n    fillColorPlanes(width, height, yuv, stride_align);\n    std::vector<ColorPlane> result(yuv.begin(), yuv.end());\n    return result;\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, std::array<ColorPlane, 2>& color_planes,\n                bool stride_align = true) {\n    return fillColorPlanes(width, height, color_planes, stride_align);\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, bool stride_align = true) {\n    return fillColorPlanes(width, height, default_yuv, stride_align);\n  }\n};\n\n// Specifies NV12 Y/CbCr 4:2:0 multi-planar variants\ntemplate <VideoFormat T>\nstruct VideoFormatSize<T, std::enable_if_t<T == VideoFormat::GXF_VIDEO_FORMAT_NV12 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_NV12_ER ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_NV12_709 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_NV12_709_ER>> {\n  std::array<ColorPlane, 2> default_yuv{ColorPlane(\"Y\", 1), ColorPlane(\"UV\", 2)};\n\n  uint64_t fillColorPlanes(uint32_t width, uint32_t height,\n                           std::array<ColorPlane, 2>& color_planes,\n                           bool stride_align = true) {\n    uint32_t widthEven = AlignToEvenDimension(width);\n    uint32_t heightEven = AlignToEvenDimension(height);\n    color_planes[0].width = widthEven;\n    color_planes[1].width = widthEven / 2;\n    color_planes[0].height = heightEven;\n    color_planes[1].height = heightEven / 2;\n\n    uint64_t size = 0;\n    for (size_t i = 0; i < color_planes.size(); ++i) {\n      color_planes[i].stride = ComputeStrides(stride_align, color_planes[i].width,\n                                color_planes[i].bytes_per_pixel, color_planes[i].stride);\n      color_planes[i].size = color_planes[i].stride * color_planes[i].height;\n      color_planes[i].offset = size;\n      size += color_planes[i].size;\n    }\n    return size;\n  }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) {\n    auto yuv = default_yuv;\n    fillColorPlanes(width, height, yuv, stride_align);\n    std::vector<ColorPlane> result(yuv.begin(), yuv.end());\n    return result;\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, std::array<ColorPlane, 2>& color_planes,\n                bool stride_align = true) {\n    return fillColorPlanes(width, height, color_planes, stride_align);\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, bool stride_align = true) {\n    return fillColorPlanes(width, height, default_yuv, stride_align);\n  }\n};\n\n// Specifies 8-8-8-8 single plane RGBX/XRGB variants\ntemplate <VideoFormat T>\nstruct VideoFormatSize<\n    T, std::enable_if_t<\n           T == VideoFormat::GXF_VIDEO_FORMAT_RGBA || T == VideoFormat::GXF_VIDEO_FORMAT_BGRA ||\n           T == VideoFormat::GXF_VIDEO_FORMAT_ARGB || T == VideoFormat::GXF_VIDEO_FORMAT_ABGR ||\n           T == VideoFormat::GXF_VIDEO_FORMAT_RGBX || T == VideoFormat::GXF_VIDEO_FORMAT_BGRX ||\n           T == VideoFormat::GXF_VIDEO_FORMAT_XRGB || T == VideoFormat::GXF_VIDEO_FORMAT_XBGR>> {\n  std::array<ColorPlane, 1> defaultRGBA() {\n    if (T == VideoFormat::GXF_VIDEO_FORMAT_RGBA) {\n      return {ColorPlane(\"RGBA\", 4)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_BGRA) {\n      return {ColorPlane(\"BGRA\", 4)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_ARGB) {\n      return {ColorPlane(\"ARGB\", 4)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_ABGR) {\n      return {ColorPlane(\"ABGR\", 4)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_RGBX) {\n      return {ColorPlane(\"RGBX\", 4)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_BGRX) {\n      return {ColorPlane(\"BGRX\", 4)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_XRGB) {\n      return {ColorPlane(\"XRGB\", 4)};\n    } else {  // T == VideoFormat::GXF_VIDEO_FORMAT_XBGR\n      return {ColorPlane(\"XBGR\", 4)};\n    }\n  }\n\n  uint64_t fillColorPlanes(uint32_t width, uint32_t height,\n                           std::array<ColorPlane, 1>& color_plane,\n                           bool stride_align = true) {\n    uint32_t widthEven = AlignToEvenDimension(width);\n    uint32_t heightEven = AlignToEvenDimension(height);\n    color_plane[0].width = widthEven;\n    color_plane[0].height = heightEven;\n    color_plane[0].stride = ComputeStrides(stride_align, color_plane[0].width,\n                            color_plane[0].bytes_per_pixel, color_plane[0].stride);\n    color_plane[0].size = color_plane[0].stride * color_plane[0].height;\n    color_plane[0].offset = 0;\n    return color_plane[0].size;\n  }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) {\n    auto rgba = defaultRGBA();\n    fillColorPlanes(width, height, rgba, stride_align);\n    std::vector<ColorPlane> result(rgba.begin(), rgba.end());\n    return result;\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, std::array<ColorPlane, 1>& color_planes,\n                bool stride_align = true) {\n    return fillColorPlanes(width, height, color_planes, stride_align);\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, bool stride_align = true) {\n    auto rgba = defaultRGBA();\n    return fillColorPlanes(width, height, rgba, stride_align);\n  }\n};\n\n// Specifies x-x-x(8/16/32) bit single plane RGB/BGR variants\ntemplate <VideoFormat T>\nstruct VideoFormatSize<\n    T, std::enable_if_t<\n           T == VideoFormat::GXF_VIDEO_FORMAT_RGB || T == VideoFormat::GXF_VIDEO_FORMAT_BGR ||\n           T == VideoFormat::GXF_VIDEO_FORMAT_RGB16 || T == VideoFormat::GXF_VIDEO_FORMAT_BGR16 ||\n           T == VideoFormat::GXF_VIDEO_FORMAT_RGB32 || T == VideoFormat::GXF_VIDEO_FORMAT_BGR32>> {\n  std::array<ColorPlane, 1> defaultRGB() {\n    if (T == VideoFormat::GXF_VIDEO_FORMAT_RGB) {\n      return {ColorPlane(\"RGB\", 3)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_BGR) {\n      return {ColorPlane(\"BGR\", 3)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_RGB16) {\n      return {ColorPlane(\"RGB\", 3 * 2)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_BGR16) {\n      return {ColorPlane(\"BGR\", 3 * 2)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_RGB32) {\n      return {ColorPlane(\"RGB\", 3 * 4)};\n    } else {  // T == VideoFormat::GXF_VIDEO_FORMAT_BGR32\n      return {ColorPlane(\"BGR\", 3 * 4)};\n    }\n  }\n\n  uint64_t fillColorPlanes(uint32_t width, uint32_t height,\n                           std::array<ColorPlane, 1>& color_plane,\n                           bool stride_align = true) {\n    uint32_t widthEven = AlignToEvenDimension(width);\n    uint32_t heightEven = AlignToEvenDimension(height);\n    color_plane[0].width = widthEven;\n    color_plane[0].height = heightEven;\n    color_plane[0].stride = ComputeStrides(stride_align, color_plane[0].width,\n                            color_plane[0].bytes_per_pixel, color_plane[0].stride);\n    color_plane[0].size = color_plane[0].stride * color_plane[0].height;\n    color_plane[0].offset = 0;\n    return color_plane[0].size;\n  }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) {\n    auto rgb = defaultRGB();\n    fillColorPlanes(width, height, rgb, stride_align);\n    std::vector<ColorPlane> result(rgb.begin(), rgb.end());\n    return result;\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, std::array<ColorPlane, 1>& color_planes,\n                bool stride_align = true) {\n    return fillColorPlanes(width, height, color_planes, stride_align);\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, bool stride_align = true) {\n    auto rgb = defaultRGB();\n    return fillColorPlanes(width, height, rgb, stride_align);\n  }\n};\n\n// Specifies x-x-x(8/16/32) bit multi planar RGB/BGR variants\ntemplate <VideoFormat T>\nstruct VideoFormatSize<T, std::enable_if_t<T == VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_B8_G8_R8 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_R16_G16_B16 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_B16_G16_R16 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_B32_G32_R32>> {\n  std::array<ColorPlane, 3> defaultRGB() {\n    if (T == VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8) {\n      return {ColorPlane(\"R\", 1), ColorPlane(\"G\", 1), ColorPlane(\"B\", 1)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_B8_G8_R8) {\n      return {ColorPlane(\"B\", 1), ColorPlane(\"G\", 1), ColorPlane(\"R\", 1)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_R16_G16_B16) {\n      return {ColorPlane(\"R\", 2), ColorPlane(\"G\", 2), ColorPlane(\"B\", 2)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_B16_G16_R16) {\n      return {ColorPlane(\"B\", 2), ColorPlane(\"G\", 2), ColorPlane(\"R\", 2)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32) {\n      return {ColorPlane(\"R\", 4), ColorPlane(\"G\", 4), ColorPlane(\"B\", 4)};\n    } else {  // T == VideoFormat::GXF_VIDEO_FORMAT_B32_G32_R32\n      return {ColorPlane(\"B\", 4), ColorPlane(\"G\", 4), ColorPlane(\"R\", 4)};\n    }\n  }\n\n  uint64_t fillColorPlanes(uint32_t width, uint32_t height,\n                           std::array<ColorPlane, 3>& color_planes,\n                           bool stride_align = true) {\n    uint32_t widthEven = AlignToEvenDimension(width);\n    uint32_t heightEven = AlignToEvenDimension(height);\n\n    uint64_t size = 0;\n    for (size_t i = 0; i < color_planes.size(); ++i) {\n      color_planes[i].width = widthEven;\n      color_planes[i].height = heightEven;\n      color_planes[i].stride = ComputeStrides(stride_align, color_planes[i].width,\n                            color_planes[i].bytes_per_pixel, color_planes[i].stride);\n      color_planes[i].size = color_planes[i].stride * color_planes[i].height;\n      color_planes[i].offset = size;\n      size += color_planes[i].size;\n    }\n    return size;\n  }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) {\n    auto rgb = defaultRGB();\n    fillColorPlanes(width, height, rgb, stride_align);\n    std::vector<ColorPlane> result(rgb.begin(), rgb.end());\n    return result;\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, std::array<ColorPlane, 3>& color_planes,\n                bool stride_align = true) {\n    return fillColorPlanes(width, height, color_planes, stride_align);\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, bool stride_align = true) {\n    auto rgb = defaultRGB();\n    return fillColorPlanes(width, height, rgb, stride_align);\n  }\n};\n\n// Specifies x-x-x(8/16/32) bit multi planar RGBD variants\ntemplate <VideoFormat T>\nstruct VideoFormatSize<T, std::enable_if_t<T == VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8_D8 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_R16_G16_B16_D16 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32_D32>> {\n  std::array<ColorPlane, 4> defaultRGBD() {\n    if (T == VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8_D8) {\n      return {ColorPlane(\"R\", 1), ColorPlane(\"G\", 1), ColorPlane(\"B\", 1), ColorPlane(\"D\", 1)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_R16_G16_B16_D16) {\n      return {ColorPlane(\"R\", 2), ColorPlane(\"G\", 2), ColorPlane(\"B\", 2), ColorPlane(\"D\", 2)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32_D32) {\n      return {ColorPlane(\"R\", 4), ColorPlane(\"G\", 4), ColorPlane(\"B\", 4), ColorPlane(\"D\", 4)};\n    }\n  }\n\n  uint64_t fillColorPlanes(uint32_t width, uint32_t height,\n                           std::array<ColorPlane, 4>& color_planes,\n                           bool stride_align = true) {\n    uint32_t widthEven = AlignToEvenDimension(width);\n    uint32_t heightEven = AlignToEvenDimension(height);\n\n    uint64_t size = 0;\n    for (size_t i = 0; i < color_planes.size(); ++i) {\n      color_planes[i].width = widthEven;\n      color_planes[i].height = heightEven;\n      color_planes[i].stride = ComputeStrides(stride_align, color_planes[i].width,\n                            color_planes[i].bytes_per_pixel, color_planes[i].stride);\n      color_planes[i].size = color_planes[i].stride * color_planes[i].height;\n      color_planes[i].offset = size;\n      size += color_planes[i].size;\n    }\n    return size;\n  }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) {\n    auto rgbd = defaultRGBD();\n    fillColorPlanes(width, height, rgbd, stride_align);\n    std::vector<ColorPlane> result(rgbd.begin(), rgbd.end());\n    return result;\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, std::array<ColorPlane, 4>& color_planes,\n                bool stride_align = true) {\n    return fillColorPlanes(width, height, color_planes, stride_align);\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, bool stride_align = true) {\n    auto rgbd = defaultRGBD();\n    return fillColorPlanes(width, height, rgbd, stride_align);\n  }\n};\n\n// Specifies x-x-x-x(8/16/32) bit float single plane RGBD variants\ntemplate <VideoFormat T>\nstruct VideoFormatSize<T, std::enable_if_t<T == VideoFormat::GXF_VIDEO_FORMAT_RGBD8 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_RGBD16 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_RGBD32>> {\n  std::array<ColorPlane, 1> defaultRGBD() {\n    if (T == VideoFormat::GXF_VIDEO_FORMAT_RGBD8) {\n      return {ColorPlane(\"RGBD\", 4)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_RGBD16) {\n      return {ColorPlane(\"RGBD\", 8)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_RGBD32) {\n      return {ColorPlane(\"RGBD\", 16)};\n    }\n  }\n\n  uint64_t fillColorPlanes(uint32_t width, uint32_t height,\n                           std::array<ColorPlane, 1>& color_plane,\n                           bool stride_align = true) {\n    uint32_t widthEven = AlignToEvenDimension(width);\n    uint32_t heightEven = AlignToEvenDimension(height);\n\n    color_plane[0].width = widthEven;\n    color_plane[0].height = heightEven;\n    color_plane[0].stride = ComputeStrides(stride_align, color_plane[0].width,\n                            color_plane[0].bytes_per_pixel, color_plane[0].stride);\n    color_plane[0].size = color_plane[0].stride * color_plane[0].height;\n    color_plane[0].offset = 0;\n    return color_plane[0].size;\n  }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) {\n    auto rgbd = defaultRGBD();\n    fillColorPlanes(width, height, rgbd, stride_align);\n    std::vector<ColorPlane> result(rgbd.begin(), rgbd.end());\n    return result;\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, std::array<ColorPlane, 1>& color_planes,\n                bool stride_align = true) {\n    return fillColorPlanes(width, height, color_planes, stride_align);\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, bool stride_align = true) {\n    auto rgbd = defaultRGBD();\n    return fillColorPlanes(width, height, rgbd, stride_align);\n  }\n};\n\n// Specifies 32/64 bit float single plane Depth\ntemplate <VideoFormat T>\nstruct VideoFormatSize<T, std::enable_if_t<T == VideoFormat::GXF_VIDEO_FORMAT_D32F ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_D64F >> {\n  std::array<ColorPlane, 1> defaultDepth() {\n    if (T == VideoFormat::GXF_VIDEO_FORMAT_D32F) {\n      return {ColorPlane(\"D\", 4)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_D64F) {\n      return {ColorPlane(\"D\", 8)};\n    }\n  }\n\n  uint64_t fillColorPlanes(uint32_t width, uint32_t height,\n                           std::array<ColorPlane, 1>& color_plane,\n                           bool stride_align = true) {\n    uint32_t widthEven = AlignToEvenDimension(width);\n    uint32_t heightEven = AlignToEvenDimension(height);\n\n    color_plane[0].width = widthEven;\n    color_plane[0].height = heightEven;\n    color_plane[0].stride = ComputeStrides(stride_align, color_plane[0].width,\n                            color_plane[0].bytes_per_pixel, color_plane[0].stride);\n    color_plane[0].size = color_plane[0].stride * color_plane[0].height;\n    color_plane[0].offset = 0;\n    return color_plane[0].size;\n  }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) {\n    auto depth = defaultDepth();\n    fillColorPlanes(width, height, depth, stride_align);\n    std::vector<ColorPlane> result(depth.begin(), depth.end());\n    return result;\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, std::array<ColorPlane, 1>& color_planes,\n                bool stride_align = true) {\n    return fillColorPlanes(width, height, color_planes, stride_align);\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, bool stride_align = true) {\n    auto depth = defaultDepth();\n    return fillColorPlanes(width, height, depth, stride_align);\n  }\n};\n\n\n// Specifies x-x-x(8/16/32) bit single plane GRAY scale\ntemplate <VideoFormat T>\nstruct VideoFormatSize<T, std::enable_if_t<T == VideoFormat::GXF_VIDEO_FORMAT_GRAY ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_GRAY16 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_GRAY32 ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_GRAY32F>> {\n  std::array<ColorPlane, 1> defaultGray() {\n    if (T == VideoFormat::GXF_VIDEO_FORMAT_GRAY) {\n      return {ColorPlane(\"gray\", 1)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_GRAY16) {\n      return {ColorPlane(\"gray\", 2)};\n    } else {  // T == VideoFormat::GXF_VIDEO_FORMAT_GRAY32 & VideoFormat::GXF_VIDEO_FORMAT_GRAY32F\n      return {ColorPlane(\"gray\", 4)};\n    }\n  }\n\n  uint64_t fillColorPlanes(uint32_t width, uint32_t height,\n                           std::array<ColorPlane, 1>& color_plane,\n                           bool stride_align = true) {\n    uint32_t widthEven = AlignToEvenDimension(width);\n    uint32_t heightEven = AlignToEvenDimension(height);\n    color_plane[0].width = widthEven;\n    color_plane[0].height = heightEven;\n    color_plane[0].stride = ComputeStrides(stride_align, color_plane[0].width,\n                              color_plane[0].bytes_per_pixel, color_plane[0].stride);\n    color_plane[0].size = color_plane[0].stride * color_plane[0].height;\n    color_plane[0].offset = 0;\n    return color_plane[0].size;\n  }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) {\n    auto gray = defaultGray();\n    fillColorPlanes(width, height, gray, stride_align);\n    std::vector<ColorPlane> result(gray.begin(), gray.end());\n    return result;\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, std::array<ColorPlane, 1>& color_plane,\n                bool stride_align = true) {\n    return fillColorPlanes(width, height, color_plane, stride_align);\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, bool stride_align = true) {\n    auto gray = defaultGray();\n    return fillColorPlanes(width, height, gray, stride_align);\n  }\n};\n\n// Specifies x-x-x-x(16) bit single plane Bayer RAW16\ntemplate <VideoFormat T>\nstruct VideoFormatSize<T, std::enable_if_t<T == VideoFormat::GXF_VIDEO_FORMAT_RAW16_RGGB ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_RAW16_BGGR ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_RAW16_GRBG ||\n                                           T == VideoFormat::GXF_VIDEO_FORMAT_RAW16_GBRG>> {\n  std::array<ColorPlane, 1> defaultBayerRaw16() {\n    if (T == VideoFormat::GXF_VIDEO_FORMAT_RAW16_RGGB) {\n      return {ColorPlane(\"Raw_RGGB\", 4 * 2)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_RAW16_BGGR) {\n      return {ColorPlane(\"Raw_BGGR\", 4 * 2)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_RAW16_GRBG) {\n      return {ColorPlane(\"Raw_GRBG\", 4 * 2)};\n    } else if (T == VideoFormat::GXF_VIDEO_FORMAT_RAW16_GBRG) {\n      return {ColorPlane(\"Raw_GBRG\", 4 * 2)};\n    }\n  }\n\n  uint64_t fillColorPlanes(uint32_t width, uint32_t height,\n                           std::array<ColorPlane, 1>& color_plane,\n                           bool stride_align = true) {\n    uint32_t widthEven = AlignToEvenDimension(width);\n    uint32_t heightEven = AlignToEvenDimension(height);\n    color_plane[0].width = widthEven;\n    color_plane[0].height = heightEven;\n    color_plane[0].stride = ComputeStrides(stride_align, color_plane[0].width,\n                              color_plane[0].bytes_per_pixel, color_plane[0].stride);\n    color_plane[0].size = color_plane[0].stride * color_plane[0].height;\n    color_plane[0].offset = 0;\n    return color_plane[0].size;\n  }\n\n  std::vector<ColorPlane> getDefaultColorPlanes(uint32_t width, uint32_t height,\n                                                bool stride_align = true) {\n    auto bayerRaw16 = defaultBayerRaw16();\n    fillColorPlanes(width, height, bayerRaw16, stride_align);\n    std::vector<ColorPlane> result(bayerRaw16.begin(), bayerRaw16.end());\n    return result;\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, std::array<ColorPlane, 1>& color_plane,\n                bool stride_align = true) {\n    return fillColorPlanes(width, height, color_plane, stride_align);\n  }\n\n  uint64_t size(uint32_t width, uint32_t height, bool stride_align = true) {\n    auto bayerRaw16 = defaultBayerRaw16();\n    return fillColorPlanes(width, height, bayerRaw16, stride_align);\n  }\n};\n\n// Descriptor for a VideoBuffer\nstruct VideoBufferInfo {\n  // width of a video frame\n  uint32_t width;\n  // height of a video frame\n  uint32_t height;\n  // color format of a video frame\n  VideoFormat color_format;\n  // Color plane info\n  std::vector<ColorPlane> color_planes;\n  // surface memory layout of a video frame\n  SurfaceLayout surface_layout;\n};\n\n// A media type which stores information corresponding to a video frame\n// resize(...) function is used to allocate memory for the video frame based on\n// height, width and color format of the frame\nclass VideoBuffer {\n public:\n  VideoBuffer() = default;\n\n  ~VideoBuffer() { memory_buffer_.freeBuffer(); }\n\n  VideoBuffer(const VideoBuffer&) = delete;\n\n  VideoBuffer(VideoBuffer&& other) { *this = std::move(other); }\n\n  VideoBuffer& operator=(const VideoBuffer&) = delete;\n\n  VideoBuffer& operator=(VideoBuffer&& other) {\n    buffer_info_ = other.buffer_info_;\n    memory_buffer_ = std::move(other.memory_buffer_);\n\n    return *this;\n  }\n\n  // Resizes the video frame and allocates the corresponding memory with the allocator provided\n  // Any data previously stored in the frame would be freed\n  template <VideoFormat C>\n  Expected<void> resize(uint32_t width, uint32_t height, SurfaceLayout layout,\n                        MemoryStorageType storage_type, Handle<Allocator> allocator,\n                        bool stride_align = true) {\n    VideoTypeTraits<C> video_type;\n    VideoFormatSize<C> color_format;\n    uint64_t size = color_format.size(width, height, stride_align);\n    auto color_planes = color_format.getDefaultColorPlanes(width, height, stride_align);\n    VideoBufferInfo buffer_info{width, height, video_type.value, std::move(color_planes), layout};\n    return resizeCustom(std::move(buffer_info), size, storage_type, allocator);\n  }\n\n  // Type of the callback function to release memory passed to the VideoFrame using the\n  // wrapMemory method\n  using release_function_t = MemoryBuffer::release_function_t;\n\n  // Wrap existing memory inside the VideoBuffer. A callback function of type release_function_t\n  // may be passed that will be called when the VideoBuffer wants to release the memory.\n  Expected<void> wrapMemory(VideoBufferInfo buffer_info, uint64_t size,\n                            MemoryStorageType storage_type, void* pointer,\n                            release_function_t release_func);\n\n  // Moves the existing video buffer to a tensor element specified by the handle\n  Expected<void> moveToTensor(Handle<Tensor>& tensor);\n\n  // Moves the existing video buffer to a tensor element specified by the pointer\n  Expected<void> moveToTensor(Tensor* tensor);\n\n  // Moves memory buffer from tensor to video buffer works on rank 2 and 3 tensors,\n  // ordering of dimensions in tensor is assumed to be [whc]\n  // OBS: If successful, tensor destructor will be called and handle set to null\n  template <VideoFormat C>\n  Expected<void> createFromTensor(Handle<Tensor>& tensor, SurfaceLayout layout,\n                                  bool stride_align = true);\n\n  // VideoBufferInfo of the video frame\n  VideoBufferInfo video_frame_info() const { return buffer_info_; }\n\n  // The type of memory where the frame data is stored.\n  MemoryStorageType storage_type() const { return memory_buffer_.storage_type(); }\n\n  // Size of the video frame in bytes\n  uint64_t size() const { return memory_buffer_.size(); }\n\n  // Raw pointer to the first byte of the video frame\n  byte* pointer() const { return memory_buffer_.pointer(); }\n\n  // Resizes the video frame and allocates the corresponding memory with the allocator provided\n  // Any data previously stored in the frame would be freed\n  Expected<void> resizeCustom(VideoBufferInfo buffer_info, uint64_t size,\n                              MemoryStorageType storage_type, Handle<Allocator> allocator);\n\n  // Helper function to get primitive types for color formats that are valid (planar) for moving\n  // from  Tensor to VideoBuffer, and vice versa. If color_format is not valid,\n  // PrimitiveType::kCustom is returned\n  static Expected<PrimitiveType> getPlanarPrimitiveType(VideoFormat color_format) {\n    PrimitiveType primitive_type;\n    switch (color_format) {\n        case   VideoFormat::GXF_VIDEO_FORMAT_NV12:          // BT.601 2 planes Y, UV\n        case   VideoFormat::GXF_VIDEO_FORMAT_RGBA:          // RGBA-8-8-8-8 single plane\n        case   VideoFormat::GXF_VIDEO_FORMAT_BGRA:          // BGRA-8-8-8-8 single plane\n        case   VideoFormat::GXF_VIDEO_FORMAT_ARGB:          // ARGB-8-8-8-8 single plane\n        case   VideoFormat::GXF_VIDEO_FORMAT_ABGR:          // ABGR-8-8-8-8 single plane\n        case   VideoFormat::GXF_VIDEO_FORMAT_RGBX:          // RGBX-8-8-8-8 single plane\n        case   VideoFormat::GXF_VIDEO_FORMAT_BGRX:          // BGRX-8-8-8-8 single plane\n        case   VideoFormat::GXF_VIDEO_FORMAT_XRGB:          // XRGB-8-8-8-8 single plane\n        case   VideoFormat::GXF_VIDEO_FORMAT_XBGR:          // XBGR-8-8-8-8 single plane\n        case   VideoFormat::GXF_VIDEO_FORMAT_RGB:           // RGB-8-8-8 single plane\n        case   VideoFormat::GXF_VIDEO_FORMAT_BGR:           // BGR-8-8-8 single plane\n        case   VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8:      // RGB - unsigned 8 bit multiplanar\n        case   VideoFormat::GXF_VIDEO_FORMAT_B8_G8_R8:      // BGR - unsigned 8 bit multiplanar\n        case   VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8_D8:   // RGBD unsigned 8 bit multiplanar\n        case   VideoFormat::GXF_VIDEO_FORMAT_RGBD8:         // RGBD 8 bit unsigned single plane\n        case   VideoFormat::GXF_VIDEO_FORMAT_GRAY: {        // 8 bit GRAY scale single plane\n          primitive_type = PrimitiveType::kUnsigned8;\n        } break;\n\n        case  VideoFormat::GXF_VIDEO_FORMAT_GRAY16:             // 16 bit GRAY scale single plane\n        case  VideoFormat::GXF_VIDEO_FORMAT_RGB16:              // RGB-16-16-16 single plane\n        case  VideoFormat::GXF_VIDEO_FORMAT_BGR16:              // BGR-16-16-16 single plane\n        case  VideoFormat::GXF_VIDEO_FORMAT_R16_G16_B16:        // RGB unsigned 16 bit multiplanar\n        case  VideoFormat::GXF_VIDEO_FORMAT_B16_G16_R16:        // BGR unsigned 16 bit multiplanar\n        case  VideoFormat::GXF_VIDEO_FORMAT_R16_G16_B16_D16:    // RGBD unsigned 16 bit multiplanar\n        case  VideoFormat::GXF_VIDEO_FORMAT_RGBD16: {           // RGBD 16 bit unsigned single plane\n          primitive_type = PrimitiveType::kUnsigned16;\n        } break;\n\n        case VideoFormat::GXF_VIDEO_FORMAT_GRAY32:              // 32 bit GRAY scale single plane\n        case VideoFormat::GXF_VIDEO_FORMAT_RGB32:               // RGB-32-32-32 single plane\n        case VideoFormat::GXF_VIDEO_FORMAT_BGR32:               // BGR-32-32-32 single plane\n        case VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32:         // RGB unsigned 32 bit multiplanar\n        case VideoFormat::GXF_VIDEO_FORMAT_B32_G32_R32:         // BGR unsigned 32 bit multiplanar\n        case VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32_D32:     // RGBD unsigned 32 bit multiplanar\n        case VideoFormat::GXF_VIDEO_FORMAT_RGBD32: {            // RGBD 32 bit unsigned single plane\n          primitive_type = PrimitiveType::kUnsigned32;\n        } break;\n\n        case VideoFormat::GXF_VIDEO_FORMAT_D32F:                 // Depth 32 bit float single plane\n        case VideoFormat::GXF_VIDEO_FORMAT_GRAY32F: {            // GRAY 32 bit float single plane\n          primitive_type = PrimitiveType::kFloat32;\n        } break;\n\n        case VideoFormat::GXF_VIDEO_FORMAT_D64F: {               // Depth 64 bit float single plane\n          primitive_type = PrimitiveType::kFloat64;\n        } break;\n\n        case   VideoFormat::GXF_VIDEO_FORMAT_CUSTOM: {        // CUSTOM undefined\n          primitive_type = PrimitiveType::kCustom;\n        } break;\n\n        default: {                                               // Non-planar type given\n          GXF_LOG_ERROR(\"VideoFormat is of non-planar color format (%ld),\"\n                        \" which cannot be moved from tensor\", static_cast<int64_t>(color_format));\n          return Unexpected{GXF_INVALID_DATA_FORMAT};\n        } break;\n    }\n\n    return primitive_type;\n  }\n\n private:\n  VideoBufferInfo buffer_info_;\n  MemoryBuffer memory_buffer_;\n};\n\ntemplate <VideoFormat C>\nExpected<void> VideoBuffer::createFromTensor(Handle<Tensor>& tensor,\n                                             SurfaceLayout layout,\n                                             bool stride_align) {\n  if (!tensor) {\n    GXF_LOG_ERROR(\"createFromTensor received invalid tensor handle\");\n    return Unexpected{GXF_ARGUMENT_NULL};\n  }\n\n  // Tensor must have rank 2 (single-plane image) or 3 (multi-plane image)\n  const uint32_t rank = tensor->rank();\n  if ((rank < 2) || (rank > 3)) {\n      GXF_LOG_ERROR(\"Tensor cannot be moved to VideoBuffer.\"\n                    \" Invalid rank=[%d], should be 2 or 3\", rank);\n      return Unexpected{GXF_INVALID_DATA_FORMAT};\n  }\n\n  // Get primitive type and sanity check color format\n  VideoTypeTraits<C> video_type;\n  VideoFormatSize<C> color_format;\n  Expected<PrimitiveType> primitive_type = getPlanarPrimitiveType(video_type.value);\n  if (!primitive_type) { return ForwardError(primitive_type); }\n\n  // Ensure video buffer and tensor has same data type\n  PrimitiveType primite_type_tensor = tensor->element_type();\n\n  if (primitive_type.value() != primite_type_tensor) {\n      GXF_LOG_ERROR(\"Type of video buffer (%d) is different from\"\n                    \" type of tensor (%d)\", static_cast<int32_t>(primitive_type.value()),\n                    static_cast<int32_t>(primite_type_tensor));\n      return Unexpected{GXF_INVALID_DATA_FORMAT};\n  }\n\n  // Get tensor dimensions\n  auto width = static_cast<uint32_t>(tensor->shape().dimension(0));\n  auto height = static_cast<uint32_t>(tensor->shape().dimension(1));\n  auto channels = static_cast<uint32_t>(tensor->shape().dimension(2));\n\n  // Get color planes\n  auto color_planes = color_format.getDefaultColorPlanes(width, height, stride_align);\n\n  // Sanity check that number of tensor channels corresponds to video format\n  if (channels != color_planes.size()) {\n      GXF_LOG_ERROR(\"Number of channels in tensor (%u) is \"\n                    \" different from video buffer (%lu)\", channels, color_planes.size());\n      return Unexpected{GXF_INVALID_DATA_FORMAT};\n  }\n\n  // Set buffer info\n  VideoBufferInfo buffer_info{width, height, video_type.value, std::move(color_planes), layout};\n  buffer_info_ = std::move(buffer_info);\n\n  // Move memory buffer\n  memory_buffer_ = tensor->move_buffer();\n\n  // Explicit call to destructor as tensor is no longer usable (also set to Null handle)\n  tensor->~Tensor();\n  tensor = Handle<Tensor>::Null();\n\n  return Success;\n}\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_MULTIMEDIA_VIDEO_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/network/tcp_client.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_NETWORK_TCP_CLIENT_HPP_\n#define NVIDIA_GXF_NETWORK_TCP_CLIENT_HPP_\n\n#include \"gxf/network/tcp_codelet.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Codelet that functions as a client in a TCP connection\nclass TcpClient : public TcpCodelet {\n protected:\n  Expected<void> openSockets() override;\n  Expected<void> reconnectSockets() override;\n  Expected<void> closeSockets() override;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_NETWORK_TCP_CLIENT_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/network/tcp_client_socket.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_NETWORK_TCP_CLIENT_SOCKET_HPP_\n#define NVIDIA_GXF_NETWORK_TCP_CLIENT_SOCKET_HPP_\n\n#include <sys/poll.h>\n\n#include <utility>\n#include <vector>\n\n#include \"gxf/serialization/endpoint.hpp\"\n#include \"gxf/serialization/entity_serializer.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Header containing metadata for TCP packets\n#pragma pack(push, 1)\nstruct TcpHeader {\n  uint64_t payload_size;  // Size of the payload in bytes\n  uint64_t entity_count;  // Number of entities in the payload\n};\n#pragma pack(pop)\n\n// Header containing metadata for entities\n#pragma pack(push, 1)\nstruct MessageHeader {\n  uint64_t channel_id;  // Hashed channel name\n};\n#pragma pack(pop)\n\n// TCP message consisting of a header and an entity\nstruct TcpMessage {\n  MessageHeader header;\n  Entity entity;\n};\n\n// Endpoint for exchanging data over a TCP connection.\n// Connection must be established prior to use.\n// Packets are organized in the following format:\n//\n//   | TCP Header || Message Header | Entity | ... | ... |\n//\n// Each entity will have a header prepended to form a message.\n// Messages will be grouped together to form the packet.\n// Little-endian is used over big-endian for better performance on x86 and arm platforms.\nclass TcpClientSocket : public Endpoint {\n public:\n  TcpClientSocket() = default;\n  ~TcpClientSocket() = default;\n  TcpClientSocket(const TcpClientSocket& other) = delete;\n  TcpClientSocket(TcpClientSocket&& other) { *this = std::move(other); }\n  TcpClientSocket& operator=(const TcpClientSocket& other) = delete;\n  TcpClientSocket& operator=(TcpClientSocket&& other) {\n    fd_socket_ = other.fd_socket_;\n    connected_ = other.connected_;\n    maximum_attempts_ = other.maximum_attempts_;\n    other.fd_socket_ = -1;\n    other.connected_ = false;\n    other.maximum_attempts_ = 1;\n    return *this;\n  }\n\n  Expected<void> open();\n  Expected<void> close();\n\n  gxf_result_t write_abi(const void* data, size_t size, size_t* bytes_written) override;\n  gxf_result_t read_abi(void* data, size_t size, size_t* bytes_read) override;\n\n  // Initializes endpoint with a connected socket.\n  // This is used by TcpServerSocket when establishing a connection.\n  Expected<void> openConnectedSocket(int socket);\n  // Closes and reopens socket to enable reconnecting.\n  // Socket needs to be reset after disconnecting to be used again.\n  Expected<void> reopenSocket();\n  // Configures the maximum number of attempts\n  void setMaximumAttempts(size_t attempts) { maximum_attempts_ = attempts; }\n  // Returns true if the socket has data available for reading\n  bool available() const;\n  // Returns true if the socket is connected\n  bool connected() const { return connected_; }\n  // Attempts to connect to a TCP server at the given IP address.\n  // Call will fail if IP address is invalid or server is unreachable.\n  Expected<void> connect(const char* address, uint16_t port);\n\n  // Sends a set of messages via TCP\n  Expected<size_t> sendMessages(const std::vector<TcpMessage>& messages,\n                                EntitySerializer* serializer);\n  // Receives a set of messages via TCP\n  Expected<std::vector<TcpMessage>> receiveMessages(gxf_context_t context,\n                                                    EntitySerializer* serializer);\n\n private:\n  // Sends a message to the socket\n  Expected<size_t> sendMessage(const TcpMessage& message, EntitySerializer* serializer);\n  // Receives a message from the socket\n  Expected<TcpMessage> receiveMessage(gxf_context_t context, EntitySerializer* serializer);\n  // Sends a TCP header to the socket\n  Expected<size_t> sendTcpHeader(TcpHeader header);\n  // Receives a TCP header from the socket\n  Expected<TcpHeader> receiveTcpHeader();\n  // Sends a message header to the socket\n  Expected<size_t> sendMessageHeader(MessageHeader header);\n  // Receives a message header from the socket\n  Expected<MessageHeader> receiveMessageHeader();\n\n  // Socket file descriptor\n  int fd_socket_{-1};\n\n  // TCP connection state\n  bool connected_{false};\n  // Maximum number of attempts for each I/O operation\n  size_t maximum_attempts_{1};\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_NETWORK_TCP_CLIENT_SOCKET_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/network/tcp_codelet.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_NETWORK_TCP_CODELET_HPP_\n#define NVIDIA_GXF_NETWORK_TCP_CODELET_HPP_\n\n#include <future>\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#include \"gxf/network/tcp_server_socket.hpp\"\n#include \"gxf/serialization/entity_serializer.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Abstract codelet supporting asynchronous TCP client/server\nclass TcpCodelet : public Codelet {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n\n  gxf_result_t tick() override;\n  gxf_result_t stop() override;\n\n protected:\n  // Asynchronously monitors client socket and receivers. If data is available from either source,\n  // schedules the TcpServer via the AsynchronousSchedulingTerm. Also handles connecting client\n  // socket.\n  Expected<void> monitor();\n\n  virtual Expected<void> openSockets() = 0;\n  virtual Expected<void> reconnectSockets() = 0;\n  virtual Expected<void> closeSockets() = 0;\n\n  Parameter<std::vector<Handle<Receiver>>> receivers_;\n  Parameter<std::vector<Handle<Transmitter>>> transmitters_;\n  Parameter<Handle<EntitySerializer>> entity_serializer_;\n  Parameter<std::string> address_;\n  Parameter<int> port_;\n  Parameter<uint64_t> timeout_ms_;\n  Parameter<std::string> timeout_text_;\n  Parameter<uint64_t> maximum_attempts_;\n  Parameter<int64_t> max_msg_delay_ms_;\n  Parameter<int64_t> max_duration_ms_;\n  Parameter<uint64_t> max_connection_attempts_;\n  Parameter<Handle<AsynchronousSchedulingTerm>> async_scheduling_term_;\n\n  int64_t timeout_ns_;\n\n  // Maps channel IDs to transmitters\n  std::unordered_map<uint64_t, Handle<Transmitter>> channel_map_;\n  // TCP client socket\n  TcpClientSocket client_socket_;\n  // Tracks monitor thread result\n  std::future<Expected<void>> monitor_future_;\n  // Stores receivers with available data\n  std::vector<Handle<Receiver>> available_receivers_;\n  // Stores messages read from receivers\n  std::vector<TcpMessage> rx_messages_;\n  // Stores messages read from socket\n  std::vector<TcpMessage> tx_messages_;\n  // Time when last message was received\n  Expected<std::chrono::time_point<std::chrono::steady_clock>> last_msg_timestamp_ =\n      Unexpected{GXF_UNINITIALIZED_VALUE};\n  // Time when monitor thread began\n  Expected<std::chrono::time_point<std::chrono::steady_clock>> monitor_start_timestamp_ =\n      Unexpected{GXF_UNINITIALIZED_VALUE};\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_NETWORK_TCP_CODELET_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/network/tcp_server.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_NETWORK_TCP_SERVER_HPP_\n#define NVIDIA_GXF_NETWORK_TCP_SERVER_HPP_\n\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#include \"gxf/network/tcp_codelet.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Codelet that functions as a server in a TCP connection\nclass TcpServer : public TcpCodelet {\n protected:\n  Expected<void> openSockets() override;\n  Expected<void> reconnectSockets() override;\n  Expected<void> closeSockets() override;\n\n private:\n  // TCP server socket\n  TcpServerSocket server_socket_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_NETWORK_TCP_SERVER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/network/tcp_server_socket.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_NETWORK_TCP_SERVER_SOCKET_HPP_\n#define NVIDIA_GXF_NETWORK_TCP_SERVER_SOCKET_HPP_\n\n#include <string>\n\n#include \"gxf/network/tcp_client_socket.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Object for managing a TCP socket on the server side.\n// Connection must be established prior to use.\n// Uses a TcpClientSocket as a proxy for communication.\nclass TcpServerSocket {\n public:\n  TcpServerSocket(const char* address, uint16_t port)\n    : address_{address}, port_{port}, socket_{-1} {}\n  TcpServerSocket() : address_{\"0.0.0.0\"}, port_{0}, socket_{-1} {}\n  ~TcpServerSocket() = default;\n  TcpServerSocket(const TcpServerSocket& other) = delete;\n  TcpServerSocket(TcpServerSocket&& other) = default;\n  TcpServerSocket& operator=(const TcpServerSocket& other) = delete;\n  TcpServerSocket& operator=(TcpServerSocket&& other) = default;\n\n  // Initializes server socket.\n  // Creates socket file descriptor.\n  // Binds IP address to socket.\n  // Listens for incoming connections on socket.\n  Expected<void> open();\n  // Deinitializes server socket.\n  // Destroys socket file descriptor.\n  Expected<void> close();\n  // Attempts to connect to a TCP client.\n  // Returns a TCP client socket that can be used as an endpoint.\n  // Call will fail if there are no clients requesting to connect.\n  Expected<TcpClientSocket> connect();\n\n private:\n  // Server address\n  std::string address_;\n  // Server port\n  uint16_t port_;\n  // Socket file descriptor\n  int socket_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_NETWORK_TCP_SERVER_SOCKET_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/npp/nppi_mul_c.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_NPP_NPPI_MUL_C_HPP\n#define NVIDIA_GXF_NPP_NPPI_MUL_C_HPP\n\n#include <vector>\n\n#include \"gxf/core/parameter_parser_std.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Multiplies a CUDA tensor with a constant factor using NPP.\nclass NppiMulC : public Codelet {\n public:\n  virtual ~NppiMulC() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t tick() override;\n\n private:\n  Parameter<Handle<Receiver>> in_;\n  Parameter<std::vector<double>> factor_;\n  Parameter<Handle<Allocator>> pool_;\n  Parameter<Handle<Transmitter>> out_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/npp/nppi_set.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_NPP_NPPI_SET_HPP\n#define NVIDIA_GXF_NPP_NPPI_SET_HPP\n\n#include <array>\n#include <vector>\n\n#include \"gxf/core/parameter_parser_std.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Creates a CUDA tensor with constant values using NPP.\nclass NppiSet : public Codelet {\n public:\n  virtual ~NppiSet() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t tick() override;\n\n private:\n  Parameter<int32_t> rows_;\n  Parameter<int32_t> columns_;\n  Parameter<int32_t> channels_;\n  Parameter<Handle<Allocator>> pool_;\n  Parameter<std::vector<double>> value_;\n  Parameter<Handle<Transmitter>> out_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/rmm/rmm_allocator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_RMM_RMM_ALLOCATOR_HPP_\n#define NVIDIA_GXF_RMM_RMM_ALLOCATOR_HPP_\n\n#include <cuda_runtime.h>\n#include <cuda_runtime_api.h>\n\n#include <memory>\n#include <shared_mutex>  // NOLINT\n#include <string>\n#include <unordered_map>\n#include <utility>\n\n#include \"common/assert.hpp\"\n#include \"common/logger.hpp\"\n#include \"gxf/core/parameter.hpp\"\n#include \"gxf/cuda/cuda_allocator.hpp\"\n#include \"gxf/std/resources.hpp\"\n#include \"rmm/device_buffer.hpp\"\n#include \"rmm/mr/device/cuda_async_memory_resource.hpp\"\n#include \"rmm/mr/device/pool_memory_resource.hpp\"\n#include \"rmm/mr/host/pinned_memory_resource.hpp\"\nnamespace nvidia {\nnamespace gxf {\n\n// RMM based device memory allocator\nclass RMMAllocator : public CudaAllocator {\n public:\n  RMMAllocator() = default;\n  ~RMMAllocator() = default;\n\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t is_available_abi(uint64_t size) override;\n  gxf_result_t is_rmm_available_abi(uint64_t size, MemoryStorageType type);\n  gxf_result_t allocate_abi(uint64_t size, int32_t storage_type, void** pointer) override;\n  gxf_result_t allocate_async_abi(uint64_t size, void** pointer,\n                                  cudaStream_t stream) override;\n  gxf_result_t free_async_abi(void* pointer, cudaStream_t stream) override;\n  gxf_result_t free_abi(void* pointer) override;\n\n  Expected<size_t> get_pool_size(MemoryStorageType type) const override;\n\n private:\n  Parameter<std::string> device_memory_initial_size_;\n  Parameter<std::string> device_memory_max_size_;\n  Parameter<std::string> host_memory_initial_size_;\n  Parameter<std::string> host_memory_max_size_;\n  Resource<Handle<GPUDevice>> gpu_device_;\n\n  AllocatorStage stage_{AllocatorStage::kUninitialized};\n  cudaStream_t stream_ = {};\n  size_t device_max_memory_pool_size_;\n  size_t host_max_memory_pool_size_;\n\n  // Create a CUDA memory resource\n  std::unique_ptr<rmm::mr::cuda_memory_resource> device_mr_;\n  std::unique_ptr<rmm::mr::pool_memory_resource<rmm::mr::cuda_memory_resource>> pool_mr_device_;\n  std::unique_ptr<rmm::mr::pinned_memory_resource> pinned_mr_;\n  std::unique_ptr<rmm::mr::pool_memory_resource<rmm::mr::pinned_memory_resource>> pool_mr_host_;\n\n  mutable std::shared_mutex shared_mutex_;\n  std::unordered_map<void*, std::pair<std::size_t, MemoryStorageType>> pool_map_ = {};\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_RMM_RMM_ALLOCATOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/component_serializer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_COMPONENT_SERIALIZER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_COMPONENT_SERIALIZER_HPP_\n\n#include <functional>\n#include <shared_mutex>  // NOLINT\n#include <unordered_map>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/serialization/endpoint.hpp\"\n#include \"gxf/serialization/tid_hash.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Interface for serializing components\nclass ComponentSerializer : public Component {\n public:\n  virtual ~ComponentSerializer() = default;\n\n  // Checks if the serializer supports the given component type\n  // Searches serializer map by default\n  virtual gxf_result_t is_supported_abi(gxf_tid_t tid) {\n    return getSerializer(tid) ? GXF_SUCCESS : GXF_FAILURE;\n  }\n  // Serializes component and writes to endpoint\n  // Returns the size of the serialized component in bytes\n  // Uses functions in serializer map by default\n  virtual gxf_result_t serialize_component_abi(gxf_uid_t cid, Endpoint* endpoint, uint64_t* size);\n  // Reads from endpoint and deserializes component\n  // Uses functions in serializer map by default\n  virtual gxf_result_t deserialize_component_abi(gxf_uid_t cid, Endpoint* endpoint);\n\n  // C++ API wrappers\n  bool isSupported(gxf_tid_t tid) { return is_supported_abi(tid) == GXF_SUCCESS; }\n  Expected<size_t> serializeComponent(UntypedHandle component, Endpoint* endpoint);\n  Expected<void> deserializeComponent(UntypedHandle component, Endpoint* endpoint);\n\n protected:\n  // Serializer function handle\n  // Takes a component pointer and an endpoint pointer as input\n  // Returns the size of the serialized component in bytes\n  using Serializer = std::function<Expected<size_t>(void*, Endpoint*)>;\n  // Deserializer function handle\n  // Takes a component pointer and an endpoint pointer as input\n  using Deserializer = std::function<Expected<void>(void*, Endpoint*)>;\n\n  // Returns a serializer for the given type ID\n  Expected<Serializer> getSerializer(gxf_tid_t tid) const;\n  // Returns a deserializer for the given type ID\n  Expected<Deserializer> getDeserializer(gxf_tid_t tid) const;\n  // Adds a serializer for the given type ID\n  Expected<void> setSerializer(gxf_tid_t tid, Serializer serializer);\n  // Adds a deserializer for the given type ID\n  Expected<void> setDeserializer(gxf_tid_t tid, Deserializer deserializer);\n  // Removes a serializer for the given type ID\n  Expected<void> clearSerializer(gxf_tid_t tid) { return setSerializer(tid, nullptr); }\n  // Removes a deserializer for the given type ID\n  Expected<void> clearDeserializer(gxf_tid_t tid) { return setDeserializer(tid, nullptr); }\n\n  // Returns a serializer for the given type\n  template <typename T>\n  Expected<Serializer> getSerializer() const {\n    gxf_tid_t tid;\n    return ExpectedOrCode(GxfComponentTypeId(context(), TypenameAsString<T>(), &tid))\n        .and_then([&](){ return getSerializer(tid); });\n  }\n  // Returns a deserializer for the given type\n  template <typename T>\n  Expected<Deserializer> getDeserializer() const {\n    gxf_tid_t tid;\n    return ExpectedOrCode(GxfComponentTypeId(context(), TypenameAsString<T>(), &tid))\n        .and_then([&](){ return getDeserializer(tid); });\n  }\n  // Adds a serializer for the given type\n  template <typename T>\n  Expected<void> setSerializer(Serializer serializer) {\n    gxf_tid_t tid;\n    return ExpectedOrCode(GxfComponentTypeId(context(), TypenameAsString<T>(), &tid))\n        .and_then([&](){ return setSerializer(tid, serializer); });\n  }\n  // Adds a deserializer for the given type\n  template <typename T>\n  Expected<void> setDeserializer(Deserializer deserializer) {\n    gxf_tid_t tid;\n    return ExpectedOrCode(GxfComponentTypeId(context(), TypenameAsString<T>(), &tid))\n        .and_then([&](){ return setDeserializer(tid, deserializer); });\n  }\n  // Removes a serializer for the given type\n  template <typename T>\n  Expected<void> clearSerializer(Serializer serializer) {\n    gxf_tid_t tid;\n    return ExpectedOrCode(GxfComponentTypeId(context(), TypenameAsString<T>(), &tid))\n        .and_then([&](){ return clearSerializer(tid); });\n  }\n  // Removes a deserializer for the given type\n  template <typename T>\n  Expected<void> clearDeserializer(Deserializer deserializer) {\n    gxf_tid_t tid;\n    return ExpectedOrCode(GxfComponentTypeId(context(), TypenameAsString<T>(), &tid))\n        .and_then([&](){ return clearDeserializer(tid); });\n  }\n\n private:\n  // Structure for organizing serialize-deserialize function pairs\n  struct SerializerFunctions {\n    Serializer serializer;\n    Deserializer deserializer;\n  };\n\n  // Table that maps component type ID to serializer functions\n  std::unordered_map<gxf_tid_t, SerializerFunctions, TidHash> serializer_map_;\n  // Mutex to guard concurrent access to serializer map\n  mutable std::shared_timed_mutex mutex_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_COMPONENT_SERIALIZER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/endpoint.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_ENDPOINT_HPP_\n#define NVIDIA_GXF_SERIALIZATION_ENDPOINT_HPP_\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/std/allocator.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Interface for exchanging data external to an application graph\nclass Endpoint : public Component {\n public:\n  virtual ~Endpoint() = default;\n\n  // Returns `GXF_SUCCESS` if endpoint can be written to\n  virtual gxf_result_t is_write_available_abi() { return GXF_SUCCESS; }\n  // Returns `GXF_SUCCESS` if endpoint can be read from\n  virtual gxf_result_t is_read_available_abi() { return GXF_SUCCESS; }\n  // Writes data to the endpoint and returns the number of bytes written\n  virtual gxf_result_t write_abi(const void* data, size_t size, size_t* bytes_written) = 0;\n  // Reads data from the endpoint and returns the number of bytes read\n  virtual gxf_result_t read_abi(void* data, size_t size, size_t* bytes_read) = 0;\n  // Writes pointer to the data and size of the data to a vector\n  virtual gxf_result_t write_ptr_abi(const void* pointer, size_t size, MemoryStorageType type) {\n    return GXF_NOT_IMPLEMENTED;\n  }\n\n  // C++ API wrappers\n  bool isWriteAvailable();\n  bool isReadAvailable();\n  Expected<size_t> write(const void* data, size_t size);\n  Expected<size_t> read(void* data, size_t size);\n  Expected<void> write_ptr(const void* pointer, size_t size, MemoryStorageType type);\n\n  // Writes an object of type T to the endpoint\n  template <typename T>\n  Expected<size_t> writeTrivialType(const T* object) { return write(object, sizeof(T)); }\n\n  // Reads an object of type T from the endpoint\n  template <typename T>\n  Expected<size_t> readTrivialType(T* object) { return read(object, sizeof(T)); }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_ENDPOINT_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/entity_recorder.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_ENTITY_RECORDER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_ENTITY_RECORDER_HPP_\n\n#include <string>\n\n#include \"gxf/serialization/entity_serializer.hpp\"\n#include \"gxf/serialization/file_stream.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Records incoming entities by serializaing and writing to a file.\n// Uses one file to store binary data and a second file as an index to enable random-access.\nclass EntityRecorder : public Codelet {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n  gxf_result_t tick() override;\n\n private:\n  Parameter<Handle<Receiver>> receiver_;\n  Parameter<Handle<EntitySerializer>> entity_serializer_;\n  Parameter<std::string> directory_;\n  Parameter<std::string> basename_;\n  Parameter<bool> flush_on_tick_;\n\n  // File stream for data index\n  FileStream index_file_stream_;\n  // File stream for binary data\n  FileStream binary_file_stream_;\n  // Offset into binary file\n  size_t binary_file_offset_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_ENTITY_RECORDER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/entity_replayer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_ENTITY_REPLAYER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_ENTITY_REPLAYER_HPP_\n\n#include <string>\n\n#include \"gxf/serialization/entity_serializer.hpp\"\n#include \"gxf/serialization/file_stream.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Replays entities by reading and deserializing from a file.\n// The file is processed sequentially and a single entity is published per tick.\nclass EntityReplayer : public Codelet {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n  gxf_result_t tick() override;\n\n private:\n  Parameter<Handle<Transmitter>> transmitter_;\n  Parameter<Handle<EntitySerializer>> entity_serializer_;\n  Parameter<Handle<gxf::BooleanSchedulingTerm>> boolean_scheduling_term_;\n  Parameter<std::string> directory_;\n  Parameter<std::string> basename_;\n  Parameter<size_t> batch_size_;\n  Parameter<bool> ignore_corrupted_entities_;\n\n  // File stream for entities\n  FileStream entity_file_stream_;\n  // File stream for index\n  FileStream index_file_stream_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_ENTITY_REPLAYER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/entity_serializer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_ENTITY_SERIALIZER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_ENTITY_SERIALIZER_HPP_\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/serialization/endpoint.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Interface for serializing entities\nclass EntitySerializer : public Component {\n public:\n  virtual ~EntitySerializer() = default;\n\n  // Deserializes a received entity.\n  // Returns the deserialized entity.\n  virtual Expected<Entity> deserialize_entity_header_abi(\n      Endpoint* endpoint) = 0;\n\n  // Serializes entity and writes to endpoint\n  // Returns the size of the serialized entity in bytes\n  virtual gxf_result_t serialize_entity_abi(gxf_uid_t eid, Endpoint* endpoint, uint64_t* size) = 0;\n  // Reads from endpoint and deserializes entity\n  virtual gxf_result_t deserialize_entity_abi(gxf_uid_t eid, Endpoint* endpoint) = 0;\n\n  // C++ API wrappers\n  Expected<size_t> serializeEntity(Entity entity, Endpoint* endpoint);\n  Expected<void> deserializeEntity(Entity entity, Endpoint* endpoint);\n\n  Expected<Entity> deserializeEntity(gxf_context_t context, Endpoint* endpoint);\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_ENTITY_SERIALIZER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/file.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_FILE_HPP_\n#define NVIDIA_GXF_SERIALIZATION_FILE_HPP_\n\n#include <cstdio>\n#include <mutex>\n#include <string>\n\n#include \"gxf/serialization/endpoint.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/memory_buffer.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Wrapper around C file I/O API\nclass File : public Endpoint {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n\n  gxf_result_t write_abi(const void* data, size_t size, size_t* bytes_written) override;\n  gxf_result_t read_abi(void* data, size_t size, size_t* bytes_read) override;\n\n  // Opens a file with the given mode\n  // Uses parameter values if arguments are not specified\n  Expected<void> open(const char* path = nullptr, const char* mode = nullptr);\n  // Closes the opened file\n  Expected<void> close();\n  // Clears the end-of-file and error indicators\n  void clear();\n  // Returns true if the end-of-file indicator is set\n  bool eof();\n  // Returns true if the error indicator is set\n  bool error();\n  // Flushes the output buffer of the file stream\n  Expected<void> flush();\n  // Sets the file position to the given offset\n  Expected<void> seek(int offset, int position = SEEK_SET);\n  // Returns the current file position\n  Expected<size_t> tell();\n  // Returns true if the file is open\n  bool isOpen();\n  // Returns the configured file path\n  const char* path();\n  // Returns the configured file mode\n  const char* mode();\n  // Renames the file\n  Expected<void> rename(const char* path);\n  // Prepends a timestamp to the file name\n  // Uses local time zone by default and UTC if specified\n  Expected<void> addTimestamp(int64_t timestamp, bool utc = false);\n  // Write-protects the file\n  Expected<void> writeProtect();\n\n private:\n  Parameter<Handle<Allocator>> allocator_;\n  Parameter<std::string> file_path_;\n  Parameter<std::string> file_mode_;\n  Parameter<size_t> buffer_size_;\n\n  // File stream\n  std::FILE* file_;\n  // Stream buffer\n  MemoryBuffer buffer_;\n  // Mutex to guard concurrent access\n  mutable std::recursive_mutex mutex_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_FILE_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/file_stream.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_FILE_STREAM_HPP_\n#define NVIDIA_GXF_SERIALIZATION_FILE_STREAM_HPP_\n\n#include <fstream>\n#include <string>\n#include <utility>\n\n#include \"gxf/serialization/endpoint.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n#pragma pack(push, 1)\nstruct EntityIndex {\n  uint64_t log_time;     // Time when data was logged\n  uint64_t data_size;    // Size of data block\n  uint64_t data_offset;  // Location of data block\n};\n#pragma pack(pop)\n\n// Graph endpoint that exchanges data using files\n// Input file path is used for reading (empty string to disable reading)\n// Output file path is used for writing (empty string to disable writing)\n// The same file can be used for both reading and writing\nclass FileStream : public Endpoint {\n public:\n  // Extension for index files\n  static constexpr const char* kIndexFileExtension = \".gxf_index\";\n  // Extension for binary files\n  static constexpr const char* kBinaryFileExtension = \".gxf_entities\";\n\n  FileStream(std::string input_file, std::string output_file)\n    : input_file_path_{std::move(input_file)}, output_file_path_{std::move(output_file)} {}\n  FileStream() = default;\n  ~FileStream() = default;\n  FileStream(const FileStream& other) = delete;\n  FileStream(FileStream&& other) { *this = std::move(other); }\n  FileStream& operator=(const FileStream& other) = delete;\n  FileStream& operator=(FileStream&& other) {\n    input_file_path_ = std::move(other.input_file_path_);\n    output_file_path_ = std::move(other.output_file_path_);\n    input_file_ = std::move(other.input_file_);\n    output_file_ = std::move(other.output_file_);\n    return *this;\n  }\n\n  Expected<void> open();\n  Expected<void> close();\n\n  gxf_result_t write_abi(const void* data, size_t size, size_t* bytes_written) override;\n  gxf_result_t read_abi(void* data, size_t size, size_t* bytes_read) override;\n\n  // Clears error state flags for input and output streams\n  void clear();\n  // Flushes output stream buffer to file\n  // Returns an error if operation failed\n  Expected<void> flush();\n  // Moves the output stream to the desired position\n  Expected<void> setWriteOffset(size_t index);\n  // Returns the current position of the output stream\n  Expected<size_t> getWriteOffset();\n  // Moves the input stream to the desired position\n  Expected<void> setReadOffset(size_t index);\n  // Returns the current position of the input stream\n  Expected<size_t> getReadOffset();\n\n private:\n  // Path to input file\n  std::string input_file_path_;\n  // Path to output file\n  std::string output_file_path_;\n  // Input file stream\n  std::ifstream input_file_;\n  // Output file stream\n  std::ofstream output_file_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_FILE_STREAM_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/serialization_buffer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_SERIALIZATION_BUFFER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_SERIALIZATION_BUFFER_HPP_\n\n#include <mutex>\n\n#include \"gxf/serialization/endpoint.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/memory_buffer.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Buffer to hold serialized data\nclass SerializationBuffer : public Endpoint {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override { return ToResultCode(buffer_.freeBuffer()); }\n\n  gxf_result_t write_abi(const void* data, size_t size, size_t* bytes_written) override;\n  gxf_result_t read_abi(void* data, size_t size, size_t* bytes_read) override;\n\n  // Resizes the buffer\n  Expected<void> resize(size_t size, MemoryStorageType storage_type);\n\n  // Type of the callback function to release memory passed to the SerializationBuffer\n  // using the wrapMemory method\n  using release_function_t = MemoryBuffer::release_function_t;\n\n  // Wrap existing memory inside the SerializationBuffer.\n  // A callback function of type release_function_t may be passed that will be called when the\n  // SerializationBuffer wants to release the memory.\n  Expected<void> wrapMemory(void* pointer, uint64_t size,\n                            MemoryStorageType storage_type,\n                            release_function_t release_func);\n\n  // The type of memory where the data is stored\n  MemoryStorageType storage_type() const { return buffer_.storage_type(); }\n  // Returns a read-only pointer to buffer data\n  const byte* data() const { return buffer_.pointer(); }\n  // Returns the capacity of the buffer\n  size_t capacity() const { return buffer_.size(); }\n  // Returns the number of bytes written to the buffer\n  size_t size() const;\n  // Resets buffer for sequential access\n  void reset();\n\n private:\n  Parameter<Handle<Allocator>> allocator_;\n  Parameter<size_t> buffer_size_;\n  Parameter<int32_t> storage_type_;\n\n  // Data buffer\n  MemoryBuffer buffer_;\n  // Offset for sequential writes\n  size_t write_offset_;\n  // Offset for sequential reads\n  size_t read_offset_;\n  // Mutex to guard concurrent access\n  mutable std::mutex mutex_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_SERIALIZATION_BUFFER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/std_component_serializer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_STD_COMPONENT_SERIALIZER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_STD_COMPONENT_SERIALIZER_HPP_\n\n#include<string>\n\n#include \"common/endian.hpp\"\n#include \"gxf/cuda/cuda_common.hpp\"\n#include \"gxf/serialization/component_serializer.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Serializer that supports serializaing Timestamps, Tensors, and integer components\n// Valid for sharing data between devices with the same endianness\nclass StdComponentSerializer : public ComponentSerializer {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override { return GXF_SUCCESS; }\n\n private:\n  // Configures all serializer functions\n  Expected<void> configureSerializers();\n  // Configures all deserializer functions\n  Expected<void> configureDeserializers();\n  // Serializes a nvidia::gxf::Timestamp\n  Expected<size_t> serializeTimestamp(Timestamp timestamp, Endpoint* endpoint);\n  // Deserializes a nvidia::gxf::Timestamp\n  Expected<Timestamp> deserializeTimestamp(Endpoint* endpoint);\n  // Serializes a nvidia::gxf::Tensor\n  Expected<size_t> serializeTensor(const Tensor& tensor, Endpoint* endpoint);\n  // Deserializes a nvidia::gxf::Tensor\n  Expected<Tensor> deserializeTensor(Endpoint* endpoint);\n  // Serializes an integer\n  template <typename T>\n  Expected<size_t> serializeInteger(T value, Endpoint* endpoint) {\n    if (!endpoint) {\n      return Unexpected{GXF_ARGUMENT_NULL};\n    }\n    T encoded = EncodeLittleEndian(value);\n    return endpoint->writeTrivialType(&encoded);\n  }\n  // Deserializes an integer\n  template <typename T>\n  Expected<T> deserializeInteger(Endpoint* endpoint) {\n    if (!endpoint) {\n      return Unexpected{GXF_ARGUMENT_NULL};\n    }\n    T encoded;\n    return endpoint->readTrivialType(&encoded)\n        .and_then([&]() { return DecodeLittleEndian(encoded); });\n  }\n\n  Parameter<Handle<Allocator>> allocator_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_STD_COMPONENT_SERIALIZER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/std_entity_id_serializer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_STD_ENTITY_ID_SERIALIZER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_STD_ENTITY_ID_SERIALIZER_HPP_\n\n#include <unordered_map>\n\n#include \"common/fixed_vector.hpp\"\n#include \"gxf/serialization/entity_serializer.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Serializes and deserializes entity id. No component serializer used.\n\nclass StdEntityIdSerializer : public EntitySerializer {\n public:\n#pragma pack(push, 1)\n  struct EntityHeader {\n    uint64_t entity_id;\n    uint64_t sequence_number;\n  };\n#pragma pack(pop)\n\n  gxf_result_t initialize() override { return GXF_SUCCESS; }\n  gxf_result_t deinitialize() override { return GXF_SUCCESS; }\n\n  Expected<Entity> deserialize_entity_header_abi(Endpoint* endpoint) override;\n\n  gxf_result_t serialize_entity_abi(gxf_uid_t eid, Endpoint* endpoint,\n                                    uint64_t* size) override;\n  gxf_result_t deserialize_entity_abi(gxf_uid_t eid,\n                                      Endpoint* endpoint) override;\n\n private:\n  // Sequence number for outgoing messages\n  uint64_t outgoing_sequence_number_{0};\n  // Sequence number for incoming messages\n  uint64_t incoming_sequence_number_{0};\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_STD_ENTITY_ID_SERIALIZER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/std_entity_serializer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_STD_ENTITY_SERIALIZER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_STD_ENTITY_SERIALIZER_HPP_\n\n#include <unordered_map>\n\n#include \"common/fixed_vector.hpp\"\n#include \"gxf/serialization/component_serializer.hpp\"\n#include \"gxf/serialization/entity_serializer.hpp\"\n#include \"gxf/serialization/tid_hash.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Serializes and deserializes entities with the provided component serializers\n// Little-endian is used over big-endian for better performance on x86 and arm platforms\n// Entities are serialized in the following format:\n//\n//   | Entity Header || Component Header | Component Name | Component | ... | ... | ... |\n//\n// Components will be serialized in the order they are added to the entity\n// Components without serializers will be skipped\n// Each component will be preceded by a component header and the name of the component\n// The component itself will be serialized with a component serializer\n// An entity header will be added at the beginning\nclass StdEntitySerializer : EntitySerializer {\n public:\n  #pragma pack(push, 1)\n  // Header preceding entities\n  struct EntityHeader {\n    uint64_t serialized_size;  // Size of the serialized entity in bytes\n    uint32_t checksum;         // Checksum to verify the integrity of the message\n    uint64_t sequence_number;  // Sequence number of the message\n    uint32_t flags;            // Flags to specify delivery options\n    uint64_t component_count;  // Number of components in the entity\n    uint64_t reserved;         // Bytes reserved for future use\n  };\n  #pragma pack(pop)\n\n  #pragma pack(push, 1)\n  // Header preceding components\n  struct ComponentHeader {\n    uint64_t serialized_size;  // Size of the serialized component in bytes\n    gxf_tid_t tid;             // Type ID of the component\n    uint64_t name_size;        // Size of the component name in bytes\n  };\n  #pragma pack(pop)\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override { return GXF_SUCCESS; }\n\n  gxf_result_t serialize_entity_abi(gxf_uid_t eid, Endpoint* endpoint, uint64_t* size) override;\n  gxf_result_t deserialize_entity_abi(gxf_uid_t eid, Endpoint* endpoint) override;\n  Expected<Entity> deserialize_entity_header_abi(Endpoint* endpoint) override;\n\n private:\n  // Structure used to organize serializable components\n  struct ComponentEntry;\n\n  // Populates a list of component entries using a list of component handles\n  Expected<FixedVector<ComponentEntry, kMaxComponents>> createComponentEntries(\n      const FixedVector<UntypedHandle, kMaxComponents>& components);\n  // Serializes a list of components and writes them to an endpoint\n  // Returns the total number of bytes serialized\n  Expected<size_t> serializeComponents(const FixedVector<ComponentEntry, kMaxComponents>& entries,\n                                       Endpoint* endpoint);\n  // Reads from an endpoint and deserializes a list of components\n  Expected<void> deserializeComponents(size_t component_count, Entity entity, Endpoint* endpoint);\n  // Searches for a component serializer that supports the given type ID\n  // Uses the first valid serializer found and caches it for subsequent lookups\n  // Returns an Unexpected if no valid serializer is found\n  Expected<Handle<ComponentSerializer>> findComponentSerializer(gxf_tid_t tid);\n\n  Parameter<FixedVector<Handle<ComponentSerializer>, kMaxComponents>> component_serializers_;\n  Parameter<bool> verbose_warning_;\n\n  // Table that caches type ID with a valid component serializer\n  std::unordered_map<gxf_tid_t, Handle<ComponentSerializer>, TidHash> serializer_cache_;\n  // Sequence number for outgoing messages\n  uint64_t outgoing_sequence_number_ = 0;\n  // Sequence number for incoming messages\n  uint64_t incoming_sequence_number_ = 0;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_STD_ENTITY_SERIALIZER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/tests/serialization_tester.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_TESTS_SERIALIZATION_TESTER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_TESTS_SERIALIZATION_TESTER_HPP_\n\n#include <memory>\n\n#include \"gxf/serialization/entity_serializer.hpp\"\n#include \"gxf/serialization/serialization_buffer.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\nnamespace test {\n\n// Codelet that serializes incoming messages and stores them in a buffer\n// Messages are immediately deserialized from the buffer and published\nclass SerializationTester : public Codelet {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override { return GXF_SUCCESS; }\n  gxf_result_t deinitialize() override { return GXF_SUCCESS; }\n  gxf_result_t tick() override;\n\n private:\n  Parameter<Handle<Receiver>> input_;\n  Parameter<Handle<Transmitter>> output_;\n  Parameter<Handle<EntitySerializer>> entity_serializer_;\n  Parameter<Handle<SerializationBuffer>> serialization_buffer_;\n};\n\n}  // namespace test\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_TESTS_SERIALIZATION_TESTER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/serialization/tid_hash.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_TID_HASH_HPP_\n#define NVIDIA_GXF_SERIALIZATION_TID_HASH_HPP_\n\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Hash function for gxf_tid_t that XORs upper 64 bits with lower 64 bits\nstruct TidHash {\n  size_t operator()(const gxf_tid_t& tid) const { return tid.hash1 ^ tid.hash2; }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_TID_HASH_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/allocator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_ALLOCATOR_HPP\n#define NVIDIA_GXF_STD_ALLOCATOR_HPP\n\n#include <string>\n\n#include \"common/byte.hpp\"\n#include \"gxf/core/component.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nenum struct MemoryStorageType {\n  kHost = 0,        // Host Pinned Memory\n  kDevice = 1,      // Cuda Device Memory\n  kSystem = 2,      // Heap Memory\n  kCudaManaged = 3  // Cuda Managed Memory\n};\n\n// Custom parameter parser for MemoryStorageType\ntemplate <>\nstruct ParameterParser<MemoryStorageType> {\n  static Expected<MemoryStorageType> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                       const char* key, const YAML::Node& node,\n                                       const std::string& prefix) {\n    const std::string value = node.as<std::string>();\n    if (strcmp(value.c_str(), \"Host\") == 0) {\n      return MemoryStorageType::kHost;\n    }\n    if (strcmp(value.c_str(), \"Device\") == 0) {\n      return MemoryStorageType::kDevice;\n    }\n    if (strcmp(value.c_str(), \"System\") == 0) {\n      return MemoryStorageType::kSystem;\n    }\n    if (strcmp(value.c_str(), \"Managed\") == 0) {\n      return MemoryStorageType::kCudaManaged;\n    }\n    return Unexpected{GXF_ARGUMENT_OUT_OF_RANGE};\n  }\n};\n\n// Custom parameter parser for MemoryStorageType\ntemplate<>\nstruct ParameterWrapper<MemoryStorageType> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context, const MemoryStorageType& value) {\n    YAML::Node node(YAML::NodeType::Scalar);\n    switch (value) {\n      case MemoryStorageType::kHost: {\n        node = std::string(\"Host\");\n        break;\n      }\n      case MemoryStorageType::kDevice: {\n        node = std::string(\"Device\");\n        break;\n      }\n      case MemoryStorageType::kSystem: {\n        node = std::string(\"System\");\n        break;\n      }\n      case MemoryStorageType::kCudaManaged: {\n        node = std::string(\"Managed\");\n        break;\n      }\n      default:\n        return Unexpected{GXF_PARAMETER_OUT_OF_RANGE};\n    }\n    return node;\n  }\n};\n\n// Lifecycle stages of an allocator\nenum struct AllocatorStage : uint8_t {\n  kUninitialized = 0,\n  kInitializationInProgress = 1,\n  kInitialized = 2,\n  kDeinitializationInProgress = 3,\n};\n\n// Provides allocation and deallocation of memory.\nstruct Allocator : public Component {\n  virtual ~Allocator() = default;\n\n  virtual gxf_result_t is_available_abi(uint64_t size) = 0;\n  virtual gxf_result_t allocate_abi(uint64_t size, int32_t type, void** pointer) = 0;\n  virtual gxf_result_t free_abi(void* pointer) = 0;\n  virtual uint64_t block_size_abi() const;\n\n  // Returns true if the allocator can provide a memory block with the given size.\n  bool is_available(uint64_t size);\n\n  // Allocates a memory block with the given size.\n  Expected<byte*> allocate(uint64_t size, MemoryStorageType type);\n\n  // Frees the given memory block.\n  Expected<void> free(byte* pointer);\n\n  // Get the block size of this allocator, defaults to 1 for byte-based allocators\n  uint64_t block_size() const;\n\n  // Get the string value of allocator status\n  const char* allocator_stage_str(AllocatorStage stage) const;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/async_buffer_receiver.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_ASYNC_BUFFER_RECEIVER_HPP_\n#define NVIDIA_GXF_STD_ASYNC_BUFFER_RECEIVER_HPP_\n\n#include <memory>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/std/receiver.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief A receiver which uses a Simpson's four-slot buffer to enable lockless/lock-free and\n * asynchronous communication. This receiver is designed to be used in a single producer and single\n * consumer scenario.\n *\n */\nclass AsyncBufferReceiver : public Receiver {\n public:\n  gxf_result_t initialize() override;\n\n  gxf_result_t deinitialize() override;\n\n  gxf_result_t pop_abi(gxf_uid_t* uid) override;\n\n  gxf_result_t push_abi(gxf_uid_t other) override;\n\n  gxf_result_t peek_abi(gxf_uid_t* uid, int32_t index) override;\n\n  gxf_result_t peek_back_abi(gxf_uid_t* uid, int32_t index) override;\n\n  size_t capacity_abi() override;\n\n  size_t size_abi() override;\n\n  gxf_result_t receive_abi(gxf_uid_t* uid) override;\n\n  size_t back_size_abi() override;\n\n  gxf_result_t sync_abi() override;\n\n  gxf_result_t sync_io_abi() override;\n\n private:\n  // Resets the four-slot control variables.\n  void reset_buffer();\n  Entity read_freshest();\n\n  // The following variables are used to implement a Simpson's four-slot buffer.\n  Entity entity_data_[2][2];\n  // Four-slot control variables\n  int32_t freshest_;\n  int32_t reading_;\n  // control bits\n  int32_t slots_[2];\n\n  // To indicate whether the buffer has been filled at least once. After first-time, receiving end\n  // will always be able to retrieve a value from the buffer.\n  bool is_filled_first_time_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif /* NVIDIA_GXF_STD_ASYNC_BUFFER_RECEIVER_HPP_ */\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/async_buffer_transmitter.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_ASYNC_BUFFER_TRANSMITTER_HPP_\n#define NVIDIA_GXF_STD_ASYNC_BUFFER_TRANSMITTER_HPP_\n\n#include <memory>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief A transmitter which uses a Simpson's four-slot buffer to enable lockless and asynchronous\n * communication. This transmitter is designed to be used in a single producer and single consumer\n * scenario.\n *\n */\nclass AsyncBufferTransmitter : public Transmitter {\n public:\n  gxf_result_t initialize() override;\n\n  gxf_result_t pop_abi(gxf_uid_t* uid) override;\n\n  gxf_result_t pop_io_abi(gxf_uid_t* uid) override;\n\n  gxf_result_t push_abi(gxf_uid_t other) override;\n\n  gxf_result_t peek_abi(gxf_uid_t* uid, int32_t index) override;\n\n  size_t capacity_abi() override;\n\n  size_t size_abi() override;\n\n  gxf_result_t publish_abi(gxf_uid_t uid) override;\n\n  size_t back_size_abi() override;\n\n  gxf_result_t sync_abi() override;\n\n  gxf_result_t sync_io_abi() override;\n\n private:\n  Entity entity_;\n  int size_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif /* NVIDIA_GXF_STD_ASYNC_BUFFER_TRANSMITTER_HPP_ */\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/block_memory_pool.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_BLOCK_MEMORY_POOL_HPP\n#define NVIDIA_GXF_STD_BLOCK_MEMORY_POOL_HPP\n\n#include <atomic>\n#include <cstdint>\n#include <memory>\n#include <mutex>\n\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/resources.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nclass FixedPoolUint64;\n\n// A memory pools which provides a maximum number of equally sized blocks of\n// memory.\nclass BlockMemoryPool : public Allocator {\n public:\n  BlockMemoryPool();\n  ~BlockMemoryPool();\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t is_available_abi(uint64_t size) override;\n  gxf_result_t allocate_abi(uint64_t size, int32_t type, void** pointer) override;\n  gxf_result_t free_abi(void* pointer) override;\n  gxf_result_t deinitialize() override;\n  uint64_t block_size_abi() const override;\n\n  // Returns the storage type of the memory blocks\n  MemoryStorageType storage_type() const {\n    return static_cast<MemoryStorageType>(storage_type_.get());\n  }\n\n  // Returns the total number of blocks\n  uint64_t num_blocks() const {\n    return num_blocks_.get();\n  }\n\n private:\n  Parameter<int32_t> storage_type_;\n  Parameter<uint64_t> block_size_;\n  Parameter<uint64_t> num_blocks_;\n  Resource<Handle<GPUDevice>> gpu_device_;\n\n  void* pointer_;\n  std::unique_ptr<FixedPoolUint64> stack_;\n  std::mutex stack_mutex_;\n  // Holds lifecycle stage of the allocator\n  std::atomic<AllocatorStage> stage_{AllocatorStage::kUninitialized};\n  int32_t dev_id_ = -1;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/clock.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_CLOCK_HPP_\n#define NVIDIA_GXF_STD_CLOCK_HPP_\n\n#include <chrono>\n\n#include \"gxf/core/component.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/// @brief Keeps track of time\n//\n// This clock is based on a steady clock however time can be scaled to run slower or faster.\nclass Clock : public Component {\n public:\n  virtual ~Clock() = default;\n\n  /// @brief The current time of the clock. Time is measured in seconds.\n  virtual double time() const = 0;\n\n  /// @brief The current timestamp of the clock. Timestamps are measured in nanoseconds.\n  virtual int64_t timestamp() const = 0;\n\n  /// @brief Waits until the given duration has elapsed on the clock\n  virtual Expected<void> sleepFor(int64_t duration_ns) = 0;\n\n  /// @brief Waits until the given target time\n  virtual Expected<void> sleepUntil(int64_t target_ns) = 0;\n};\n\n/// @brief A clock which runs based on a realtime clock\nclass RealtimeClock : public Clock {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n  double time() const override;\n  int64_t timestamp() const override;\n  Expected<void> sleepFor(int64_t duration_ns) override;\n  Expected<void> sleepUntil(int64_t target_time_ns) override;\n\n  // Changes time scaling used by the clock.\n  Expected<void> setTimeScale(double time_scale);\n\n private:\n  Parameter<double> initial_time_offset_;\n  Parameter<double> initial_time_scale_;\n  Parameter<bool> use_time_since_epoch_;\n\n  std::chrono::time_point<std::chrono::steady_clock> reference_;\n  double time_offset_;\n  double time_scale_;\n};\n\n/// @brief A clock where time flow is controlled manually\nclass ManualClock : public Clock {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n  double time() const override;\n  int64_t timestamp() const override;\n  Expected<void> sleepFor(int64_t duration_ns) override;\n  Expected<void> sleepUntil(int64_t target_time_ns) override;\n\n private:\n  Parameter<int64_t> initial_timestamp_;\n\n  int64_t current_time_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/codelet.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cstdint>\n\n#include \"gxf/core/component.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Codelets are special components which allow the execution of custom code. The user can\n// create her own codelets by deriving from this class and overriding the functions initialize,\n// start, tick, stop, and deinitialize.\nclass Codelet : public Component {\n public:\n  virtual ~Codelet() = default;\n\n  // This function is called during the start phase of the codelet. It allows derived classes to\n  // execute custom code during the start phase. This is a good place to obtain resources which\n  // are necessary for ticking the codelet. This function is guaranteed to be called before the\n  // first call to tick.\n  virtual gxf_result_t start() { return GXF_SUCCESS; }\n\n  // This function is called whenever the codelet is expected to do work, e.g. when an event was\n  // received or periodically. The tick method can be specified with various other member functions.\n  // This function is the main work horse of the codelet.\n  virtual gxf_result_t tick() = 0;\n\n  // This function is called during the stop phase of the codelet. It allows derived classes to\n  // execute custom code during the stop phase. This is a good place to clean up any resources which\n  // where obtained during 'start'. After the codelet is stopped it should be in the same state as\n  // it was before 'start' was called. Be careful to not leave any unintended left overs as 'start'\n  // might be called again afterwards. It is guaranteed that stop is called after the last\n  // call to tick. When start was called stop will be called, too.\n  virtual gxf_result_t stop() { return GXF_SUCCESS; }\n\n  // Timestamp (in nanoseconds) of the beginning of the start, tick or stop function. The execution\n  // timestamp does not change during the start, tick or stop function.\n  int64_t getExecutionTimestamp() const { return execution_timestamp_; }\n\n  // Similar to getExecutionTimestamp but returns time as a floating point number and using seconds\n  // as unit. Equivalent to 'ToSeconds(getExecutionCount())'.\n  double getExecutionTime() const { return execution_time_; }\n\n  // The delta between the current execution time and the execution time of the previous execution.\n  // During the start function this will return 0.\n  double getDeltaTime() const { return delta_time_; }\n\n  // Returns the number of times a codelet is executed. This will return 0 during start and 1 during\n  // the first tick.\n  int64_t getExecutionCount() const { return execution_count_; }\n\n  // Returns true if this is the first time tick is called after start.\n  bool isFirstTick() const { return getExecutionCount() == 1; }\n\n private:\n  // Class is friend to allow EntityExecutor to call private member functions\n  friend class EntityExecutor;\n\n  // Called by EntityExecutor before each 'start'\n  void beforeStart(int64_t timestamp);\n\n  // Called by EntityExecutor before each 'tick'\n  void beforeTick(int64_t timestamp);\n\n  // Called by EntityExecutor before each 'stop'\n  void beforeStop();\n\n  // The number of times the codelet tick function was called.\n  int64_t execution_count_{0};\n  // The timestamp of the previous execution. Equal to 'execution_timestamp' during 'start'.\n  int64_t previous_execution_timestamp_{0};\n  // The timestamp of the current execution in nanoseconds.\n  int64_t execution_timestamp_{0};\n  // Same as execution_timestamp_ but in seconds and as a floating point.\n  double execution_time_{0.0};\n  // The difference between the current and the previous execution time in seconds.\n  double delta_time_{0.0};\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/complex.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_COMPLEX_HPP_\n#define NVIDIA_GXF_STD_COMPLEX_HPP_\n\n#include <math.h>\n#include <cuda_runtime.h>   //NOLINT\n#include <cuda/std/complex> //NOLINT\n\nnamespace nvidia {\nnamespace gxf {\n\nusing complex64 = cuda::std::complex<float>;\nusing complex128 = cuda::std::complex<double>;\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/component_allocator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_COMPONENT_ALLOCATOR_HPP\n#define NVIDIA_GXF_STD_COMPONENT_ALLOCATOR_HPP\n\n#include \"gxf/core/expected.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Base class for allocating components.\nclass ComponentAllocator {\n public:\n  virtual ~ComponentAllocator() = default;\n\n  ComponentAllocator(const ComponentAllocator&) = delete;\n  ComponentAllocator(ComponentAllocator&&) = delete;\n  ComponentAllocator& operator=(const ComponentAllocator&) = delete;\n  ComponentAllocator& operator=(ComponentAllocator&&) = delete;\n\n  // Allocates a new component of the specific component type this allocator is handling.\n  virtual gxf_result_t allocate_abi(void** out_pointer) = 0;\n\n  // Deallocates a component which was previously allocated by this allocator.\n  virtual gxf_result_t deallocate_abi(void* pointer) = 0;\n\n  Expected<void*> allocate();\n\n  Expected<void> deallocate(void* pointer);\n\n protected:\n  ComponentAllocator() = default;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/component_factory.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_COMPONENT_FACTORY_HPP\n#define NVIDIA_GXF_STD_COMPONENT_FACTORY_HPP\n\n#include \"gxf/core/expected.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Base class for extension factories. An extension factory is used to create instances\n// of components.\nclass ComponentFactory {\n public:\n  virtual ~ComponentFactory() = default;\n\n  ComponentFactory(const ComponentFactory&) = delete;\n  ComponentFactory(ComponentFactory&&) = delete;\n  ComponentFactory& operator=(const ComponentFactory&) = delete;\n  ComponentFactory& operator=(ComponentFactory&&) = delete;\n\n  // Allocates a component of the given type\n  virtual gxf_result_t allocate_abi(gxf_tid_t tid, void** out_pointer) = 0;\n\n  // Frees a component of the given type\n  virtual gxf_result_t deallocate_abi(gxf_tid_t tid, void* pointer) = 0;\n\n  Expected<void*> allocate(gxf_tid_t tid);\n\n  Expected<void> deallocate(gxf_tid_t tid, void* pointer);\n\n protected:\n  ComponentFactory() = default;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/controller.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_CONTROLLER_HPP_\n#define NVIDIA_GXF_STD_CONTROLLER_HPP_\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/std/scheduling_condition.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/// @brief The status returned by controller to executor deciding termination\n/// policy\n/// - GXF_EXECUTE_SUCCESS: executor resumes execution\n/// - GXF_EXECUTE_FAILURE_REPEAT: codelet fails and executor repeatedly execute\n/// this entity\n/// - GXF_EXECUTE_FAILURE_DEACTIVATE: codelet fails and executor deactivate this\n/// entity\n/// - GXF_EXECUTE_FAILURE: codelet fails and executor deactivates all the\n/// entities and stops the entire graph\ntypedef enum {\n  GXF_EXECUTE_SUCCESS = 0,\n  GXF_EXECUTE_FAILURE_REPEAT = 1,\n  GXF_EXECUTE_FAILURE_DEACTIVATE = 2,\n  GXF_EXECUTE_FAILURE = 3,\n} gxf_execution_status_t;\n\n// Type to represent codelet::tick() result for behavior tree parent and\n// execution status for executor to use termination policy\nstruct gxf_controller_status_t {\n  entity_state_t behavior_status;\n  gxf_execution_status_t exec_status;\n  gxf_controller_status_t(entity_state_t b_status,\n                          gxf_execution_status_t e_status)\n      : behavior_status(b_status), exec_status(e_status) {}\n  gxf_controller_status_t() {\n    behavior_status = GXF_BEHAVIOR_SUCCESS;\n    exec_status = GXF_EXECUTE_SUCCESS;\n  }\n};\n\n// Interface for controlling entity's termination policy and entity's execution\n// status\nclass Controller : public Component {\n public:\n  virtual ~Controller() = default;\n\n  // Return a struct encapsulating the determined behavior status and execution\n  // status by the controller given the result of codelet's tick()\n  virtual gxf_controller_status_t control(gxf_uid_t eid,\n                                          Expected<void> code) = 0;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_CONTROLLER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/cpu_thread.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_CPU_THREAD_HPP\n#define NVIDIA_GXF_STD_CPU_THREAD_HPP\n\n#include <string>\n#include <vector>\n\n#include \"gxf/core/component.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Real-time scheduling policies supported by POSIX and Linux kernel\nenum class SchedulingPolicy : int32_t {\n  kFirstInFirstOut = 1,  // SCHED_FIFO supported by POSIX and Linux kernel\n  kRoundRobin = 2,  // SCHED_RR supported by POSIX and Linux kernel\n  kDeadline = 6  // SCHED_DEADLINE supported by Linux kernel\n};\n\n// Custom parameter parser for SchedulingPolicy\ntemplate <>\nstruct ParameterParser<SchedulingPolicy> {\n  static Expected<SchedulingPolicy> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                         const char* key, const YAML::Node& node,\n                                         const std::string& prefix) {\n    const std::string value = node.as<std::string>();\n    if (strcmp(value.c_str(), \"SCHED_FIFO\") == 0) {\n      return SchedulingPolicy::kFirstInFirstOut;\n    }\n    if (strcmp(value.c_str(), \"SCHED_RR\") == 0) {\n      return SchedulingPolicy::kRoundRobin;\n    }\n    if (strcmp(value.c_str(), \"SCHED_DEADLINE\") == 0) {\n      return SchedulingPolicy::kDeadline;\n    }\n    GXF_LOG_ERROR(\"Invalid scheduling policy: %s\", value.c_str());\n    return Unexpected{GXF_ARGUMENT_OUT_OF_RANGE};\n  }\n};\n\n// Custom parameter wrapper for SchedulingPolicy\ntemplate<>\nstruct ParameterWrapper<SchedulingPolicy> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context, const SchedulingPolicy& value) {\n    YAML::Node node(YAML::NodeType::Scalar);\n    switch (value) {\n      case SchedulingPolicy::kFirstInFirstOut: {\n        node = std::string(\"SCHED_FIFO\");\n        break;\n      }\n      case SchedulingPolicy::kRoundRobin: {\n        node = std::string(\"SCHED_RR\");\n        break;\n      }\n      case SchedulingPolicy::kDeadline: {\n        node = std::string(\"SCHED_DEADLINE\");\n        break;\n      }\n      default:\n        GXF_LOG_ERROR(\"Invalid scheduling policy: %d\", static_cast<int32_t>(value));\n        return Unexpected{GXF_PARAMETER_OUT_OF_RANGE};\n    }\n    return node;\n  }\n};\n\nclass CPUThread : public Component {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n\n  bool pinned() const {\n    return pin_entity_;\n  }\n\n  std::vector<uint32_t> pinCores() const {\n    const auto maybe_pin_cores = pin_cores_.try_get();\n    if (maybe_pin_cores) {\n      return maybe_pin_cores.value();\n    }\n    return {};\n  }\n\n  Expected<SchedulingPolicy> schedPolicy() const {\n    return sched_policy_.try_get();\n  }\n\n  Expected<uint32_t> schedPriority() const {\n    return sched_priority_.try_get();\n  }\n\n  Expected<uint64_t> schedRuntime() const {\n    return sched_runtime_.try_get();\n  }\n\n  Expected<uint64_t> schedDeadline() const {\n    return sched_deadline_.try_get();\n  }\n\n  Expected<uint64_t> schedPeriod() const {\n    return sched_period_.try_get();\n  }\n\n  // Helper method to check if real-time scheduling is used\n  bool isRealtime() const {\n    const auto maybe_sched_policy = sched_policy_.try_get();\n    if (maybe_sched_policy) {\n      auto sched_policy_value = maybe_sched_policy.value();\n      return sched_policy_value == SchedulingPolicy::kFirstInFirstOut ||\n             sched_policy_value == SchedulingPolicy::kRoundRobin ||\n             sched_policy_value == SchedulingPolicy::kDeadline;\n    }\n    return false;\n  }\n\n private:\n  // Keep track of whether or not the component should be pinned to a worker thread\n  Parameter<bool> pin_entity_;\n  // CPU core IDs to pin the worker thread to (empty means no core pinning)\n  Parameter<std::vector<uint32_t>> pin_cores_;\n\n  // Real-time scheduling parameters (optional)\n  // Real-time scheduling policy (kFirstInFirstOut, kRoundRobin, kDeadline)\n  Parameter<SchedulingPolicy> sched_policy_;\n  // Thread priority (only for kFirstInFirstOut and kRoundRobin)\n  Parameter<uint32_t> sched_priority_;\n  // Expected worst case execution time in nanoseconds (only for kDeadline)\n  Parameter<uint64_t> sched_runtime_;\n  // Relative deadline in nanoseconds (only for kDeadline)\n  Parameter<uint64_t> sched_deadline_;\n  // Period in nanoseconds (only for kDeadline)\n  Parameter<uint64_t> sched_period_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_CPU_THREAD_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/cuda_green_context.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_CUDA_GREEN_CONTEXT_HPP_\n#define NVIDIA_GXF_STD_CUDA_GREEN_CONTEXT_HPP_\n\n#include \"cuda.h\"\n#include <cuda_runtime.h>\n\n#include <mutex>\n#include <string>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/std/cuda_green_context_pool.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief Holds and provides access to CUgreenCtx. CudaGreenContext is allocated and\n * recycled by CudaGreenContextPool\n */\nclass CudaGreenContext : public Component {\n public:\n  CudaGreenContext() = default;\n  ~CudaGreenContext();\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n\n  // Retrieves CUgreenCtx\n  Expected<CUgreenCtx> greenContext() const;\n\n  // Retrieves CUcontext that associated with this green context\n  Expected<CUcontext> cudaContext();\n\n  uint32_t index() const { return pool_index_; }\n\n  CudaGreenContextPool* cudaGreenContextPool() const {\n    return cuda_green_context_pool_.try_get().value();\n  }\n\n private:\n  Parameter<int32_t> index_;\n  Parameter<Handle<CudaGreenContextPool>> cuda_green_context_pool_;\n  Parameter<std::string> nvtx_identifier_;\n\n  CUgreenCtx green_context_ = nullptr;\n  CUcontext cuda_context_ = nullptr;\n  uint32_t pool_index_ = 0;\n  mutable std::shared_timed_mutex mutex_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_CUDA_GREEN_CONTEXT_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/cuda_green_context_pool.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_CUDA_GREEN_CONTEXT_POOL_HPP_\n#define NVIDIA_GXF_STD_CUDA_GREEN_CONTEXT_POOL_HPP_\n\n#include \"cuda.h\"\n#include <cuda_runtime.h>\n\n#include <algorithm>\n#include <atomic>\n#include <mutex>\n#include <string>\n#include <vector>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/resources.hpp\"\n\n#define CHECK_CUDA_ERROR_RESULT(cu_result, fmt, ...)                      \\\n  do {                                                                    \\\n    cudaError_t err = (cu_result);                                        \\\n    if (err != cudaSuccess) {                                             \\\n      GXF_LOG_ERROR(fmt \", cuda_error: %s, error_str: %s\", ##__VA_ARGS__, \\\n                    cudaGetErrorName(err), cudaGetErrorString(err));      \\\n      return GXF_FAILURE;                                                 \\\n    }                                                                     \\\n  } while (0)\n\n#define CHECK_CUDA_DRV_ERROR(cu_result, driver_fn_table, fmt, ...)          \\\n  do {                                                                      \\\n    if (CUDA_SUCCESS != cu_result) {                                        \\\n        const char* errorName = nullptr;                                    \\\n        const char* errorString = nullptr;                                  \\\n        driver_fn_table.cuGetErrorName(cu_result, &errorName);             \\\n        driver_fn_table.cuGetErrorString(cu_result, &errorString);         \\\n        GXF_LOG_ERROR(fmt \", cuda driver API failed with error: %s:%s \",    \\\n                      errorName, errorString);                              \\\n      return GXF_FAILURE;                                                   \\\n    }                                                                       \\\n  } while (0)\n\n#define GET_CUDA_DRV_API_PTR(func, version, name)                           \\\n  do {                                                                      \\\n    cudaDriverEntryPointQueryResult driver_status;                          \\\n    auto result = cudaGetDriverEntryPointByVersion(name,                    \\\n        reinterpret_cast<void**>(&func),                                    \\\n        version,                                                            \\\n        cudaEnableDefault,                                                  \\\n        &driver_status);                                                    \\\n    if (func == nullptr) {                                                  \\\n      GXF_LOG_ERROR(\"Failed to load %s from CUDA runtime library, \"         \\\n        \"error[%d], driver_status: %d\", name, result, driver_status);       \\\n      return GXF_FAILURE;                                                   \\\n    }                                                                       \\\n  } while (0)\n\nnamespace nvidia {\nnamespace gxf {\n\nstruct CudaDriverFunctionTable {\n  CUresult (*cuDeviceGet)(CUdevice*, int) = nullptr;\n  CUresult (*cuGreenCtxGetDevResource)(CUgreenCtx, CUdevResource*, CUdevResourceType) = nullptr;\n  CUresult (*cuGreenCtxCreate)(CUgreenCtx*, CUdevResourceDesc, CUdevice, unsigned int) = nullptr;\n  CUresult (*cuDevSmResourceSplitByCount)(\n      CUdevResource*, unsigned int*, const CUdevResource*, CUdevResource*, unsigned int,\n      unsigned int) = nullptr;\n  CUresult (*cuCtxFromGreenCtx)(CUcontext*, CUgreenCtx) = nullptr;\n  CUresult (*cuDevResourceGenerateDesc)(\n      CUdevResourceDesc*, CUdevResource*, unsigned int) = nullptr;\n  CUresult (*cuDeviceGetDevResource)(\n      CUdevice, CUdevResource*, CUdevResourceType) = nullptr;\n  CUresult (*cuGetErrorName)(CUresult, const char**) = nullptr;\n  CUresult (*cuGetErrorString)(CUresult, const char**) = nullptr;\n};\n\n/**\n * @brief A pool containing a specified number of CUDA Green Contexts on a single device.\n */\nclass CudaGreenContextPool : public Component {\n public:\n  CudaGreenContextPool() = default;\n  ~CudaGreenContextPool();\n\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n  gxf_result_t registerInterface(Registrar* registrar) override;\n\n  Expected<CUgreenCtx> getGreenContext(uint32_t index);\n  Expected<CUcontext> getCudaContext(uint32_t index);\n  Expected<uint32_t> getPartitionSms(uint32_t index);\n  Expected<uint32_t> getDefaultContextIndex();\n  Expected<CUgreenCtx> getDefaultContext();\n  Expected<uint32_t> getDeviceTotalSms(uint32_t index);\n\n private:\n  gxf_result_t reserveGreenContexts(int32_t compute_capability_major);\n  gxf_result_t loadCudaDriver();\n\n  Resource<Handle<GPUDevice>> gpu_device_;\n  Parameter<uint32_t> green_context_flags_;\n  Parameter<uint32_t> num_partitions_;\n  Parameter<uint32_t> min_sm_count_;\n  Parameter<std::vector<int32_t>> sms_per_partition_;\n  Parameter<std::string> nvtx_identifier_;\n  Parameter<int32_t> default_context_;\n\n  std::atomic<bool> initialized_{false};\n  std::mutex mutex_;\n  std::vector<CUgreenCtx> green_context_;\n  std::vector<CUcontext> cu_context_;\n  uint32_t max_sm_count_ = 0;\n  int32_t dev_id_ = -1;\n  uint32_t default_index_ = 0;\n\n  // Function table for CUDA driver symbols\n  CudaDriverFunctionTable cuda_driver_fn_table_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_CUDA_GREEN_CONTEXT_POOL_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/default_extension.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_DEFAULT_EXTENSION_HPP_\n#define NVIDIA_GXF_STD_DEFAULT_EXTENSION_HPP_\n\n#include <algorithm>\n#include <memory>\n#include <string>\n#include <type_traits>\n\n#include \"common/fixed_vector.hpp\"\n#include \"common/type_name.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/component_allocator.hpp\"\n#include \"gxf/std/extension.hpp\"\n#include \"gxf/std/new_component_allocator.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nnamespace detail {\n\nstruct VoidBaseHelper {};\n\ntemplate <typename T>\nstruct BaseTypenameAsString {\n  static const char* Value() { return TypenameAsString<T>(); }\n};\n\ntemplate <>\nstruct BaseTypenameAsString<VoidBaseHelper> {\n  static constexpr const char* Value() { return \"\"; }\n};\n\n}  // namespace detail\n\n// A standard component factor for GXF extensions. It keeps track of all components in the\n// extensions and provides mechanisms to create components.\nclass DefaultExtension : public Extension {\n public:\n  ~DefaultExtension() override = default;\n\n  // Sets the extension metadata info\n  gxf_result_t setInfo_abi(gxf_tid_t tid, const char* name, const char* desc, const char* author,\n                           const char* version, const char* license) override;\n\n\n  // Capture extension display metadata\n  gxf_result_t setDisplayInfo_abi(const char* display_name, const char* category,\n                                  const char* brief) override;\n\n  // Gets description of the extension and list of components it provides\n  gxf_result_t getInfo_abi(gxf_extension_info_t* info) override;\n\n  gxf_result_t checkInfo_abi() override;\n\n  // Gets description of specified component (No parameter information)\n  gxf_result_t getComponentInfo_abi(const gxf_tid_t tid, gxf_component_info_t* info) override;\n\n  gxf_result_t registerComponents_abi(gxf_context_t context) override;\n\n  gxf_result_t hasComponent_abi(const gxf_tid_t& tid) override;\n\n  gxf_result_t getComponentTypes_abi(gxf_tid_t* pointer, size_t* size) override;\n\n  gxf_result_t allocate_abi(gxf_tid_t tid, void** out_pointer) override;\n\n  gxf_result_t deallocate_abi(gxf_tid_t tid, void* pointer) override;\n\n  gxf_result_t getParameterInfo_abi(gxf_context_t context, const gxf_tid_t cid, const char* key,\n               gxf_parameter_info_t* info) override;\n\n  gxf_tid_t tid () const { return tid_; }\n\n  template <typename T, typename Base = detail::VoidBaseHelper>\n  Expected<void> add(gxf_tid_t tid, const char* description, const char* display_name = \"\",\n                     const char* brief = \"\") {\n    static_assert(std::is_same<Base, detail::VoidBaseHelper>::value\n                  || std::is_base_of<Base, T>::value,\n                  \"The given base class is not actually a base class\");\n    static_assert(!std::is_base_of<Codelet, T>::value ||\n                  (std::is_base_of<Codelet, Base>::value || std::is_same<Codelet, T>::value),\n                  \"If a component derives from Codelet then its base class also needs to derive \"\n                  \"from Codelet. Have you used Component as base class instead of Codelet?\");\n\n    if (find(tid)) {\n      return Unexpected{GXF_FACTORY_DUPLICATE_TID};\n    }\n\n    std::string display_name_str{display_name};\n    if (display_name_str.length() > 50) {\n      GXF_LOG_ERROR(\"Component display name '%s' exceeds 50 characters\", display_name);\n      return Unexpected{GXF_ARGUMENT_OUT_OF_RANGE};\n    }\n\n    std::string brief_str{brief};\n    if (brief_str.length() > 128) {\n      GXF_LOG_ERROR(\"Component brief '%s' exceeds 128 characters\", brief);\n      return Unexpected{GXF_ARGUMENT_OUT_OF_RANGE};\n    }\n\n    std::string description_str{description};\n    if (description_str.length() > 1026) {\n      GXF_LOG_ERROR(\"Component description '%s' exceeds 1026 characters\", description);\n      return Unexpected{GXF_ARGUMENT_OUT_OF_RANGE};\n    }\n\n    auto result = entries_.push_back({\n        tid, TypenameAsString<T>(), detail::BaseTypenameAsString<Base>::Value(),\n        description_str, display_name_str, brief_str,\n        std::is_abstract<T>::value ? nullptr : std::make_unique<NewComponentAllocator<T>>()});\n    if (!result) {\n      GXF_LOG_WARNING(\"Exceeding maximum number of components\");\n      return Unexpected{GXF_EXCEEDING_PREALLOCATED_SIZE};\n    }\n    return Success;\n  }\n\n private:\n  struct Entry {\n    gxf_tid_t tid;\n    std::string name;\n    std::string base;\n    std::string description;\n    std::string display_name;\n    std::string brief;\n    std::unique_ptr<ComponentAllocator> allocator;\n  };\n\n  Expected<Entry&> find(const gxf_tid_t& tid);\n\n  FixedVector<Entry, kMaxComponents> entries_;\n  gxf_tid_t tid_{GxfTidNull()};\n  std::string name_;\n  std::string description_;\n  std::string author_;\n  std::string extension_version_;\n  std::string gxf_core_version_{kGxfCoreVersion};  // FIXME: Use setInfo() api to set this post 2.3\n  std::string license_;\n  std::string display_name_;\n  std::string category_;\n  std::string brief_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_DEFAULT_EXTENSION_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/dlpack_utils.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cstdint>\n#include <string>\n#include <vector>\n\n#include \"dlpack/dlpack.h\"\n#include \"gxf/core/expected.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Determine the GPU device (if any) associated with a raw pointer\nExpected<DLDevice> DLDeviceFromPointer(void* ptr);\n\n/**\n * @brief Fill strides from the given DLTensor object.\n *\n * The following fields are used to fill strides:\n *\n * - ndim\n * - shape\n * - dtype\n *\n * If tensor's strides is nullptr, `strides` argument is filled with the calculated strides of the\n * given DLTensor object. Otherwise, `strides` argument is filled with the given DLTensor object's\n * strides. `strides` vector would be resized to the size of `ndim` field of the given DLTensor\n * object.\n *\n * @param tensor DLTensor object that holds information to fill strides.\n * @param[out] strides Strides to fill.\n * @param to_num_elments If true, the strides in `strides` argument are in number of elements, not\n * bytes (default: false).\n */\nvoid ComputeDLPackStrides(const DLTensor& tensor, std::vector<int64_t>& strides,\n                          bool to_num_elements = false);\n\n// Return DLDataType object from a NumPy type string.\nExpected<DLDataType> DLDataTypeFromTypeString(const std::string& typestr);\n\n/**\n * @brief Return a string providing the basic type of the homogeneous array in NumPy.\n *\n * Note: This method assumes little-endian for now.\n *\n * @return A const character pointer that represents a string\n */\nExpected<const char*> numpyTypestr(const DLDataType dtype);\n}  // namespace gxf\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/double_buffer_receiver.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef DOUBLE_BUFFER_RECEIVER_HPP\n#define DOUBLE_BUFFER_RECEIVER_HPP\n\n#include <memory>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/std/gems/staging_queue/staging_queue.hpp\"\n#include \"gxf/std/receiver.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// A receiver which uses a double-buffered queue where new messages are first pushed to a\n// backstage. Incoming messages are not immediately available and need to be moved to the mainstage\n// first.\nclass DoubleBufferReceiver : public Receiver {\n public:\n  using queue_t = ::gxf::staging_queue::StagingQueue<Entity>;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n\n  gxf_result_t initialize() override;\n\n  gxf_result_t deinitialize() override;\n\n  gxf_result_t pop_abi(gxf_uid_t* uid) override;\n\n  gxf_result_t push_abi(gxf_uid_t other) override;\n\n  gxf_result_t peek_abi(gxf_uid_t* uid, int32_t index) override;\n\n  gxf_result_t peek_back_abi(gxf_uid_t* uid, int32_t index) override;\n\n  size_t capacity_abi() override;\n\n  size_t size_abi() override;\n\n  gxf_result_t receive_abi(gxf_uid_t* uid) override;\n\n  size_t back_size_abi() override;\n\n  gxf_result_t sync_abi() override;\n\n  gxf_result_t sync_io_abi() override;\n\n  Parameter<uint64_t> capacity_;\n  Parameter<uint64_t> policy_;\n\n private:\n  std::unique_ptr<queue_t> queue_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/double_buffer_transmitter.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef DOUBLE_BUFFER_TRANSMITTER_HPP\n#define DOUBLE_BUFFER_TRANSMITTER_HPP\n\n#include <memory>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/std/gems/staging_queue/staging_queue.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// A transmitter which uses a double-buffered queue where messages are pushed to a backstage after\n// they are published. Outgoing messages are not immediately available and need to be\n// moved to the backstage first.\nclass DoubleBufferTransmitter : public Transmitter {\n public:\n  using queue_t = ::gxf::staging_queue::StagingQueue<Entity>;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n\n  gxf_result_t initialize() override;\n\n  gxf_result_t deinitialize() override;\n\n  gxf_result_t pop_abi(gxf_uid_t* uid) override;\n\n  gxf_result_t pop_io_abi(gxf_uid_t* uid) override;\n\n  gxf_result_t push_abi(gxf_uid_t other) override;\n\n  gxf_result_t peek_abi(gxf_uid_t* uid, int32_t index) override;\n\n  size_t capacity_abi() override;\n\n  size_t size_abi() override;\n\n  gxf_result_t publish_abi(gxf_uid_t uid) override;\n\n  size_t back_size_abi() override;\n\n  gxf_result_t sync_abi() override;\n\n  gxf_result_t sync_io_abi() override;\n\n  Parameter<uint64_t> capacity_;\n  Parameter<uint64_t> policy_;\n\n private:\n  std::unique_ptr<queue_t> queue_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/eos.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef EOS_HPP\n#define EOS_HPP\n\n#include <utility>\n\n#include \"gxf/core/entity.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// A component which represents an end-of-stream notification\nclass EndOfStream {\n public:\n  // Returns the stream which generated the notification. Negative value indicates\n  // pipeline/graph EoS. Non-negative value indicates individual stream EoS\n  int64_t stream_id() { return stream_id_; }\n\n  void stream_id(int64_t stream_id) { stream_id_ = stream_id; }\n\n  EndOfStream() = default;\n\n  ~EndOfStream() = default;\n\n  EndOfStream(const EndOfStream&) = delete;\n\n  EndOfStream(EndOfStream&& other) { *this = std::move(other); }\n\n  EndOfStream& operator=(const EndOfStream&) = delete;\n\n  EndOfStream& operator=(EndOfStream&& other) {\n    stream_id_ = other.stream_id_;\n    return *this;\n  }\n\n  // Factory method to create EOS message entity\n  static Expected<Entity> createEoSMessage(gxf_context_t context, int64_t stream_id = -1) {\n    Expected<Entity> message = Entity::New(context);\n    if (!message) { return message; }\n\n    auto eos = message.value().add<EndOfStream>();\n    if (!eos) { return ForwardError(eos); }\n    eos.value()->stream_id_ = stream_id;\n\n    return message;\n  }\n\n private:\n  int64_t stream_id_ = -1;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/event_based_scheduler.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_EVENT_BASED_SCHEDULER_HPP_\n#define NVIDIA_GXF_STD_EVENT_BASED_SCHEDULER_HPP_\n\n#include <algorithm>\n#include <atomic>\n#include <chrono>\n#include <condition_variable>\n#include <map>\n#include <memory>\n#include <mutex>\n#include <set>\n#include <shared_mutex>  // NOLINT\n#include <string>\n#include <thread>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"cuda.h\"\n\n#include \"common/fixed_map.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/std/clock.hpp\"\n#include \"gxf/std/cpu_thread.hpp\"\n#include \"gxf/std/cuda_green_context_pool.hpp\"\n#include \"gxf/std/gems/event_list/unique_event_list.hpp\"\n#include \"gxf/std/gems/staging_queue/staging_queue.hpp\"\n#include \"gxf/std/gems/timed_job_list/timed_job_list.hpp\"\n#include \"gxf/std/resources.hpp\"\n#include \"gxf/std/scheduler.hpp\"\n#include \"gxf/std/scheduling_condition.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Forward declarations\nclass EntityExecutor;\n\n/// @brief Event based scheduler\n///\n/// The scheduler requires a Clock to keep track of time. It spawns a dispatcher thread and several\n/// worker threads as configured. Dispatcher thread checks condition of entities and place them in\n/// worker queue if they are ready to run or have expected time to run.\nclass EventBasedScheduler : public Scheduler {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n  gxf_result_t prepare_abi(EntityExecutor* executor) override;\n  gxf_result_t schedule_abi(gxf_uid_t eid) override;\n  gxf_result_t unschedule_abi(gxf_uid_t eid) override;\n  gxf_result_t runAsync_abi() override;\n  gxf_result_t stop_abi() override;\n  gxf_result_t wait_abi() override;\n  gxf_result_t event_notify_abi(gxf_uid_t eid, gxf_event_t event) override;\n  gxf_result_t notifyDispatcher(gxf_uid_t eid = kNullUid);\n\n private:\n  // Enum to indicate if a worker thread has acquired an entity for execution or not\n  enum class EntityOwnership : uint8_t {\n    kFree = 0,\n    kAcquired\n  };\n\n  class ScheduleEntity {\n   public:\n    ScheduleEntity(gxf_uid_t eid, const char * name) {\n      eid_ = eid;\n      name_ = std::string(name);\n      condition_ = SchedulingCondition{SchedulingConditionType::READY, 0};\n      ownership_ = EntityOwnership::kFree;\n      queue_index_ = -1;\n      thread_pool_id_ = -1;\n      thread_id_ = kDefaultThreadPoolThreadId;\n      unschedule_ = false;\n      is_present_in_ready_queue_ = false;\n      cuda_context_ = nullptr;\n      cuda_green_context_pool_ = nullptr;\n    }\n\n    void updateCondition() {\n      condition_ = SchedulingCondition{SchedulingConditionType::READY, 0};\n    }\n\n    Expected<void> tryToAcquire(void) {\n      EntityOwnership free = EntityOwnership::kFree;\n      if (ownership_.compare_exchange_strong(free, EntityOwnership::kAcquired)) {\n        return Success;\n      }\n      return Unexpected{GXF_FAILURE};\n    }\n\n    Expected<void> releaseOwnership(void) {\n      EntityOwnership acquired = EntityOwnership::kAcquired;\n      if (ownership_.compare_exchange_strong(acquired, EntityOwnership::kFree)) {\n        return Success;\n      }\n      return Unexpected{GXF_FAILURE};\n    }\n\n    int32_t queue_index_;\n    gxf_uid_t eid_;\n    SchedulingCondition condition_;\n    std::string name_;\n    std::atomic<EntityOwnership> ownership_;\n    int32_t thread_pool_id_;\n    gxf_uid_t thread_id_;\n    bool unschedule_;\n    std::shared_timed_mutex ready_queue_sync_mutex_;\n    bool is_present_in_ready_queue_;\n    CUcontext cuda_context_ = nullptr;\n    CudaGreenContextPool* cuda_green_context_pool_ = nullptr;\n  };\n\n  std::unordered_map<gxf_uid_t, std::shared_ptr<ScheduleEntity>> entities_;\n\n  struct TimeStat {\n    std::atomic<uint64_t> execTime = {0};\n    std::atomic<uint64_t> waitTime = {0};\n    std::atomic<uint64_t> execCount = {0};\n  };\n\n  struct TimeStat dispatcherStats;\n  struct TimeStat workerStats;           // operations from multiple worker threads\n\n  static constexpr int64_t kMsToNs = 1'000'000l;      // Convenient constant of 1 ms = 1e6 ns\n  static constexpr int64_t kMaxSlipNs = 1 * kMsToNs;  // Max slip tolerance set to 1 ms\n  static constexpr size_t kMaxThreads = 128;          // Max number of worker threads\n\n  // Common thread id for all the worker threads in default thread pool\n  static constexpr int64_t kDefaultThreadPoolThreadId = -1;\n  // Position of ready queue for threads belonging to default thread pool\n  static constexpr int64_t kDefaultThreadPoolQueueIndex = 0;\n  // Entrance for dispatcher threads\n  void dispatcherThreadEntrance();\n  // notify the dispatcher thread\n  // Move entity to some queue\n  void dispatchEntity(std::shared_ptr<ScheduleEntity> e);\n  void dispatchEntityAsync(std::shared_ptr<ScheduleEntity> e);\n  // Entrance for async event handler thread\n  void asyncEventThreadEntrance();\n  // Entrance for worker threads\n  void workerThreadEntrance(ThreadPool* pool, int64_t thread_number,\n                            gxf_uid_t cpu_thread_cid = kUnspecifiedUid);\n\n  // Checks if need to stop due to no active entities or expiration\n  bool checkEndingCriteria(int64_t timestamp);\n\n  // Updates condition of specific entity id and keep track of how many entities are good to run\n  void updateCondition(std::shared_ptr<ScheduleEntity> e,\n   const SchedulingCondition& next_condition);\n\n  // stops all async threads and deactivates all the entities\n  gxf_result_t stopAllThreads();\n\n  // stops all jobs queued in dispatcher and workers\n  void stopAllJobs();\n\n  // returns the current state of the scheduler as string\n  const char* schedulerStateString();\n  // cache thread info for pinned job into resources_ and set cuda context if provided\n  gxf_result_t prepareResourceMapStrict(std::shared_ptr<ScheduleEntity> e);\n  // index of the ready queue in the ready_wait_time_jobs_ vector where entity is supposed to go\n\n  uint64_t getReadyCount();\n\n  // Helper functions for configuring worker threads\n  gxf_result_t configureThread(gxf_uid_t thread_uid, gxf_uid_t cpu_thread_cid);\n  gxf_result_t setCPUAffinity(const std::vector<uint32_t>& cpu_cores);\n  bool isRealtimeCPUThread(gxf_uid_t cpu_thread_cid);\n\n  // Load CUDA Driver API functions through runtime API\n  gxf_result_t loadDriverFunctions();\n\n  // Parameters\n  Parameter<Handle<Clock>> clock_;\n  Parameter<int64_t> max_duration_ms_;\n  Parameter<bool> stop_on_deadlock_;\n  Parameter<int64_t> stop_on_deadlock_timeout_;\n  Parameter<int64_t> worker_thread_number_;\n  Parameter<bool> thread_pool_allocation_auto_;\n  Parameter<std::string> worker_thread_name_id_;\n  // CPU core IDs to pin the worker threads to (empty means no core pinning)\n  Parameter<std::vector<uint32_t>> pin_cores_;\n\n  EntityExecutor* executor_ = nullptr;\n\n  Entity default_thread_pool_entity_;\n  ThreadPool* default_thread_pool_;\n\n  // thread pool set including default pool and added pools\n  std::set<ThreadPool*> thread_pool_set_;\n\n  // map for storing the relevant ready_wait_queue\n  FixedMap<gxf_uid_t, int> thread_queue_mapping_;\n\n  gxf_result_t thread_error_code_;\n\n  // A thread to dispatch jobs to worker pool\n  std::thread dispatcher_thread_;\n\n  // A thread to trigger exit after max time duration\n  std::thread max_duration_thread_;\n\n  // Mutex to synchronize dispatcher thread\n  std::mutex dispatcher_sync_mutex_;\n\n  // Mutex to synchronize max duration thread\n  std::mutex max_duration_sync_mutex_;\n\n  // instances of dispatcher/worker threads. 0 index would be async event handler thread.\n  std::vector<std::thread> async_threads_;\n\n  // Keep track of the timestamp that the scheduler started working for checking expiration\n  int64_t start_timestamp_ = 0;\n\n  // Queue with execution time for jobs to execute\n  std::vector<std::unique_ptr<TimedJobList<gxf_uid_t>>> ready_wait_time_jobs_;\n\n  // synchronize entities\n  std::mutex external_event_notification_mutex_;\n  std::mutex internal_event_notification_mutex_;\n\n  // Mutex to synchronize worker thread pool\n  std::mutex thread_sync_mutex_;\n  std::condition_variable thread_sync_cv_;\n\n  // Keep track of asynchronous external events\n  std::unique_ptr<UniqueEventList<gxf_uid_t>> external_event_notified_;\n  // Holds entities waiting for some event\n  std::unique_ptr<UniqueEventList<gxf_uid_t>> event_waiting_;\n  // Holds entities waiting for custom conditions\n  std::unique_ptr<UniqueEventList<gxf_uid_t>> waiting_;\n\n  // Keep track of asynchronous internal events\n  std::unique_ptr<UniqueEventList<gxf_uid_t>> internal_event_notified_;\n\n  // Running count of entities in flight / execution\n  std::atomic<uint8_t> running_threads_{0};\n\n  std::condition_variable external_event_notification_cv_;\n  std::condition_variable internal_event_notification_cv_;\n\n  std::condition_variable max_duration_thread_cv_;\n\n  // enum type to represent scheduler state changes\n  enum class State : uint8_t {\n    kNotStarted = 0,       // Scheduler has not started execution yet, starts during runAsync_abi()\n    kRunning,              // Execution threads are running\n    kStopping,             // Ending criteria reached, execution threads are doing pending jobs\n    kStopped,              // Scheduler has stopped\n  };\n\n  std::atomic<State> state_{State::kNotStarted};\n  std::mutex state_change_mutex_;\n  std::condition_variable work_done_cv_;\n\n  // latest timestamp from last should_stop == false\n  int64_t last_no_stop_ts_ = 0;\n  // maintain last no stop timestamp, and check if need to update should_stop\n  gxf_result_t stop_on_deadlock_timeout(const int64_t timeout,\n    const int64_t now, bool& should_stop);\n\n  // Deadlock detection grace period end timestamp\n  int64_t grace_end_timestamp_ = 0;\n  // Armed flag to require two consecutive deadlock checks before exiting\n  bool deadlock_armed_ = false;\n\n  // CUDA Driver API\n  CUresult (*fnCuCtxPushCurrent)(CUcontext) = nullptr;\n  CUresult (*fnCuCtxPopCurrent)(CUcontext*) = nullptr;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_EVENT_BASED_SCHEDULER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/extension.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_EXTENSION_HPP\n#define NVIDIA_GXF_STD_EXTENSION_HPP\n\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/component_factory.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Interface used for extensions. An extension class is holding information about the extension\n// and allows creation of components which are provided by the extension.\nclass Extension : public ComponentFactory {\n public:\n  virtual ~Extension() = default;\n\n  Extension(const Extension&) = delete;\n  Extension(Extension&&) = delete;\n  Extension& operator=(const Extension&) = delete;\n  Extension& operator=(Extension&&) = delete;\n\n  // Capture extension metadata\n  virtual gxf_result_t setInfo_abi(gxf_tid_t tid, const char* name, const char* desc,\n                                   const char* author, const char* version,\n                                   const char* license) = 0;\n\n  // Capture extension display metadata\n  virtual gxf_result_t setDisplayInfo_abi(const char* display_name, const char* category,\n                                          const char* brief) = 0;\n\n  // Check if extension metadata has been captured\n  virtual gxf_result_t checkInfo_abi() = 0;\n\n  // Gets description of the extension and list of components it provides\n  virtual gxf_result_t getInfo_abi(gxf_extension_info_t* info) = 0;\n\n  // Registers all components in the extension with the given context.\n  virtual gxf_result_t registerComponents_abi(gxf_context_t context) = 0;\n\n  // Checks if a component is present in the extension or not\n  virtual gxf_result_t hasComponent_abi(const gxf_tid_t& tid) = 0;\n\n  // Gets a list with IDs of all types which are registered with this factory.\n  virtual gxf_result_t getComponentTypes_abi(gxf_tid_t* pointer, size_t* size) = 0;\n\n  // Gets description of specified component (No parameter information)\n  virtual gxf_result_t getComponentInfo_abi(const gxf_tid_t tid, gxf_component_info_t* info) = 0;\n\n  // Gets description of specified parameter\n  virtual gxf_result_t getParameterInfo_abi(gxf_context_t context, const gxf_tid_t cid,\n                       const char* key, gxf_parameter_info_t* info) = 0;\n\n  Expected<void> registerComponents(gxf_context_t context);\n  Expected<void> hasComponent(const gxf_tid_t& tid);\n  Expected<void> getComponentTypes(gxf_tid_t* pointer, size_t* size);\n  Expected<void> setInfo(gxf_tid_t tid, const char* name, const char* desc,\n                                   const char* author, const char* version, const char* license);\n  Expected<void> setDisplayInfo(const char* display_name, const char* category, const char* brief);\n  Expected<void> checkInfo();\n  Expected<void> getInfo(gxf_extension_info_t* info);\n  Expected<void> getComponentInfo(const gxf_tid_t tid, gxf_component_info_t* info);\n  Expected<void> getParameterInfo(gxf_context_t context, const gxf_tid_t cid, const char* key,\n                                  gxf_parameter_info_t* info);\n\n protected:\n  Extension() = default;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/extension_factory_helper.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CORE_FACTORY_HPP\n#define NVIDIA_GXF_CORE_FACTORY_HPP\n\n#include <memory>\n#include <utility>\n\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/default_extension.hpp\"\n\n// Helper macro to create an extensions factory. Every component in this extensions must be\n// explicitly registered. Otherwise it can not be used by GXF applications.\n//\n// A component can be registered using the macro GXF_EXT_FACTORY_ADD. For each\n// component the base class need to be specified. Components base classes must be registered before\n// they can be used as a base class in a component registration. If a component does not have a base\n// class the macro GXF_EXT_FACTORY_ADD_0 is used instead.\n//\n// Components can have at most one base class. Multiple base classes are not supported.\n//\n// Every component must be registered with a unique 128-bit identifier. The identifier must be\n// unique across all existing extensions.\n//\n// Note that the extension factory can also be created manually without using these macros.\n//\n// Usage example:\n//   GXF_EXT_FACTORY_BEGIN(0x8ec2d5d6b5df48bf, 0x8dee0252606fdd7e, \"1.0.0\")\n//   GXF_EXT_FACTORY_SET_INFO(\"Extension Name\", \"Extension Desc\", \"Author\", \"LICENSE\")\n//   GXF_EXT_FACTORY_ADD(0x792151bf31384603, 0xa9125ca91828dea8,\n//                       nvidia::gxf::Queue, nvidia::gxf::Component,\n//                       \"Interface for storing entities in a queue\");\n//   GXF_EXT_FACTORY_ADD(0xc30cc60f0db2409d, 0x92b6b2db92e02cce,\n//                        nvidia::gxf::Transmitter, nvidia::gxf::Queue,\n//                        \"Interface for publishing entities\");\n//   ...\n//   GXF_EXT_FACTORY_END()\n#define GXF_EXT_FACTORY_BEGIN()                                                                    \\\n  namespace {                                                                                      \\\n    nvidia::gxf::Expected<std::unique_ptr<nvidia::gxf::ComponentFactory>>                          \\\n    CreateComponentFactory() {                                                                     \\\n      auto factory = std::make_unique<nvidia::gxf::DefaultExtension>();                            \\\n      if (!factory) { return nvidia::gxf::Unexpected{GXF_OUT_OF_MEMORY}; }                         \\\n\n// See GXF_EXT_FACTORY_BEGIN for more information.\n#define GXF_EXT_FACTORY_SET_INFO(H1, H2, NAME, DESC, AUTHOR, VERSION, LICENSE)                     \\\n      {                                                                                            \\\n        const nvidia::gxf::Expected<void> result = factory->setInfo({(H1), (H2)}, NAME, DESC,      \\\n                                                                     AUTHOR, VERSION, LICENSE);    \\\n        if (!result) { return nvidia::gxf::ForwardError(result); }                                 \\\n      }                                                                                            \\\n\n// See GXF_EXT_FACTORY_BEGIN for more information.\n#define GXF_EXT_FACTORY_SET_DISPLAY_INFO(DISPLAY_NAME, CATEGORY, BRIEF)                            \\\n      {                                                                                            \\\n        const nvidia::gxf::Expected<void> result = factory->setDisplayInfo(DISPLAY_NAME, CATEGORY, \\\n                                                                           BRIEF);                 \\\n        if (!result) { return nvidia::gxf::ForwardError(result); }                                 \\\n      }\n\n// See GXF_EXT_FACTORY_BEGIN for more information.\n#define GXF_EXT_FACTORY_ADD_0(H1, H2, TYPE, DESC)                                                  \\\n      {                                                                                            \\\n        const nvidia::gxf::Expected<void> result = factory->add<TYPE>({(H1), (H2)}, DESC);         \\\n        if (!result) { return nvidia::gxf::ForwardError(result); }                                 \\\n      }                                                                                            \\\n\n// See GXF_EXT_FACTORY_BEGIN for more information.\n#define GXF_EXT_FACTORY_ADD_0_VERBOSE(H1, H2, TYPE, DISPLAY_NAME, BRIEF, DESC)                     \\\n      {                                                                                            \\\n        const nvidia::gxf::Expected<void> result = factory->add<TYPE>({(H1), (H2)}, DESC,          \\\n                                                                       DISPLAY_NAME, BRIEF);       \\\n        if (!result) { return nvidia::gxf::ForwardError(result); }                                 \\\n      }\n\n// See GXF_EXT_FACTORY_BEGIN for more information.\n#define GXF_EXT_FACTORY_ADD_0_LITE(H1, H2, TYPE)                                                   \\\n      {                                                                                            \\\n        const nvidia::gxf::Expected<void> result = factory->add<TYPE>({(H1), (H2)}, \"\");           \\\n        if (!result) { return nvidia::gxf::ForwardError(result); }                                 \\\n      }                                                                                            \\\n\n// See GXF_EXT_FACTORY_BEGIN for more information.\n#define GXF_EXT_FACTORY_ADD(H1, H2, TYPE, BASE, DESC)                                              \\\n      {                                                                                            \\\n        const nvidia::gxf::Expected<void> result = factory->add<TYPE, BASE>({(H1), (H2)}, DESC);   \\\n        if (!result) { return nvidia::gxf::ForwardError(result); }                                 \\\n      }                                                                                            \\\n\n// See GXF_EXT_FACTORY_BEGIN for more information.\n#define GXF_EXT_FACTORY_ADD_VERBOSE(H1, H2, TYPE, BASE,  DISPLAY_NAME, BRIEF, DESC)                \\\n      {                                                                                            \\\n        const nvidia::gxf::Expected<void> result = factory->add<TYPE, BASE>({(H1), (H2)}, DESC,    \\\n                                                                       DISPLAY_NAME, BRIEF);       \\\n        if (!result) { return nvidia::gxf::ForwardError(result); }                                 \\\n      }\n\n// See GXF_EXT_FACTORY_BEGIN for more information.\n#define GXF_EXT_FACTORY_ADD_LITE(H1, H2, TYPE, BASE)                                               \\\n      {                                                                                            \\\n        const nvidia::gxf::Expected<void> result = factory->add<TYPE, BASE>({(H1), (H2)}, \"\");     \\\n        if (!result) { return nvidia::gxf::ForwardError(result); }                                 \\\n      }                                                                                            \\\n\n// See GXF_EXT_FACTORY_BEGIN for more information.\n#define GXF_EXT_FACTORY_END()                                                                      \\\n      const nvidia::gxf::Expected<void> result = factory->checkInfo();                             \\\n      if (!result) { return nvidia::gxf::ForwardError(result); }                                   \\\n      return std::move(factory);                                                                   \\\n    }                                                                                              \\\n  }  /* namespace */                                                                               \\\n                                                                                                   \\\n  extern \"C\" {                                                                                     \\\n    gxf_result_t GxfExtensionFactory(void** result) {                                              \\\n      static nvidia::gxf::Expected<std::unique_ptr<nvidia::gxf::ComponentFactory>> s_factory       \\\n          = CreateComponentFactory();                                                              \\\n      if (!s_factory) { return s_factory.error(); }                                                \\\n      *result = s_factory.value().get();                                                           \\\n      return GXF_SUCCESS;                                                                          \\\n    }                                                                                              \\\n  }  /* extern \"C\" */                                                                              \\\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/gems/event_list/event_list.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_EVENT_LIST_HPP_\n#define NVIDIA_GXF_EVENT_LIST_HPP_\n\n#include <algorithm>\n#include <condition_variable>\n#include <list>\n#include <mutex>\n#include <queue>\n#include <string>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// A simple thread safe list implementation to store and retrieve events\ntemplate <typename T>\nclass EventList {\n public:\n  EventList() = default;\n  EventList(const EventList<T>&) = delete;\n  EventList& operator=(const EventList<T>&) = delete;\n\n  // Adds an item on to the event list\n  void pushEvent(T item) {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    list_.push_back(item);\n  }\n\n  // Removes an event from the list if it exists and returns true,\n  // returns false otherwise\n  void removeEvent(T item) {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n\n    for (auto it = list_.begin(); it != list_.end();) {\n      if (*it == item) {\n        // remove all instances in case of multiple event notifications\n        // for the same entity\n        it = list_.erase(it);\n      } else {\n        ++it;\n      }\n    }\n  }\n\n  // pops the first element in the list\n  Expected<T> popEvent() {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    if (list_.empty()) { return Unexpected{GXF_FAILURE}; }\n\n    T event = list_.front();\n    list_.pop_front();\n    return event;\n  }\n\n  // exports a copy of the events in the list\n  std::list<T> exportList() const {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    return list_;\n  }\n\n  // checks if the list is empty\n  bool empty() const {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    return list_.empty();\n  }\n\n  // returns size of the event list\n  size_t size() const {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    return list_.size();\n  }\n\n  // checks if the list has event\n  bool hasEvent(T item) const {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    for (const auto& element : list_) {\n      if (item == element) { return true; }\n    }\n    return false;\n  }\n\n  // clears the event list\n  void clear() {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    list_.clear();\n  }\n\n  mutable std::mutex list_mutex_;\n  std::list<T> list_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_EVENT_LIST_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/gems/event_list/unique_event_list.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_UNIQUE_EVENT_LIST_HPP_\n#define NVIDIA_GXF_UNIQUE_EVENT_LIST_HPP_\n\n#include <algorithm>\n#include <list>\n#include <mutex>\n#include <queue>\n#include <string>\n#include <unordered_map>\n#include <unordered_set>\n#include <utility>\n#include <vector>\n\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// A simple thread safe list implementation to store and retrieve events\ntemplate <typename T>\nclass UniqueEventList {\n public:\n  UniqueEventList() = default;\n  UniqueEventList(const UniqueEventList<T>&) = delete;\n  UniqueEventList& operator=(const UniqueEventList<T>&) = delete;\n\n  // Adds an item on to the event list\n  bool pushEvent(T item) {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    if (items_.find(item) != items_.end()) { return false; }\n    list_.push_back(item);\n    items_.insert(std::make_pair(item, --list_.end()));\n    return true;\n  }\n\n  // Removes an event from the list if it exists and returns true,\n  // returns false otherwise\n  void removeEvent(T item) {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n\n    if (items_.find(item) == items_.end()) { return; }\n    list_.erase(items_.at(item));\n    items_.erase(item);\n  }\n\n  // pops the first element in the list\n  Expected<T> popEvent() {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    if (list_.empty()) { return Unexpected{GXF_FAILURE}; }\n\n    T event = list_.front();\n    list_.pop_front();\n    items_.erase(event);\n    return event;\n  }\n\n  // exports a copy of the events in the list\n  std::list<T> exportList() const {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    return list_;\n  }\n\n  // checks if the list is empty\n  bool empty() const {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    return list_.empty();\n  }\n\n  // returns size of the event list\n  size_t size() const {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    return items_.size();\n  }\n\n  // checks if the list has event\n  bool hasEvent(T item) const {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    return items_.find(item) != items_.end() ? true : false;\n  }\n\n  // clears the event list\n  void clear() {\n    std::lock_guard<std::mutex> lock(list_mutex_);\n    list_.clear();\n    items_.clear();\n  }\n\n  mutable std::mutex list_mutex_;\n  std::list<T> list_;\n  std::unordered_map<T, typename std::list<T>::iterator> items_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_UNIQUE_EVENT_LIST_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/gems/queue_thread/queue_thread.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_GEMS_QUEUE_THREAD_HPP\n#define NVIDIA_GXF_STD_GEMS_QUEUE_THREAD_HPP\n\n#include <sys/syscall.h>\n#include <unistd.h>\n\n#include <condition_variable>\n#include <functional>\n#include <future>\n#include <list>\n#include <memory>\n#include <mutex>\n#include <sstream>\n#include <string>\n#include <thread>\n#include <utility>\n\n#include \"common/assert.hpp\"\n#include \"common/logger.hpp\"\n\n#define STR(str) ((str).empty() ? \"DefaultName\" : (str).c_str())\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * Entry wrapper for the user item and its associated promise\n*/\ntemplate <typename ItemType>\nstruct UserItemWithPromise {\n  // typedef ItemType user_item_type;\n  ItemType user_item;\n  std::promise<bool> promise;\n  bool is_stop_signal = false;\n\n  UserItemWithPromise(ItemType i) : user_item(std::move(i)) {}\n  UserItemWithPromise() : is_stop_signal(true) {}\n};\n\n/**\n * A thread safe generic container serving as the queue for QueueThread\n*/\ntemplate <typename ItemType>\nclass GuardQueue {\n public:\nusing Entry = UserItemWithPromise<ItemType>;\n  void push(Entry data) {\n    std::unique_lock<std::mutex> lock(queue_mutex_);\n    queue_.emplace_back(std::move(data));\n    queue_cv_.notify_one();\n  }\n  Entry pop() {\n    GXF_LOG_VERBOSE(\"GuardQueue::pop() acquiring lock...\");\n    std::unique_lock<std::mutex> lock(queue_mutex_);\n    GXF_LOG_VERBOSE(\"GuardQueue::pop() acquired lock, waiting on cv...\");\n    queue_cv_.wait(\n      lock, [this]() {\n        bool wait_condition = wakeup_once_ || !queue_.empty();\n        GXF_LOG_VERBOSE(\"GuardQueue::pop() cv wait condition[%d]\", wait_condition);\n        return wait_condition; });\n    if (wakeup_once_) {\n      wakeup_once_ = false;\n      GXF_LOG_VERBOSE(\"GuardQueue pop end on wakeup signal\");\n      // empty Entry carries is_stop_signal true\n      return Entry();\n    }\n    GXF_ASSERT_TRUE(!queue_.empty());\n    Entry ret = std::move(*queue_.begin());\n    queue_.erase(queue_.begin());\n    GXF_LOG_VERBOSE(\"GuardQueue::pop() popped an entry\");\n    return ret;\n  }\n  void wakeupOnce() {\n    GXF_LOG_VERBOSE(\"GuardQueue trigger wakeup once\");\n    std::unique_lock<std::mutex> lock(queue_mutex_);\n    wakeup_once_ = true;\n    queue_cv_.notify_all();\n    GXF_LOG_VERBOSE(\"GuardQueue finish wakeup once notification\");\n  }\n  void clear() {\n    GXF_LOG_VERBOSE(\"GuardQueue clear\");\n    std::unique_lock<std::mutex> lock(queue_mutex_);\n    queue_.clear();\n    wakeup_once_ = false;\n  }\n  int size() {\n    std::unique_lock<std::mutex> lock(queue_mutex_);\n    return queue_.size();\n  }\n\n private:\n  std::mutex queue_mutex_;\n  std::condition_variable queue_cv_;\n  std::list<Entry> queue_;\n  bool wakeup_once_ = false;\n};\n\n/**\n * The Async Runnable\n * Usage is similar to std::thread, but here the run function is event-driven callback.\n *\n * Construction:\n * 1. user event type in template;\n * 2. user run function implementation that takes user event type;\n * 3. thread name.\n *\n * Upon startup or no event is enqueued, the process thread remains sleep.\n * Whenever an event is raised in caller thread, simply enqueue that event\n * then the run function callback will process the event asynchronously\n *\n * Optionally an std::future is returned for each enqueued event, such that\n * users can synchronize completion of the event.\n*/\ntemplate <typename ItemType>\nclass QueueThread {\n public:\n  using Entry = UserItemWithPromise<ItemType>;\n  using RunFunction = std::function<bool(ItemType)>;\n\n  /**\n   * Constructor, asynchronously running RunFunction\n  */\n  QueueThread(RunFunction run_function, const std::string& name)\n    : run_function_(run_function) {\n    std::promise<void> promise;\n    std::future<void> future = promise.get_future();\n    GXF_LOG_DEBUG(\"QueueThread starting new thread\");\n    thread_ = std::thread([&promise, this]() {\n      GXF_LOG_DEBUG(\"QueueThread std::thread created[name: %s, ID: %s]\",\n        this->name_.c_str(), callerThreadId().c_str());\n      promise.set_value();\n      this->threadLoop();\n    });\n    setThreadName(name);\n    future.wait();\n  }\n\n  /**\n   * set thread name for profiling display\n  */\n  void setThreadName(const std::string& name) {\n    GXF_ASSERT_TRUE(!name.empty());\n    name_ = name;\n    // POSIX pthread_setname_np max length limit 15 characters plus null terminator\n    std::string pthread_name = (name.length() > 15) ? name.substr(0, 15) : name;\n    if (thread_.joinable()) {\n      if (pthread_setname_np(thread_.native_handle(), pthread_name.c_str()) != 0) {\n        GXF_LOG_ERROR(\"set thread name: %s failed\", STR(pthread_name));\n        return;\n      }\n      GXF_LOG_DEBUG(\"QueueThread set new thread name: %s\", STR(pthread_name));\n    }\n  }\n\n  /**\n   * Destructor\n  */\n  ~QueueThread() {\n    if (!joined_) {\n      // Ensure the stop logic is called if not already done so\n      stop();\n    }\n    // Clear any remaining items after the thread has been joined\n    guard_queue_.clear();\n  }\n\n  /**\n   * Main API. Enqueue item to process, return future to sync the result.\n  */\n  std::future<bool> queueItem(ItemType item) {\n    Entry itemWithPromise(std::move(item));\n    std::future<bool> future = itemWithPromise.promise.get_future();\n    guard_queue_.push(std::move(itemWithPromise));\n\n    return future;\n  }\n\n  /**\n   * Thread safe size check\n  */\n  int size() {\n    return guard_queue_.size();\n  }\n\n  /**\n   * Stop the Async Runnable\n  */\n  void stop() {\n    std::string caller_thread_id = callerThreadId();\n    GXF_LOG_DEBUG(\"QueueThread[%s]::stop() caller thread[%s] acquiring stop lock...\",\n      name_.c_str(), caller_thread_id.c_str());\n    {\n      std::lock_guard<std::mutex> lock(stop_mutex_);\n      stop_requested_ = true;\n    }\n    GXF_LOG_DEBUG(\"QueueThread[%s]::stop() caller thread[%s] acquired stop lock\",\n      name_.c_str(), caller_thread_id.c_str());\n    // Wake up the thread if it's waiting for new items\n    guard_queue_.wakeupOnce();\n    // Notify the wait() that stop was requested\n    stop_cv_.notify_all();\n    // Try join the thread, thread safe\n    joinThread();\n  }\n\n  /**\n   * Wait the Async Runnable\n  */\n  void wait() {\n    std::string caller_thread_id = callerThreadId();\n    GXF_LOG_DEBUG(\"QueueThread[%s]::wait() caller thread[%s] acquiring stop lock...\",\n      name_.c_str(), caller_thread_id.c_str());\n\n    std::unique_lock<std::mutex> lock(stop_mutex_);\n\n    GXF_LOG_DEBUG(\"QueueThread[%s]::wait() caller thread[%s] acquired stop lock\",\n      name_.c_str(), caller_thread_id.c_str());\n    // Block caller thread\n    stop_cv_.wait(lock, [this]() {\n      bool wait_condition = stop_requested_ && guard_queue_.size() == 0;\n      GXF_LOG_DEBUG(\"stop_requested_[%d] && guard_queue_.size()[%d], cv wait condition[%d]\",\n        stop_requested_.load(), guard_queue_.size(), wait_condition);\n      return wait_condition; });\n    // Try join the thread, thread safe\n    joinThread();\n  }\n\n private:\n  /**\n   * Thread safe join thread\n  */\n  void joinThread() {\n    std::string caller_thread_id = callerThreadId();\n    GXF_LOG_DEBUG(\"QueueThread[%s]::joinThread() caller thread[%s] acquiring join lock...\",\n      name_.c_str(), caller_thread_id.c_str());\n\n    // Ensure only one caller can join the thread\n    std::lock_guard<std::mutex> lock(join_mutex_);\n\n    GXF_LOG_DEBUG(\"QueueThread[%s]::joinThread() caller thread[%s] acquired join lock\",\n      name_.c_str(), caller_thread_id.c_str());\n    if (thread_.joinable()) {\n      GXF_LOG_DEBUG(\"QueueThread[%s]::joinThread() got its thread joinable(), joining...\",\n        name_.c_str());\n      thread_.join();\n      GXF_LOG_DEBUG(\"QueueThread[%s]::joinThread() got its thread joined\", name_.c_str());\n      joined_ = true;\n    }\n  }\n\n  /**\n   * Thread loop listening and processing tasks\n  */\n  void threadLoop() {\n    while (true) {\n      try {\n        Entry entry = guard_queue_.pop();\n        if (entry.is_stop_signal) {\n          GXF_LOG_INFO(\"QueueThread[%s]::threadLoop() return\", STR(name_));\n          return;  // okay to return\n        }\n        if (stop_requested_ && guard_queue_.size() == 0) {\n          // Exit loop if stop is requested and all entries are processed\n          break;\n        }\n        bool result = run_function_(std::move(entry.user_item));\n        entry.promise.set_value(result);\n        if (!result) {\n          GXF_LOG_INFO(\"QueueThread[%s]::threadLoop() return and stop\", STR(name_));\n          break;  // don't return\n        }\n      }\n      catch (const std::exception& e) {  // unexpected but continue\n        GXF_LOG_ERROR(\"QueueThread:%s %s\", STR(name_), e.what());\n        // move on to next\n        continue;\n      }\n      catch (...) {  // unexpected\n        GXF_LOG_ERROR(\n          \"QueueThread:%s internal unexpected error, may cause stop\", STR(name_));\n        // Usually can move on to next, but need to check\n        continue;\n      }\n    }\n\n    GXF_LOG_DEBUG(\"threadLoop() break while loop\");\n    // Clear the queue before exiting the thread loop\n    guard_queue_.clear();\n    // unblock external thread blocking on wait() call\n    stop_requested_ = true;\n    // Notify that the thread is about to exit after processing all entries\n    stop_cv_.notify_all();\n  }\n\n private:\n  // basic member variables\n  std::thread thread_;\n  std::string name_;\n  RunFunction run_function_;\n  GuardQueue<ItemType> guard_queue_;\n\n  // member variable for stopping\n  std::atomic<bool> stop_requested_{false};\n  std::mutex stop_mutex_;\n  std::condition_variable stop_cv_;\n\n  // member variable for joining thread\n  std::mutex join_mutex_;\n  bool joined_ = false;\n\n private:\n  std::string callerThreadId() {\n    pid_t tid = syscall(SYS_gettid);\n    std::stringstream ss;\n    ss << tid;\n    return ss.str();\n  }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_GEMS_QUEUE_THREAD_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/gems/staging_queue/staging_queue.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_GEMS_STAGING_QUEUE_HPP\n#define NVIDIA_GXF_STD_GEMS_STAGING_QUEUE_HPP\n\n#include <mutex>\n#include <utility>\n#include <vector>\n\n#include \"common/assert.hpp\"\n#include \"gxf/std/gems/staging_queue/staging_queue_iterator.hpp\"\n\nnamespace gxf {\nnamespace staging_queue {\n\n// Defines the behavior of a StagingQueue in case an element is added while the queue is already\n// full.\nenum class OverflowBehavior {\n  // Pops oldest item to make room for the incoming item.\n  kPop,\n  // Rejects the incoming item.\n  kReject,\n  // The queue goes into an error state. Currently this raises a PANIC.\n  kFault\n};\n\n// A thread-safe double-buffered queue implemented based on a ring buffer.\n//\n// This data structure provides a queue-like interface to a collection of items. When items are\n// added to the queue with the function push they are placed into a backstage area. The pop\n// function and query functions like size, peek or latest only operate on items on\n// the main stage. Items are moved from the backstage to the main stage by calling the sync\n// function. The queue only provides a fixed number of slots for elements. If elements are added to\n// a full queue a user-selected strategy is used to determine how to proceed. Note that the overflow\n// strategy can by triggered when calling push in case the backstage area is full, or when calling\n// sync in case the main stage is too full to receive all items from the backstage. When items are\n// removed from the queue they are overwritten with a user-defined default value. This is useful\n// for example in case shared pointers are stored in the queue. All functions of this type are\n// protected by a common mutex thus making this a thread-safe type. The queue is implemented as a\n// ring buffer internally.\n//\n// The following diagram depicts an example of a queue with a total capacity of four items. It\n// currently stores three items in the main stage and two items in the backstage. O indicates\n// empty slots, X indicates (filled) main stage slots and B indicates (filled) backstage slots.\n// There are three empty slots as neither main stage nor backstage are at full capacity. If the\n// sync function would be called in this situation the overflow behavior would be triggered as\n// the main stage does not have enough room to receive all items from the backstage.\n//\n//     # start of ringbuffer           # end of ringbuffer\n//     |                               |\n//     | O | X | X | X | B | B | O | O |\n//         |           |       |\n//         |           |       # end\n//         # begin     |\n//                     # backstage\n//\ntemplate <typename T>\nclass StagingQueue {\n public:\n  using const_iterator_t = StagingQueueIterator<T>;\n\n  // Creates a new staging queue. 'capacity' indicates the maximum number of elements allowed in\n  // the queue. Note that the queue will allocate memory to hold two times capacity slots of item\n  // type T. 'overflow_behavior' defines what will happen if a new item is added to a full queue.\n  // 'null' is the default element which is used for empty slots. If an element is removed from the\n  // queue the slot it occupied is set to this value.\n  StagingQueue(size_t capacity, OverflowBehavior overflow_behavior, T null);\n\n  // Creates an empty staging queue with no capacity and Fault overflow behavior\n  StagingQueue();\n\n  // Gets the overflow behavior which is used by this queue\n  OverflowBehavior overflow_behavior() const;\n\n  // Returns true if there are no elements in the main stage of the queue.\n  // Identical to 'size() == 0;'\n  bool empty() const;\n  // Returns the number of elements in the main stage of the queue.\n  size_t size() const;\n  // Returns the maximum number of elements which can be stored in the main stage.\n  size_t capacity() const;\n\n  // Returns the number of elements in the back main stage of the queue.\n  size_t back_size() const;\n\n  // Gets the item at position 'index' in the main stage of the queue starting with the oldest item.\n  // Returns a reference to the null object if there are no items in the main stage.\n  const T& peek(size_t index = 0) const;\n  // Gets the item at position 'index' in the back stage of the queue starting with the oldest item.\n  // Returns a reference to the null object if there are no items in the back stage.\n  const T& peek_backstage(size_t index = 0) const;\n  // Gets the item at position 'index' in the main stage of the queue starting with the newest item.\n  // Returns a reference to the null object if there are no items in the main stage.\n  // Identical to 'peek(size() - 1 - index)'.\n  const T& latest(size_t index = 0) const;\n\n  // Gives an iterator pointing to the first element in the main stage.\n  const_iterator_t begin() const;\n  // Gives an iterator pointing to the element after the last element in the main stage.\n  const_iterator_t end() const;\n\n  // Removes the oldest item from the queue's main stage and returns it. In case there are no items\n  // in the main stage this function simply returns a copy of the null object. This function\n  // invalidates iterators returned by begin() or end().\n  T pop();\n  // Removes all items from the main stage. This function invalidates iterators in the same way\n  // as the pop() function.\n  void popAll();\n\n  // Adds a new item to the back stage. In case the back stage is at capacity the overflow behavior\n  // is used to decide what to do. The added item will NOT be visible to any other functions which\n  // operate on the main stage, like size, peek, pop or similar, until 'sync' is called.\n  bool push(T item);\n\n  // Moves all items from the back stage to the main stage. In case the main stage is too full\n  // to receive all items from the back stage the specified overflow behavior is used to decide what\n  // to do. This function invalidates iterators in the same way as the pop() function.\n  bool sync();\n\n private:\n  // Returns the item in the underlying ringbuffer which is at position 'index' relative to the\n  // start of the ring buffer.\n  const T& at(size_t index) const { return items_[index % items_.size()]; }\n  T& at(size_t index) { return items_[index % items_.size()]; }\n\n  // The maximum number of items in the main stage and back stage. Total number items in the queue\n  // is two times capacity.\n  const size_t capacity_;\n  // This behavior defines if either push or sync are called but the main stage contains too many\n  // items already.\n  const OverflowBehavior overflow_behavior_;\n\n  // The null value is used for empty slots. It is also used as return value for peek and latest\n  // in case the given index is out of bounds.\n  T null_;\n\n  // The container which holds the elements of the main stage and back stage. The container\n  // allocates memory in the constructor and does not change it's capacity thereafter.\n  std::vector<T> items_;\n\n  // Index of the first element of the main stage. The value of begin_ is guaranteed to be in the\n  // range {0, ..., items_.size() - 1}.\n  size_t begin_;\n\n  // Number of elements in the mainstage.\n  size_t num_mainstage_;\n  // Number of elements in the backstage.\n  size_t num_backstage_;\n\n  // This mutext protects all functions from concurrent access.\n  mutable std::mutex mutex_;\n};\n\n//--------------------------------------------------------------------------------------------------\n\ntemplate <typename T>\nStagingQueue<T>::StagingQueue(size_t capacity, OverflowBehavior overflow_behavior, T null)\n    : capacity_(capacity),\n      overflow_behavior_(overflow_behavior),\n      null_(null),\n      items_(2 * capacity, null),\n      begin_(0),\n      num_mainstage_(0),\n      num_backstage_(0) {}\n\ntemplate <typename T>\nStagingQueue<T>::StagingQueue()\n    : capacity_(0),\n      overflow_behavior_(OverflowBehavior::kFault),\n      null_(),\n      items_(0),\n      begin_(0),\n      num_mainstage_(0),\n      num_backstage_(0) {}\n\ntemplate <typename T>\nOverflowBehavior StagingQueue<T>::overflow_behavior() const {\n  // Mutex not necessary as overflow_behavior_ is immutable.\n  return overflow_behavior_;\n}\n\ntemplate <typename T>\nbool StagingQueue<T>::empty() const {\n  std::lock_guard<std::mutex> lock(mutex_);\n  return num_mainstage_ == 0;\n}\n\ntemplate <typename T>\nsize_t StagingQueue<T>::size() const {\n  std::lock_guard<std::mutex> lock(mutex_);\n  return num_mainstage_;\n}\n\ntemplate <typename T>\nsize_t StagingQueue<T>::capacity() const {\n  // Mutex not necessary as capacity_ is immutable.\n  return capacity_;\n}\n\ntemplate <typename T>\nsize_t StagingQueue<T>::back_size() const {\n  std::lock_guard<std::mutex> lock(mutex_);\n  return num_backstage_;\n}\n\ntemplate <typename T>\nconst T& StagingQueue<T>::peek(size_t index) const {\n  std::lock_guard<std::mutex> lock(mutex_);\n  if (index < num_mainstage_) {\n    return at(begin_ + index);\n  } else {\n    return null_;\n  }\n}\n\ntemplate <typename T>\nconst T& StagingQueue<T>::peek_backstage(size_t index) const {\n  std::lock_guard<std::mutex> lock(mutex_);\n  if (index < num_backstage_) {\n    const size_t begin_backstage = begin_ + num_mainstage_;\n    return at(begin_backstage + index);\n  } else {\n    return null_;\n  }\n}\n\ntemplate <typename T>\nconst T& StagingQueue<T>::latest(size_t index) const {\n  std::lock_guard<std::mutex> lock(mutex_);\n  if (index < num_mainstage_) {\n    return at(begin_ + num_mainstage_ - index - 1);\n  } else {\n    return null_;\n  }\n}\n\ntemplate <typename T>\ntypename StagingQueue<T>::const_iterator_t StagingQueue<T>::begin() const {\n  return const_iterator_t(items_.data(), items_.size(), begin_);\n}\n\ntemplate <typename T>\ntypename StagingQueue<T>::const_iterator_t StagingQueue<T>::end() const {\n  return const_iterator_t(items_.data(), items_.size(), begin_ + num_mainstage_);\n}\n\ntemplate <typename T>\nT StagingQueue<T>::pop() {\n  std::lock_guard<std::mutex> lock(mutex_);\n  T result = null_;\n  if (num_mainstage_ > 0) {\n    std::swap(at(begin_++), result);\n    num_mainstage_--;\n  }\n  begin_ %= items_.size();\n  return result;\n}\n\ntemplate <typename T>\nvoid StagingQueue<T>::popAll() {\n  std::lock_guard<std::mutex> lock(mutex_);\n  const size_t begin_backstage = begin_ + num_mainstage_;\n  while (begin_ < begin_backstage) {\n    at(begin_++) = null_;\n  }\n  begin_ %= items_.size();\n  num_mainstage_ = 0;\n}\n\ntemplate <typename T>\nbool StagingQueue<T>::push(T item) {\n  std::lock_guard<std::mutex> lock(mutex_);\n  const size_t begin_backstage = begin_ + num_mainstage_;\n  if (num_backstage_ == capacity_) {\n    // Trying to add a item to a full backstage.\n    switch (overflow_behavior_) {\n      case OverflowBehavior::kPop: {\n        // Move items in back stage one over. This removes the oldest item and makes room for one\n        // new item.\n        const size_t backstage_end = begin_backstage + num_backstage_;\n        for (size_t i = begin_backstage + 1; i < backstage_end; i++) {\n          at(i - 1) = std::move(at(i));\n        }\n        // Add new item at the end of the backstage.\n        at(backstage_end - 1) = std::move(item);\n      } break;\n      case OverflowBehavior::kReject:\n        // Don't add the new item to the backstage.\n        break;\n      case OverflowBehavior::kFault:\n        return false;\n        // FIXME \"Added an item to a full queue while using the 'Fault' overflow behavior.\"\n      default:\n        // FIXME \"Invalid parameter\"\n        return false;\n    }\n  } else {\n    // Add the new item to the end of the backstage.\n    at(begin_backstage + num_backstage_) = std::move(item);\n    num_backstage_++;\n  }\n  return true;\n}\n\ntemplate <typename T>\nbool StagingQueue<T>::sync() {\n  std::lock_guard<std::mutex> lock(mutex_);\n  // Add back stage items to main stage\n  num_mainstage_ += num_backstage_;\n  num_backstage_ = 0;\n  // Handle overflow if necessary\n  if (num_mainstage_ > capacity_) {\n    switch (overflow_behavior_) {\n      case OverflowBehavior::kPop: {\n        // Make sure that main and back stage together don't exceed capacity. Remove excess items\n        // starting with the first item in the main stage. This effectively pops the oldest items to\n        // make room for new items.\n        const size_t new_begin = begin_ + num_mainstage_ - capacity_;\n        while (begin_ < new_begin) {\n          at(begin_++) = null_;\n        }\n        num_mainstage_ = capacity_;\n      } break;\n      case OverflowBehavior::kReject:\n        // Make sure that main and back stage together don't exceed capacity. Remove excess items\n        // starting with the last item in the back stage. This effectively rejects new items.\n        while (num_mainstage_ > capacity_) {\n          at(begin_ + --num_mainstage_) = null_;\n        }\n        break;\n      case OverflowBehavior::kFault:\n        return false;\n        // FIXME \"Added items to a full queue while using the 'Fault' overflow behavior.\";\n      default:\n        // FIXME \"Invalid parameter\"\n        return false;\n    }\n  }\n  begin_ %= items_.size();\n  return true;\n}\n\n}  // namespace staging_queue\n}  // namespace gxf\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/gems/staging_queue/staging_queue_iterator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_GEMS_STAGING_QUEUE_ITERATOR_HPP\n#define NVIDIA_GXF_STD_GEMS_STAGING_QUEUE_ITERATOR_HPP\n\n#include <iterator>\n\nnamespace gxf {\nnamespace staging_queue {\n\n// A forward iterator for a staging queue.\n//\n// This type can be used to iterate through the items of the main stage of a staging queue. (It\n// could be used to iterate over any part of the ring buffer, but is only used for the main stage.)\n// The iterator correctly \"wraps around\" the ring buffer in case the iterated sequence spans over\n// the end of the ring buffer. This iterator could in theory also be used to loop over the range\n// multiple times by adding multiples of size to the end index, however it is not used in this\n// fashion by the staging queue.\n//\n// Let's take the example of a staging queue which stores four items (capacity = 2) as depicted in\n// the diagram below. An iterator using index=3 would point to the third element. An iterator using\n// index=5 would point to the second element in the sequence. If these two iterators would be used\n// as begin and end of a range iteration would \"wrap around\" and iterate over the items A and B.\n//\n//    0   1   2   3   4\n//    #   #   #   #   #\n//    | B | O | O | A |\n//        |       |\n//        |       # begin\n//        # end\n//\n// Note that the member functions of this type are part of the standard implementation for a\n// forward iterator. Please refer to the C++ standard definition for forward iterators for details.\ntemplate <typename T>\nclass StagingQueueIterator {\n public:\n  using difference_type = std::ptrdiff_t;\n  using value_type = T;\n  using reference = const T&;\n  using pointer = const T*;\n\n  // Creates an empty (invalid) iterator\n  StagingQueueIterator() : data_(nullptr), size_(0), index_(0) {}\n\n  // Creates and iterator based on a sequence of items pointed to by 'data' which stores 'size'\n  // number of items. The given 'index' indicates the position of the element in the underlying\n  // sequence modulo the length of the sequence.\n  StagingQueueIterator(pointer data, size_t size, size_t index)\n      : data_(data), size_(size), index_(index) {}\n\n  StagingQueueIterator(const StagingQueueIterator& other) = default;\n  StagingQueueIterator& operator=(const StagingQueueIterator& other) = default;\n  StagingQueueIterator(StagingQueueIterator&& other) = default;\n  StagingQueueIterator& operator=(StagingQueueIterator&& other) = default;\n\n  ~StagingQueueIterator() = default;\n\n  const StagingQueueIterator& operator++() {\n    ++index_;\n    return *this;\n  }\n  StagingQueueIterator operator++(int) { return StagingQueueIterator(data_, size_, index_++); }\n\n  reference operator*() const { return data_[index_ % size_]; }\n  pointer operator->() const { return data_ + (index_ % size_); }\n\n  bool operator==(const StagingQueueIterator& rhs) const {\n    return data_ == rhs.data_ && size_ == rhs.size_ && index_ == rhs.index_;\n  }\n  bool operator!=(const StagingQueueIterator& rhs) const { return !(*this == rhs); }\n\n private:\n  // A pointer to the sequence of items to which this iterator refers.\n  const pointer data_;\n\n  // The length of the sequence pointer to by 'data_'.\n  const size_t size_;\n\n  // The index of the item in the sequence to which this iterator points. The index is taken modulo\n  // 'size_' when accessing 'data_'.\n  size_t index_;\n};\n\n}  // namespace staging_queue\n}  // namespace gxf\n\nnamespace std {\n\ntemplate <typename T>\nstruct iterator_traits<gxf::staging_queue::StagingQueueIterator<T>> {\n  using difference_type = typename gxf::staging_queue::StagingQueueIterator<T>::difference_type;\n  using value_type = typename gxf::staging_queue::StagingQueueIterator<T>::value_type;\n  using reference = typename gxf::staging_queue::StagingQueueIterator<T>::reference;\n  using pointer = typename gxf::staging_queue::StagingQueueIterator<T>::pointer;\n  using iterator_category = std::forward_iterator_tag;\n};\n\n}  // namespace std\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/gems/suballocators/first_fit_allocator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_GEMS_SUBALLOCATORS_FIRST_FIT_ALLOCATOR_HPP\n#define NVIDIA_GXF_STD_GEMS_SUBALLOCATORS_FIRST_FIT_ALLOCATOR_HPP\n\n#include <cstdint>\n#include <memory>\n#include <new>\n#include <utility>\n\n#include \"common/expected.hpp\"\n#include \"gxf/std/gems/suballocators/first_fit_allocator_base.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Memory management class.\n// It pre-allocates memory for a given type and allow acquiring chunk of memory and releasing them.\n// Only the allocate function is doing some memory allocation, other functions are using constant\n// amount of memory on the stack.\ntemplate<class T>\nclass FirstFitAllocator {\n public:\n  // Expected type used by this class.\n  template <typename E>\n  using expected_t = nvidia::Expected<E, FirstFitAllocatorBase::Error>;\n  // Unexpected type used by this class.\n  using unexpected_t = nvidia::Unexpected<FirstFitAllocatorBase::Error>;\n\n  FirstFitAllocator() = default;\n\n  // Allocates the require memory to handle the query for a chunk of memory of a give size .\n  // It fails if size is invalid (too big or negative).\n  // Note that a minimum chunk size to be considered can be provided. It means the memory allocated\n  // will always be a multiple of this size. This can help speed up the memory allocation, however\n  // it loses control on the exact allocated size.\n  // The total memory allocated will be around sizeof(T) * size + 32 * size / chunk_size.\n  expected_t<int32_t> allocate(const int32_t size, const int chunk_size = 1) {\n    if (buffer_.get() != nullptr) {\n      return unexpected_t(FirstFitAllocatorBase::Error::kAlreadyInUse);\n    }\n    if (size < 0 || chunk_size <= 0) {\n      return unexpected_t(FirstFitAllocatorBase::Error::kInvalidSize);\n    }\n    chunk_size_ = chunk_size;\n    const int32_t number_of_chunks = getNumberOfChunks(size);\n    // Allocate memory\n    buffer_.reset(new(std::nothrow) T[number_of_chunks * chunk_size_]);\n    if (buffer_.get() == nullptr) {\n      return unexpected_t(FirstFitAllocatorBase::Error::kOutOfMemory);\n    }\n    // Prepare the memory management.\n    auto res = memory_management_.allocate(number_of_chunks);\n    if (!res) {\n      return unexpected_t(res.error());\n    }\n    return size;\n  }\n\n  // Attempts to acquire a block of memory of a given size.\n  // If such a contiguous block exists, it will return a pointer to that block and the actual size\n  // acquired (will be the smallest multiple of chunk_size_ that exceed or equal size).\n  // If no block exists, it will return FirstFitAllocatorBase::Error::kOutOfMemory.\n  expected_t<std::pair<T*, int32_t>> acquire(const int32_t size) {\n    const int32_t number_of_chunks = getNumberOfChunks(size);\n    auto res = memory_management_.acquire(number_of_chunks);\n    if (!res) {\n      return unexpected_t(res.error());\n    }\n    return std::make_pair(&buffer_.get()[res.value() * chunk_size_],\n                          number_of_chunks * chunk_size_);\n  }\n\n  // Releases a block of memory that has been acquired with the function above.\n  // If there was no `acquire(size)` that returned this pointer, this query will fail, otherwise it\n  // will free the block of memory for further acquisition.\n  // Note once a block of memory has been released, it can't be released again.\n  expected_t<void> release(const T* ptr) {\n    const int32_t index = std::distance<const T*>(buffer_.get(), ptr);\n    if (index % chunk_size_ != 0) {\n      return unexpected_t(FirstFitAllocatorBase::Error::kInvalidSize);\n    }\n    return memory_management_.release(index / chunk_size_);\n  }\n\n private:\n  // Returns the number of chunks of memory needed to have at least a given size.\n  int32_t getNumberOfChunks(const int32_t size) const {\n    return (size + chunk_size_ - 1) / chunk_size_;\n  }\n\n  // Real memory management.\n  FirstFitAllocatorBase memory_management_;\n  // Size of the chunk of memory. Only multiple of this size can be allocated. Whenever a request is\n  // made, the allocated size will be the smaller multiple of chunk_size_ that is not smaller than\n  // the requested size.\n  int32_t chunk_size_;\n  // Buffer holding the pre-allocated memory that is provided on demand.\n  std::unique_ptr<T[]> buffer_{nullptr};\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_GEMS_SUBALLOCATORS_FIRST_FIT_ALLOCATOR_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/gems/suballocators/first_fit_allocator_base.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_GEMS_SUBALLOCATORS_FIRST_FIT_ALLOCATOR_BASE_HPP\n#define NVIDIA_GXF_STD_GEMS_SUBALLOCATORS_FIRST_FIT_ALLOCATOR_BASE_HPP\n\n#include <cstdint>\n#include <memory>\n#include <new>\n#include <utility>\n\n#include \"common/expected.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Memory management helper class, it keeps track of what part of a big chunk of memory has been\n// allocated and can efficiently find the first available chunk of memory that fits a given size.\n// This class works only with indexes and rely on an external class to hold the memory.\n// Internally it uses binary segment tree to keep track the biggest available block in a given area\n// of the the memory. All the operation should take a logarithmic time.\nclass FirstFitAllocatorBase {\n public:\n  // Error codes used by the classes of this file.\n  enum class Error {\n    // Returned when the suballocators is already in use and some memory has not been released yet.\n    kAlreadyInUse,\n    // Returned if the size is invalid (negative or too big).\n    kInvalidSize,\n    // Returned if the class can't allocate enough memory.\n    kOutOfMemory,\n    // Returned if we attempt to release a block of memory not allocated yet.\n    kBlockNotAllocated,\n    // This error happens when there is logically issue during the execution. This should never\n    // happen.\n    kLogicError,\n  };\n  // Expected type used by this class.\n  template <typename T>\n  using expected_t = nvidia::Expected<T, Error>;\n  // Unexpected type used by this class.\n  using unexpected_t = nvidia::Unexpected<Error>;\n\n  FirstFitAllocatorBase();\n  // Allocates the require memory to handle the query for a chunk of memory of a give size .\n  // It requires 32 * 2^ceil(log2(size)) Bytes of memory.\n  // It fails if size is invalid (too big or negative).\n  expected_t<void> allocate(int32_t size);\n  // Attempts to acquire a block of memory of a given size.\n  // If such a contiguous block exists, it will return the lowest index where such a block exist.\n  // This block will be blocked until a call to release(index, size) is made.\n  // If no block exists, it will return Error::kOutOfMemory.\n  expected_t<int32_t> acquire(int32_t size);\n  // Releases a block of memory that has been acquired with the function above.\n  // If there was no `acquire(size)` that returned `index`, this query will fail, otherwise it will\n  // free the block of memory for further acquisition.\n  // Note once a block of memory has been released, it can't be released again.\n  expected_t<void> release(int32_t index);\n\n private:\n  // Helper data structure to efficiently find a block of memory of a given size.\n  // It represents a node in a binary segment tree.\n  struct Memory {\n    // Updates the node using both children.\n    // size = l.size + r.size\n    // left = l.left or l.size + r.left iff l.max == l.size\n    // right = r.right or r.right + l.right iff r.max == r.size\n    // max = max(l.max, r.max, l.right + r.left)\n    void update(const Memory& left_child, const Memory& right_child);\n    // Marks a node as being free (if == 1) or acquired (if == 0).\n    void set(int32_t free);\n\n    // Left and right store the size of available memory which start respectively from the left or\n    // right side of the subtree.\n    int32_t left, right;\n    // Max is the size of biggest available block in the subtree.\n    // When a block is starting at the given index, the leaf will contain -1.\n    int32_t max;\n    // Size is constant overtime and represent the size of the subtree.\n    // When a block has been acquired, size will contain the size that has been acquired from the\n    // given position.\n    int32_t size;\n  };\n\n  // Updates a path from a given node to the root.\n  void propagateToRoot(int32_t idx);\n  // Updates a segment of the tree [left, right[ and marks it as free or acquired.\n  void update(int32_t left, int32_t right, int32_t free);\n\n  // Hold the segment tree memory.\n  std::unique_ptr<Memory[]> tree_;\n  // The total size available at the beginning.\n  int32_t size_;\n  // Helper index that helps pointing toward the leaves of the tree.\n  int32_t last_layer_first_index_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_GEMS_SUBALLOCATORS_FIRST_FIT_ALLOCATOR_BASE_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/gems/timed_job_list/timed_job_list.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_GEMS_TIMED_JOB_LIST_TIMED_JOB_LIST_HPP_\n#define NVIDIA_GXF_STD_GEMS_TIMED_JOB_LIST_TIMED_JOB_LIST_HPP_\n\n#include <algorithm>\n#include <condition_variable>\n#include <list>\n#include <mutex>\n#include <queue>\n#include <set>\n#include <string>\n#include <unordered_map>\n#include <unordered_set>\n#include <utility>\n#include <vector>\n\n#include \"gxf/core/expected.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// A thread-safe queue which sorts by target execution time. It provides a blocking pop to get\n// jobs precisely when the time is right.\n//  T: The type of jobs to store in the queue\n//  Eq: (T,T) -> bool: true if jobs are identical\ntemplate <typename JobT>\nclass TimedJobList {\n public:\n  using Clock_t = std::function<int64_t()>;\n\n  TimedJobList(Clock_t clock) : clock_(clock), is_running_(false) {}\n  // Number of queued jobs. This should only be used for logging purposes as it is not acquiring\n  // a lock.\n  size_t sizeUnsafe() const { return queue_.size(); }\n\n  // thread-safe implementation to find number of queued jobs\n  size_t size() const {\n    std::unique_lock<std::mutex> lock(queue_cv_mutex_);\n    return items_.size();\n  }\n\n  // Adds a job to the queue to be executed at the given target time. O(log(n))\n  bool insert(JobT job, int64_t target_time, int64_t slack, int priority);\n  // Notify that a job has completed.\n  void notifyDone(JobT job);\n  // This is a blocking call which will wait for an job.\n  void waitForJob(JobT&);\n  // Removes a job from the queue if present. Returns true if the job was found and removed.\n  bool remove(JobT job);\n  // Sets the job list to a running state\n  void start() { is_running_.store(true); }\n  // Sets the job list to a stopped state and wakes all waiting threads to flush.\n  void stop() {\n    // Acquire the lock to ensure a thread isn't trying to acquire a job\n    // while invoking stop.\n    std::unique_lock<std::mutex> lock(queue_cv_mutex_);\n    // Stop the queue\n    is_running_.store(false);\n    // Wake all waiting threads\n    queue_cv_.notify_all();\n  }\n  // Gets the target time of the next job; or returns an error if there is no job\n  Expected<int64_t> getNextTargetTime() const {\n    std::unique_lock<std::mutex> lock(queue_cv_mutex_);\n    if (queue_.empty()) {\n      return Unexpected{GXF_QUERY_NOT_FOUND};\n    } else {\n      return queue_.top().target_time;\n    }\n  }\n  // wakes up one waiting threads\n  void wakeOne() { queue_cv_.notify_one(); }\n  // wakes all waiting threads\n  void wakeAll() { queue_cv_.notify_all(); }\n\n  // Pops the next pending or waiting job regardless of their\n  // target execution time\n  Expected<JobT> popFront() {\n    std::unique_lock<std::mutex> lock(queue_cv_mutex_);\n\n    // return pending jobs first\n    for (auto pending_job = pending_.begin(); pending_job != pending_.end(); ++pending_job) {\n      Item result = *pending_job;\n      pending_.erase(pending_job);\n      return result.job;\n    }\n\n    // return waiting jobs next\n    if (queue_.empty()) { return Unexpected{GXF_FAILURE}; }\n\n    Item result = queue_.top();\n    queue_.pop();\n    items_.erase(result.job);\n    return result.job;\n  }\n\n  // Checks if there any pending or waiting jobs\n  bool empty() const {\n    std::unique_lock<std::mutex> lock(queue_cv_mutex_);\n    return queue_.empty() && pending_.empty();\n  }\n\n private:\n  // We allow a small tolerance when executing jobs\n// static constexpr int64_t kTimeFudge = 100'000;  // 0.1 ms\n  static constexpr int64_t kTimeFudge = 1;  // 100 ns\n\n  // Helper class used to store jobs with additional information\n  struct Item {\n    // The job to execute\n    JobT job;\n    // The target time at which the job should be executed\n    int64_t target_time;\n    // The amount of time the target time can slip by\n    int64_t slack;\n    // The priority is used as a tie breaker when two jobs would be scheduled close together.\n    int priority;\n  };\n\n  // Sorts jobs such that the earliest non-running job has highest priority\n  struct ItemPriorityCmp {\n    bool operator()(const Item& a, const Item& b) {\n      // If two jobs with their slack times are scheduled within a small\n      // interval of each other check their priorities,\n      // Otherwise earliest deadline goes first.\n      const int64_t a_time = a.target_time + a.slack;\n      const int64_t b_time = b.target_time + b.slack;\n\n      if (std::abs(a_time - b_time) < kTimeFudge && a.priority != b.priority) {\n        return a.priority < b.priority;\n      } else {\n        return a_time > b_time;\n      }\n    }\n  };\n\n  // This function pointer should be set to utilize the desired clock for the queue.\n  Clock_t clock_;\n  // state variable to control if threads should block on waiting for jobs\n  std::atomic<bool> is_running_;\n\n  mutable std::mutex queue_cv_mutex_;\n  std::condition_variable queue_cv_;\n\n  // List of future events sorted by their scheduled execution time.\n  std::priority_queue<Item, std::vector<Item>, ItemPriorityCmp> queue_;\n\n  // Item lookup to avoid duplicates\n  std::unordered_set<JobT> items_;\n\n  // List of events overdue, we have passed their scheduled execution time but have not been able to\n  // run yet.\n  std::list<Item> pending_;\n};\n\n// -------------------------------------------------------------------------------------------------\n\ntemplate <typename JobT>\nbool TimedJobList<JobT>::insert(JobT job, int64_t target_time, int64_t slack, int priority) {\n  {\n    std::unique_lock<std::mutex> lock(queue_cv_mutex_);\n    if (items_.find(job) != items_.end()) { return false; }\n    items_.insert(job);\n    queue_.push(Item{std::move(job), target_time, slack, priority});\n  }\n  queue_cv_.notify_one();\n  return true;\n}\n\ntemplate <typename JobT>\nvoid TimedJobList<JobT>::notifyDone(JobT job) {\n  std::unique_lock<std::mutex> lock(queue_cv_mutex_);\n  wakeOne();\n}\n\n// Removes a job from the pending list or priority queue and from the items_ tracking set.\n// Since std::priority_queue doesn't support direct element removal, this method rebuilds\n// the queue excluding the target job when necessary. Returns true if the job was found\n// and successfully removed, false if the job wasn't present in any container.\ntemplate <typename JobT>\nbool TimedJobList<JobT>::remove(JobT job) {\n  std::unique_lock<std::mutex> lock(queue_cv_mutex_);\n\n  // Check if the job exists in our tracking set\n  auto it = items_.find(job);\n  if (it == items_.end()) {\n    return false;  // Job not found\n  }\n\n  // Remove from items_\n  items_.erase(it);\n\n  // Check if job is in pending list and remove if found\n  for (auto pending_it = pending_.begin(); pending_it != pending_.end(); ++pending_it) {\n    if (pending_it->job == job) {\n      pending_.erase(pending_it);\n      return true;\n    }\n  }\n\n  // If not in pending, it might be in the queue\n  // Since std::priority_queue doesn't allow direct removal, we need to:\n  // 1. Create a new queue\n  // 2. Pop all elements from the old queue\n  // 3. Push all elements except the one we want to remove into the new queue\n  // 4. Swap the queues\n\n  std::priority_queue<Item, std::vector<Item>, ItemPriorityCmp> new_queue;\n  bool found = false;\n\n  while (!queue_.empty()) {\n    Item item = queue_.top();\n    queue_.pop();\n\n    if (item.job != job) {\n      // Keep this item\n      new_queue.push(item);\n    } else {\n      // Found the job to remove\n      found = true;\n    }\n  }\n\n  // Replace the old queue with the new one\n  queue_.swap(new_queue);\n\n  // Wake waiting threads to recalculate wait times\n  wakeOne();\n\n  return found;\n}\n\ntemplate <typename JobT>\nvoid TimedJobList<JobT>::waitForJob(JobT& job) {\n  while (is_running_) {\n    // Acquire the lock to check for a job\n    std::unique_lock<std::mutex> lock(queue_cv_mutex_);\n    // Double check the running status as a stop could have been invoked.\n    if (!is_running_) {\n      return;\n    }\n    int64_t wait_duration = 0;\n    const int64_t now = clock_();\n    while (!queue_.empty()) {\n      // We have a job, check if it is time for it\n      Item top_item = queue_.top();\n      const int64_t eta = top_item.target_time - now;\n      // If the jobs target time is within our fudge factor fire now.\n      if (eta <= kTimeFudge) {\n        pending_.push_back(top_item);\n        queue_.pop();\n      } else {\n        // wait till next eta minus a small window\n        wait_duration = std::max(eta - kTimeFudge, static_cast<int64_t>(0));\n        break;\n      }\n    }\n    // Loop through the list of pending jobs to find one that can be run.\n    for (auto pending_job = pending_.begin(); pending_job != pending_.end(); ++pending_job) {\n      job = pending_job->job;\n      pending_job->target_time = now;\n      pending_.erase(pending_job);\n      items_.erase(job);\n      return;\n    }\n    // Wait for a new job or until time is up (the lock is released while waiting)\n    if (wait_duration > 0) {\n      queue_cv_.wait_for(lock, std::chrono::nanoseconds(wait_duration));\n    } else {\n      queue_cv_.wait(lock);\n    }\n  }\n}\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_GEMS_TIMED_JOB_LIST_TIMED_JOB_LIST_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/gems/utils/time.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_GXF_STD_GEMS_UTILS_TIME_HPP_\n#define NVIDIA_GXF_GXF_STD_GEMS_UTILS_TIME_HPP_\n\n#include <chrono>\n#include <string>\n#include \"gxf/core/expected.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/// @brief Converts time in seconds to a timestamp in nanoseconds\nint64_t TimeToTimestamp(double time);\n\n/// @brief Converts a timestamp Nanoseconds to seconds\ndouble TimestampToTime(int64_t timestamp);\n\n/// @brief Parses given text to return the desired period in nanoseconds.\n///\n/// @param text Text containing number and time-units, to be parsed for desired period\n/// @param cid cid of component for which text is being parsed\n/// @return Period in nanoseconds if successful, or otherwise one of the GXF error codes.\nExpected<int64_t> ParseRecessPeriodString(std::string text, const gxf_uid_t& cid);\n\ninline uint64_t getCurrentTimeUs() {\n  using std::chrono::duration_cast;\n  using std::chrono::system_clock;\n  return duration_cast<std::chrono::microseconds>(system_clock::now().time_since_epoch()).count();\n}\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GXF_STD_GEMS_UTILS_TIME_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/gems/video_buffer/allocator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_GEMS_VIDEO_BUFFER_HPP\n#define NVIDIA_GXF_STD_GEMS_VIDEO_BUFFER_HPP\n\n#include <array>\n#include <vector>\n#include \"gxf/multimedia/video.hpp\"\n#include \"gxf/std/allocator.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\ntemplate <gxf::VideoFormat T>\nstruct NoPaddingColorPlanes {};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB> {\n  explicit NoPaddingColorPlanes(uint32_t width) : planes({gxf::ColorPlane(\"RGB\", 3, width * 3)}) {}\n  std::array<gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"BGR\", 3, width * 3)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_RGBA> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"RGBA\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_BGRA> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"BGRA\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB16> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"RGB\", 6, width * 6)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR16> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"BGR\", 6, width * 6)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB32> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"RGB\", 12, width * 12)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR32> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"BGR\", 12, width * 12)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"gray\", 1, width)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY16> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"gray\", 2, width * 2)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"gray\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32F> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"gray\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n                nvidia::gxf::ColorPlane(\"UV\", 2, width)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12_ER> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n                nvidia::gxf::ColorPlane(\"UV\", 2, width)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n                nvidia::gxf::ColorPlane(\"UV\", 2, width * 2)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24_ER> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n                nvidia::gxf::ColorPlane(\"UV\", 2, width * 2)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes({gxf::ColorPlane(\"R\", 1, width), gxf::ColorPlane(\"G\", 1, width),\n    gxf::ColorPlane(\"B\", 1, width)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_R16_G16_B16> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes({gxf::ColorPlane(\"R\", 2, width * 2), gxf::ColorPlane(\"G\", 2, width * 2),\n    gxf::ColorPlane(\"B\", 2, width * 2)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes(\n        {gxf::ColorPlane(\"R\", 4, width * 4), gxf::ColorPlane(\"G\", 4, width * 4),\n        gxf::ColorPlane(\"B\", 4, width * 4)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_B8_G8_R8> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes({gxf::ColorPlane(\"B\", 1, width), gxf::ColorPlane(\"G\", 1, width),\n    gxf::ColorPlane(\"R\", 1, width)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_B16_G16_R16> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes(\n        {gxf::ColorPlane(\"R\", 2, width * 2), gxf::ColorPlane(\"G\", 2, width * 2),\n        gxf::ColorPlane(\"B\", 2, width * 2)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_B32_G32_R32> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes(\n        {gxf::ColorPlane(\"B\", 4, width * 4), gxf::ColorPlane(\"G\", 4, width * 4),\n        gxf::ColorPlane(\"R\", 4, width * 4)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\n// This includes the list of video buffer formats that supported for the allocator\nconstexpr bool IsSupportedVideoFormat(const gxf::VideoFormat format) {\n  return format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGBA ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGRA ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB16 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR16 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB32 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR32 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY16 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32F ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_B8_G8_R8 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_R16_G16_B16 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_B16_G16_R16 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_B32_G32_R32 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12_ER ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24_ER;\n}\ntemplate<gxf::VideoFormat T, typename std::enable_if<!IsSupportedVideoFormat(T)>::type* = nullptr>\ngxf::Expected<void> AllocateUnpaddedVideoBuffer(\n    gxf::Handle<gxf::VideoBuffer> frame, uint32_t width, uint32_t height,\n    gxf::MemoryStorageType storage_type,\n    gxf::Handle<gxf::Allocator> allocator) {\n  GXF_LOG_ERROR(\"Received unsupported video format!\");\n  return gxf::Unexpected{GXF_FAILURE};\n}\ntemplate<gxf::VideoFormat T, typename std::enable_if<IsSupportedVideoFormat(T)>::type* = nullptr>\ngxf::Expected<void> AllocateUnpaddedVideoBuffer(\n    gxf::Handle<gxf::VideoBuffer> frame, uint32_t width, uint32_t height,\n    gxf::MemoryStorageType storage_type, gxf::Handle<gxf::Allocator> allocator) {\n  if (width % 2 != 0 || height % 2 != 0) {\n    GXF_LOG_ERROR(\n        \"Error: expected even width and height but received %u width and %u height\",\n        width, height);\n    return gxf::Unexpected{GXF_FAILURE};\n  }\n  NoPaddingColorPlanes<T> nopadding_planes(width);\n  gxf::VideoFormatSize<T> video_format_size;\n  auto size = video_format_size.size(width, height, nopadding_planes.planes);\n  std::vector<gxf::ColorPlane> color_planes{\n      nopadding_planes.planes.begin(), nopadding_planes.planes.end()};\n  gxf::VideoBufferInfo buffer_info{width, height, T, color_planes,\n      gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR};\n  return frame->resizeCustom(buffer_info, size, storage_type, allocator);\n}\ntemplate<gxf::VideoFormat T>\ngxf::Expected<void> AllocateVideoBuffer(\n    gxf::Handle<gxf::VideoBuffer> frame, uint32_t width, uint32_t height,\n    gxf::SurfaceLayout layout, gxf::MemoryStorageType storage_type,\n    gxf::Handle<gxf::Allocator> allocator) {\n    return frame->resize<T>(width, height, layout, storage_type, allocator);\n}\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/graph_driver.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_GRAPH_DRIVER_HPP_\n#define NVIDIA_GXF_GRAPH_DRIVER_HPP_\n\n#include <memory>\n#include <set>\n#include <string>\n#include <thread>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"gxf/std/gems/queue_thread/queue_thread.hpp\"\n#include \"gxf/std/graph_driver_worker_common.hpp\"\n#include \"gxf/std/ipc_client.hpp\"\n#include \"gxf/std/ipc_server.hpp\"\n#include \"gxf/std/system.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * ECS layer Component, resolving segments connection addresses\n * Works with GraphWorker as server-client pair\n *\n * Has event-based thread, IPC server, and IPC client\n *\n * Segments connection graph:\n * 1. Users to provide, via config or add connection API\n *\n * Resolve address of each segment pair:\n * 1. Listen to remote GraphWorkers that each runs one or more segments.\n * 2. Register all GraphWorkers until all segments in static graph are discovered\n * 3. Resolve connection address between each segment pair\n * 4. Send result address to each target segment via GraphWorker that manages the segment\n*/\nclass GraphDriver : public System {\n public:\n  using ConnectionMap = std::unordered_map<std::string, std::string>;\n  using GxfSystemThread = QueueThread<std::string>;\n  GraphDriver() = default;\n\n  gxf_result_t schedule_abi(gxf_uid_t eid) override;\n  gxf_result_t unschedule_abi(gxf_uid_t eid) override;\n  gxf_result_t runAsync_abi() override;\n  gxf_result_t stop_abi() override;\n  gxf_result_t wait_abi() override;\n  gxf_result_t event_notify_abi(gxf_uid_t eid, gxf_event_t event) override;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n\n  // C++ API exclusive\n  Expected<void> addSegmentConnection(const std::string& source, const std::string& target);\n\n private:\n  /**\n   * Member thread event-based entrance\n  */\n  bool asyncRunnerCallback(std::string event, GraphDriver* self);\n\n  /**\n   * Service to register a remote GraphWorker\n  */\n  Expected<void> onRegisterGraphWorker(const std::string& resource, const std::string& payload);\n\n  /**\n   * Service to record completion of each GraphWorker\n   * then finally gently tear down this GraphDriver\n  */\n  Expected<void> onGraphWorkerComplete(const std::string& resource, const std::string& payload);\n\n  /**\n   * After discover all segments from remote GraphWorkers, resolve segments connection\n   * then send remote request to update connection addresses\n  */\n  Expected<void> resolveConnections();\n\n  /**\n   * After update segments connection in each segment, they're ready to execute\n   * send remote request to all GraphWorkers to run the segments\n  */\n  Expected<void> executeWorkers();\n\n  /**\n   * Send remote request to all GraphWorkers to deactivate the segments\n  */\n  Expected<void> deactivateWorkers();\n\n  /**\n   * Send remote request to all GraphWorkers to stop the segments\n  */\n  Expected<void> stopWorkers();\n\n private:\n  // Communication layer\n  Parameter<Handle<IPCServer>> server_;\n  Parameter<Handle<IPCClient>> client_;\n  Parameter<std::vector<ConnectionMap>> list_of_connections_;\n  ConnectionMap connections_;\n  ConnectionMap reverse_connections_;\n  std::set<std::string> segment_names_;\n  std::unordered_map<std::string, std::vector<std::string>> workers_;\n  std::set<std::string> completed_workers_;\n  std::unique_ptr<GxfSystemThread> driver_thread_;\n  std::set<std::string> requested_segment_names_;\n  std::unordered_map<std::string, std::string> segment_ip_address_;\n\n private:\n  struct Event {\n    static constexpr const char* kNotInitialized = \"kNotInitialized\";\n    static constexpr const char* kResolveConnections = \"kResolveConnections\";\n    static constexpr const char* kExecuteWorkers = \"kExecuteWorkers\";\n    static constexpr const char* kDeactivateWorkers = \"kDeactivateWorkers\";\n    static constexpr const char* kStopWorkers = \"kStopWorkers\";\n  };\n};\n\n/**\n * Utils\n*/\ntemplate <>\nstruct ParameterParser<GraphDriver::ConnectionMap> {\n  static Expected<GraphDriver::ConnectionMap> Parse(\n    gxf_context_t context, gxf_uid_t component_uid, const char* key,\n    const YAML::Node& node, const std::string& prefix) {\n      if (!node.IsMap()) {\n        return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n      }\n      GraphDriver::ConnectionMap connections;\n      for (const auto& p : node) {\n        const std::string k = p.first.as<std::string>();\n        const auto maybe = ParameterParser<std::string>::Parse(\n          context, component_uid, k.c_str(), node[k], prefix);\n        if (!maybe) {\n          return ForwardError(maybe);\n        }\n        connections[k] = maybe.value();\n      }\n      return connections;\n    }\n};\n\n\ntemplate <>\nstruct ParameterWrapper<GraphDriver::ConnectionMap> {\n  // Wrap the value to a YAML::Node instance\n  static Expected<YAML::Node> Wrap(\n    gxf_context_t context,\n    const GraphDriver::ConnectionMap& value) {\n    YAML::Node node(YAML::NodeType::Map);\n    for (auto &i : value) {\n      auto maybe = ParameterWrapper<std::string>::Wrap(context, i.second);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node[i.first] = maybe.value();\n    }\n    return node;\n  }\n};\n\ntemplate<>\nstruct ParameterWrapper<std::vector<GraphDriver::ConnectionMap>> {\n  static Expected<YAML::Node> Wrap(\n    gxf_context_t context,\n    const std::vector<GraphDriver::ConnectionMap>& value) {\n      YAML::Node node(YAML::NodeType::Sequence);\n      for (auto &h : value) {\n        auto maybe = ParameterWrapper<GraphDriver::ConnectionMap>::Wrap(context, h);\n        if (!maybe) {\n          return Unexpected{maybe.error()};\n        }\n        node.push_back(maybe.value());\n      }\n    return node;\n  }\n};\n\ntemplate <>\nstruct ParameterParser<std::vector<GraphDriver::ConnectionMap>> {\n  static Expected<std::vector<GraphDriver::ConnectionMap>> Parse(\n                                        gxf_context_t context, gxf_uid_t component_uid,\n                                        const char* key, const YAML::Node& node,\n                                        const std::string& prefix) {\n    if (!node.IsSequence()) {\n      const char* component_name = \"UNKNOWN\";\n      GxfParameterGetStr(context, component_uid, kInternalNameParameterKey, &component_name);\n      GXF_LOG_ERROR(\"Parameter '%s' in component '%s' must be a vector\", key, component_name);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    std::vector<GraphDriver::ConnectionMap> result(node.size());\n    for (size_t i = 0; i < node.size(); i++) {\n      const auto maybe = ParameterParser<GraphDriver::ConnectionMap>::Parse(\n        context, component_uid, key, node[i], prefix);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      result[i] = std::move(maybe.value());\n    }\n    return result;\n  }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GRAPH_DRIVER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/graph_driver_worker_common.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_GRAPH_DRIVER_WORKER_COMMON_HPP_\n#define NVIDIA_GXF_GRAPH_DRIVER_WORKER_COMMON_HPP_\n\n#include <atomic>\n#include <condition_variable>\n#include <map>\n#include <memory>\n#include <mutex>\n#include <sstream>\n#include <string>\n#include <thread>\n#include <vector>\n\n#include \"common/logger.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\nvoid parseIpAddress(const std::string& ip_address_port, std::string& ip_address, int& port);\n\nvoid parseSegmentEntityComponentName(\n  const std::string& segment_entity_component_name,\n  std::string& segment_name,\n  std::string& entity_name,\n  std::string& component_name);\n\n//\n// JSON schema parsing\n//\nstruct ComponentParam {\n  struct ParamInfo {\n    std::string key;\n    std::string value;\n    std::string value_type;\n    static Expected<bool> strToBool(const std::string& str);\n    static Expected<int32_t> strToInt32(const std::string& str);\n    static Expected<uint32_t> strToUInt32(const std::string& str);\n    static Expected<int64_t> strToInt64(const std::string& str);\n    static Expected<uint64_t> strToUInt64(const std::string& str);\n    static Expected<float> strToFloat32(const std::string& str);\n    static Expected<double> strToFloat64(const std::string& str);\n    static Expected<uint16_t> strToUInt16(const std::string& str);\n  };\n  std::string segment_name;\n  std::string entity_name;\n  std::string component_name;\n  std::vector<ParamInfo> params;\n  // Helpers\n  std::string serialize() const {\n    return segment_name + \".\" + entity_name + \".\" + component_name;\n  }\n};\n\nstruct ComponentInfo {\n  std::string segment_name;\n  std::string entity_name;\n  std::string component_name;\n  // Helpers\n  std::string serialize() const {\n    return segment_name + \".\" + entity_name + \".\" + component_name;\n  }\n  static ComponentInfo deserialize(const std::string& data) {\n    std::istringstream stream(data);\n    std::string segment;\n    std::string entity;\n    std::string component;\n    std::getline(stream, segment, '.');\n    std::getline(stream, entity, '.');\n    std::getline(stream, component, '.');\n    // Validate that all parts are present.\n    if (segment.empty() || entity.empty() || component.empty()) {\n      GXF_LOG_ERROR(\"Invalid data %s\", data.c_str());\n    }\n\n    return {segment, entity, component};\n  }\n};\n\nstruct SegmentInfo {\n  std::string segment_name;\n  // key: \"Segment_name/Entity_name/UcxRx_name\"\n  // value: \"10.0.0.1:3000\"\n  std::map<std::string, std::string> ip_port_address_map;\n};\n\nstruct WorkerInfo {\n  std::string server_ip_address;\n  std::string server_port;\n  std::vector<SegmentInfo> segment_info_list;\n  std::string ip_port() const {\n    return server_ip_address + \":\" + server_port;\n  }\n};\n\nclass GraphDriverWorkerParser {\n public:\n  GraphDriverWorkerParser() = default;\n\n  static Expected<std::vector<ComponentParam>>\n  deserialize_onSetComponentParams(const std::string& payload);\n\n  static Expected<std::string>\n  serialize_onSetComponentParams(const std::vector<ComponentParam>& component_param_list);\n\n  static Expected<WorkerInfo>\n  deserialize_onRegisterGraphWorker(const std::string& payload);\n\n  static Expected<std::string>\n  serialize_onRegisterGraphWorker(const WorkerInfo& worker_info);\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GRAPH_DRIVER_WORKER_COMMON_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/graph_worker.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_GRAPH_WORKER_HPP_\n#define NVIDIA_GXF_GRAPH_WORKER_HPP_\n\n#include <atomic>\n#include <map>\n#include <memory>\n#include <string>\n#include <thread>\n\n#include \"gxf/std/gems/queue_thread/queue_thread.hpp\"\n#include \"gxf/std/graph_driver_worker_common.hpp\"\n#include \"gxf/std/ipc_client.hpp\"\n#include \"gxf/std/ipc_server.hpp\"\n#include \"gxf/std/system.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// copy tid from ucx extension\nstatic const gxf_tid_t ucx_transmitter_tid{0x58165d0378b74696, 0xb20071621f90aee7};\n\nstruct GraphSpec {\n  std::string app_path;\n  std::string param_path;\n  std::string manifest_path;\n  uint32_t severity = 3;  // default severity\n};\n\n/**\n * SegmentRunner, context to run each graph segment\n*/\nclass SegmentRunner {\n public:\n  using GxfSystemThread = QueueThread<std::string>;\n  // YAML API compose graph\n  SegmentRunner(const std::string& name, const GraphSpec& graph_spec)\n    : name_(name), graph_spec_(graph_spec) {\n    runner_thread_ = std::make_unique<GxfSystemThread>(\n    std::bind(&SegmentRunner::asyncRunnerCallback, this, std::placeholders::_1, this),\n    name);\n  }\n  SegmentRunner(const std::string& name, const GraphSpec& graph_spec,\n    std::shared_ptr<GxfSystemThread> worker_thread)\n    : SegmentRunner(name, graph_spec) {  // delegating to the first constructor\n    worker_thread_ = worker_thread;\n  }\n  // C++ API compose graph\n  SegmentRunner(const std::string& name, const gxf_context_t context)\n    : name_(name), context_(context) {\n    runner_thread_ = std::make_unique<GxfSystemThread>(\n    std::bind(&SegmentRunner::asyncRunnerCallback, this, std::placeholders::_1, this),\n    name);\n  }\n  SegmentRunner(const std::string& name, const gxf_context_t context,\n    std::shared_ptr<GxfSystemThread> worker_thread)\n    : SegmentRunner(name, context) {\n    worker_thread_ = worker_thread;\n  }\n  // ~SegmentRunner();\n  void stop();\n  void wait();\n  // combined/simplified gxf run sequence\n  std::future<bool> asyncInitializeGxfGraph();\n  std::future<bool> asyncActivateGxfGraph();\n  std::future<bool> asyncRunGxfGraph();\n  std::future<bool> runGxfGraph();\n  std::future<bool> asyncDeactivateGxfGraph();\n  std::future<bool> asyncDestroyGxfGraph();\n  gxf_result_t setParameter(const std::string& entity_name, const std::string& comp_name,\n    const std::string& key, const std::string& value, const std::string& value_type);\n  Expected<SegmentInfo> createSegmentInfo(const std::string& worker_host_ip);\n\n private:\n  std::unique_ptr<GxfSystemThread> runner_thread_;\n  std::shared_ptr<GxfSystemThread> worker_thread_;\n  bool asyncRunnerCallback(std::string event, SegmentRunner* self);\n  struct Event {\n    // GXF C API sequence event\n    static constexpr const char* kCreateGxfContext = \"kCreateContext\";\n    static constexpr const char* kLoadGxfManifest = \"kLoadGxfManifest\";\n    static constexpr const char* kLoadGxfGraph = \"kLoadGxfGraph\";\n    static constexpr const char* kActivateGxfGraph = \"kActivateGxfGraph\";\n    static constexpr const char* kNonBlockingRunGxfGraph = \"kNonBlockingRunGxfGraph\";\n    static constexpr const char* kBlockingRunGxfGraph = \"kBlockingRunGxfGraph\";\n    static constexpr const char* kInterruptGxfGraph = \"kInterruptGxfGraph\";\n    static constexpr const char* kDeactivateGxfGraph = \"kDeactivateGxfGraph\";\n    static constexpr const char* kDestroyGxfGraph = \"kDestroyGxfGraph\";\n  };\n  const std::string name_;\n  const GraphSpec graph_spec_;\n  gxf_context_t context_ = kNullContext;\n  gxf_context_t s_signal_context_ = kNullContext;\n  std::mutex context_mutex_;\n\n private:\n  // gxf wrappers, non thread safe to gxf_context\n  gxf_result_t wrapCreateGxfConext();\n  gxf_result_t wrapLoadGxfManifest();\n  gxf_result_t wrapLoadGxfGraph();\n  gxf_result_t wrapActivateGxfGraph();\n  gxf_result_t wrapBlockingRunGxfGraph();\n  gxf_result_t wrapNonBlockingRunGxfGraph();\n  gxf_result_t wrapInterruptGxfGraph();\n  gxf_result_t wrapDeactivateGxfGraph();\n  gxf_result_t wrapDestroyGxfGraph();\n};\n\n\n/**\n * ECS layer Component, manages executing a set of segments\n * Works with GraphDriver as server-client pair\n *\n * Has event-based thread, IPC server, and IPC client.\n *\n * Segments to manage:\n * 1. Users to provide, via config or add segment context API\n*/\nclass GraphWorker : public System {\n public:\n  using GxfSystemThread = QueueThread<std::string>;\n  GraphWorker() = default;\n\n  gxf_result_t schedule_abi(gxf_uid_t eid) override;\n  gxf_result_t unschedule_abi(gxf_uid_t eid) override;\n  gxf_result_t runAsync_abi() override;\n  gxf_result_t stop_abi() override;\n  gxf_result_t wait_abi() override;\n  gxf_result_t event_notify_abi(gxf_uid_t eid, gxf_event_t event) override;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n\n  // C++ API exclusive\n  Expected<void> addSegment(const std::string& name, const gxf_context_t context);\n\n private:\n  // This ECS component layer\n  /**\n   * Member thread event-based entrance\n  */\n  bool asyncRunnerCallback(std::string event, GraphWorker* self);\n\n  Parameter<std::map<std::string, GraphSpec>> graph_specs_;\n  Parameter<int64_t> driver_reconnection_times_;\n  std::map<std::string, std::unique_ptr<SegmentRunner>> segment_runners_;\n  std::shared_ptr<GxfSystemThread> worker_thread_;\n  size_t completed_segment_runner_nums_;\n  struct Event {\n    static constexpr const char* kInstantiateSegmentRunner = \"kInstantiateSegmentRunner\";\n    static constexpr const char* kRegisterWorker = \"kRegisterWorker\";\n    static constexpr const char* kCheckWorkComplete = \"kCheckWorkComplete\";\n  };\n\n  // Communication layer\n  Parameter<Handle<gxf::IPCServer>> server_;\n  Parameter<Handle<gxf::IPCClient>> client_;\n\n  // Service layer\n  Parameter<std::string> initialize_segments_uri_;\n  Parameter<std::string> set_component_params_uri_;\n  Parameter<std::string> activate_segments_uri_;\n  Parameter<std::string> run_segments_uri_;\n  Parameter<std::string> deactivate_segments_uri_;\n  Parameter<std::string> destroy_segments_uri_;\n  Parameter<std::string> stop_worker_uri_;\n\n private:\n  // Services to handle request from the GraphDriver\n  Expected<void> onInitializeSegments(const std::string& resource, const std::string& payload);\n  Expected<void> onSetComponentParams(const std::string& resource, const std::string& payload);\n  Expected<void> onActivateSegments(const std::string& resource, const std::string& payload);\n  Expected<void> onRunSegments(const std::string& resource, const std::string& payload);\n  Expected<void> onDeactivateSegments(const std::string& resource, const std::string& payload);\n  Expected<void> onDestroySegments(const std::string& resource, const std::string& payload);\n  Expected<void> onStopWorker(const std::string& resource, const std::string& payload);\n\n  Expected<void> instantiateSegmentRunners();\n  Expected<void> registerGraphWorker();\n  Expected<void> checkComplete();\n  std::unique_ptr<WorkerInfo> worker_info_;\n\n  gxf_result_t stop_all_segments();\n\n  // helper methods\n  std::string getPrimaryIp();\n  Expected<std::string> createWorkerInfo();\n\n  friend class SegmentRunner;\n};\n\n//\n// utils for Parameter<std::map<std::string, GraphSpec>>\n//\ntemplate <>\nstruct ParameterParser<GraphSpec> {\n  static Expected<GraphSpec> Parse(\n    gxf_context_t context, gxf_uid_t component_uid, const char* key,\n    const YAML::Node& node, const std::string& prefix) {\n      if (!node.IsMap() || node.size() < 2) {\n        return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n      }\n      GraphSpec graph_spec;\n      // mandatory fields\n      graph_spec.app_path = node[\"app-path\"].as<std::string>();\n      graph_spec.manifest_path = node[\"manifest-path\"].as<std::string>();\n      // optional fields\n      if (node[\"param-path\"].IsDefined()) {  // Check if \"param-path\" is defined\n        graph_spec.param_path = node[\"param-path\"].as<std::string>();\n      } else {\n        graph_spec.param_path = \"\";\n      }\n      if (node[\"severity\"].IsDefined()) {\n        graph_spec.severity = node[\"severity\"].as<uint32_t>();\n      } else {\n        graph_spec.severity = 3;\n      }\n      return graph_spec;\n    }\n};\n\ntemplate <>\nstruct ParameterWrapper<GraphSpec> {\n  // Wrap the value to a YAML::Node instance\n  static Expected<YAML::Node> Wrap(\n    gxf_context_t context,\n    const GraphSpec& value) {\n    YAML::Node node;\n    // mandatory fields\n    node[\"app-path\"] = value.app_path;\n    node[\"manifest-path\"] = value.manifest_path;\n    // optional fields\n    if (!value.param_path.empty()) {\n      node[\"param-path\"] = value.param_path;\n    }\n    if (value.severity >= 0 && value.severity <= 4) {\n      node[\"severity\"] = value.severity;\n    }\n    return node;\n  }\n};\n\ntemplate <>\nstruct ParameterParser<std::map<std::string, GraphSpec>> {\n  static Expected<std::map<std::string, GraphSpec>> Parse(\n    gxf_context_t context, gxf_uid_t component_uid, const char* key,\n    const YAML::Node& node, const std::string& prefix) {\n      if (!node.IsMap()) {\n        return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n      }\n      std::map<std::string, GraphSpec> segment_runners;\n      for (const auto& p : node) {\n        const std::string k = p.first.as<std::string>();\n        const auto maybe = ParameterParser<GraphSpec>::Parse(\n          context, component_uid, k.c_str(), node[k], prefix);\n        if (!maybe) {\n          return ForwardError(maybe);\n        }\n        segment_runners[k] = maybe.value();\n      }\n      return segment_runners;\n    }\n};\n\ntemplate <>\nstruct ParameterWrapper<std::map<std::string, GraphSpec>> {\n  // Wrap the value to a YAML::Node instance\n  static Expected<YAML::Node> Wrap(\n    gxf_context_t context,\n    const std::map<std::string, GraphSpec>& value) {\n    YAML::Node node(YAML::NodeType::Map);\n    for (auto &i : value) {\n      auto maybe = ParameterWrapper<GraphSpec>::Wrap(context, i.second);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node[i.first] = maybe.value();\n    }\n    return node;\n  }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_GRAPH_WORKER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/greedy_scheduler.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_GREEDY_SCHEDULER_HPP_\n#define NVIDIA_GXF_STD_GREEDY_SCHEDULER_HPP_\n\n#include <algorithm>\n#include <atomic>\n#include <memory>\n#include <thread>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"common/fixed_vector.hpp\"\n#include \"common/logger.hpp\"\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/std/clock.hpp\"\n#include \"gxf/std/gems/event_list/event_list.hpp\"\n#include \"gxf/std/scheduler.hpp\"\n#include \"gxf/std/scheduling_condition.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Forward declarations\nclass EntityExecutor;\n\n/// @brief A basic single-threaded scheduler which tests scheduling term greedily\n///\n/// This scheduler is great for simple use cases and predictable execution. It evaluates\n/// scheduling terms greedily and may incur a large overhead of scheduling term execution. Thus it\n/// may not be suitable for large applications.\n///\n/// The scheduler requires a Clock to keep track of time. Based on the choice of clock the scheduler\n/// will execute differently. If a Realtime clock is used the scheduler will execute in realtime.\n/// This means for example pausing execution, i.e. sleeping the thread, until periodic scheduling\n/// terms are due again. If a ManualClock is used scheduling will happen \"time-compressed\". This\n/// means flow of time is altered to execute codelets immediately after each other.\nclass GreedyScheduler : public Scheduler {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n  gxf_result_t prepare_abi(EntityExecutor* executor) override;\n  gxf_result_t schedule_abi(gxf_uid_t eid) override;\n  gxf_result_t unschedule_abi(gxf_uid_t eid) override;\n  gxf_result_t runAsync_abi() override;\n  gxf_result_t stop_abi() override;\n  gxf_result_t wait_abi() override;\n  gxf_result_t event_notify_abi(gxf_uid_t eid, gxf_event_t event) override;\n\n private:\n  Parameter<Handle<Clock>> clock_;\n  Parameter<bool> realtime_;\n  Parameter<int64_t> max_duration_ms_;\n  Parameter<bool> stop_on_deadlock_;\n  Parameter<double> check_recession_period_ms_;\n  Parameter<int64_t> stop_on_deadlock_timeout_;\n\n  EntityExecutor* executor_ = nullptr;\n\n  std::atomic_bool stopping_{true};\n  std::atomic<gxf_result_t> thread_error_code_;\n  std::unique_ptr<std::thread> thread_ = nullptr;\n\n  // Used temporarily until realtime flag is removed.\n  Entity clock_entity_;\n\n  // Used for keeping track of async events\n  uint64_t count_wait_event_{0};\n  std::mutex event_mutex_;\n  std::unique_ptr<EventList<gxf_uid_t>> event_notified_;\n  std::unique_ptr<EventList<gxf_uid_t>> event_waiting_;\n  std::condition_variable event_notification_cv_;\n\n  // Used for keeping track of graph entities\n  FixedVector<gxf_uid_t> active_entities_;\n  FixedVector<gxf_uid_t> new_entities_;\n  std::unique_ptr<EventList<gxf_uid_t>> unschedule_entities_;\n  std::mutex entity_mutex_;\n  // Dedicated mutex for each entity\n  std::unordered_map<gxf_uid_t, std::unique_ptr<std::mutex>> entity_mutex_map_;\n\n  // latest timestamp from last should_stop == false\n  int64_t last_no_stop_ts_ = 0;\n  // maintain last no stop timestamp, and check if need to update should_stop\n  gxf_result_t stop_on_deadlock_timeout(const int64_t timeout, const int64_t now,\n    bool& should_stop);\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_GREEDY_SCHEDULER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/ipc_client.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_GXF_STD_IPC_CLIENT_HPP_\n#define NVIDIA_GXF_GXF_STD_IPC_CLIENT_HPP_\n\n#include <string>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * interface of client to IPCServer\n*/\nclass IPCClient : public Component {\n public:\n  static constexpr const char* kDefaultPingServiceName = \"ping\";\n  /**\n   * param:\n   *   rervice: object that implements the query API\n   *   resource: on which the query is to be performed\n   * return: text response that carries the result of a query\n  */\n  virtual Expected<std::string> query(const std::string& service,\n                                      const std::string& resource) = 0;\n  /**\n   * param:\n   *   service: object that implements the action API\n   *   resource: on which the action is to be performed\n   *   data: action data\n   * return: indicator of a success or failure\n  */\n  virtual Expected<void> action(const std::string& service,\n                                const std::string& resource,\n                                const std::string& data) = 0;\n  virtual Expected<std::string>\n    ping(const std::string& service = kDefaultPingServiceName) = 0;\n  virtual IPCClient& changeAddress(const std::string& ip, uint32_t port) = 0;\n\n protected:\n  Parameter<uint32_t> port_;\n  Parameter<std::string> server_ip_address_;\n  std::string toIpPort(const std::string& ip, uint32_t port) {\n    return ip + \":\" + std::to_string(port);\n  }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/ipc_server.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_GXF_STD_IPC_SERVER_HPP_\n#define NVIDIA_GXF_GXF_STD_IPC_SERVER_HPP_\n\n#include <functional>\n#include <string>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// interface of a request/response based server which accepts requests composed\n// of a list of strings as parameters and returns a text(json) response\nclass IPCServer : public Component {\n public:\n  typedef enum {\n    kQuery = 0,\n    kAction\n  } ServiceType;\n\n  // service handler for query requests\n  // params:\n  //   resource: on which the query is to be performed\n  // return: text response that carries the result of a query\n  typedef std::function<Expected<std::string>(const std::string& resource)> QueryHandler;\n\n  // service handler for action requests\n  // params:\n  //  resource: on which the action is to be performed\n  //  data: action data\n  // return: indicator of a success or failure\n  typedef std::function<Expected<void>(const std::string& resource,\n                                       const std::string& data)> ActionHandler;\n\n  // callback to handle the request received by the server\n  typedef struct {\n    std::string name;\n    ServiceType type;\n    // TODO(chunlinl): use std::variant to wrap the handler once c++17 support is added\n    struct {\n      QueryHandler query;\n      ActionHandler action;\n    } handler;\n  } Service;\n\n  // register a named service on the IPC server to handler requests from remote clients\n  virtual Expected<void> registerService(const Service& service) = 0;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/memory_buffer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_MEMORY_BUFFER_HPP_\n#define NVIDIA_GXF_STD_MEMORY_BUFFER_HPP_\n\n#include <utility>\n\n#include \"common/byte.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/std/allocator.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nclass MemoryBuffer {\n public:\n  MemoryBuffer() = default;\n  MemoryBuffer(const MemoryBuffer&) = delete;\n  MemoryBuffer& operator=(const MemoryBuffer&) = delete;\n\n  MemoryBuffer(MemoryBuffer&& other) { *this = std::move(other); }\n\n  MemoryBuffer& operator=(MemoryBuffer&& other) {\n    size_ = other.size_;\n    storage_type_ = other.storage_type_;\n    pointer_ = other.pointer_;\n    release_func_ =  std::move(other.release_func_);\n\n    other.pointer_ = nullptr;\n    other.release_func_ = nullptr;\n\n    return *this;\n  }\n\n  // Type of the callback function to release memory passed to the MemoryBuffer\n  // using the wrapMemory method\n  using release_function_t = std::function<Expected<void> (void* pointer)>;\n\n  Expected<void> freeBuffer() {\n    if (release_func_ && pointer_) {\n      const Expected<void> result = release_func_(pointer_);\n      if (!result) { return ForwardError(result); }\n\n      release_func_ = nullptr;\n      pointer_ = nullptr;\n      size_ = 0;\n    }\n\n    return Success;\n  }\n\n  virtual ~MemoryBuffer() { freeBuffer(); }\n\n  Expected<void> resize(Handle<Allocator> allocator, uint64_t size,\n                         MemoryStorageType storage_type) {\n    const auto result = freeBuffer();\n    if (!result) {\n      GXF_LOG_ERROR(\"Failed to free memory. Error code: %s\", GxfResultStr(result.error()));\n      return ForwardError(result);\n    }\n\n    const auto maybe = allocator->allocate(size, storage_type);\n    if (!maybe) {\n      GXF_LOG_ERROR(\"%s Failed to allocate %ld size of memory of type %d. Error code: %s\",\n                    allocator->name(), size, static_cast<int32_t>(storage_type),\n                    GxfResultStr(maybe.error()));\n      return ForwardError(maybe);\n    }\n\n    storage_type_ = storage_type;\n    pointer_ = maybe.value();\n    size_ = size;\n\n    release_func_ = [allocator] (void *data) {\n      return allocator->free(reinterpret_cast<byte*>(data));\n    };\n\n    return Success;\n  }\n\n  // Wrap existing memory inside the MemoryBuffer. A callback function of type\n  // release_function_t may be passed that will be called when the MemoryBuffer\n  // wants to release the memory.\n  Expected<void> wrapMemory(void* pointer, uint64_t size,\n                            MemoryStorageType storage_type,\n                            release_function_t release_func) {\n    if (pointer != this->pointer()) {\n      const auto result = freeBuffer();\n      if (!result) { return ForwardError(result); }\n\n      pointer_ = reinterpret_cast<byte*>(pointer);\n    }\n    storage_type_ = storage_type;\n    size_ = size;\n    release_func_ = release_func;\n\n    return Success;\n  }\n\n  // The type of memory where the data is stored.\n  MemoryStorageType storage_type() const { return storage_type_; }\n\n  // Raw pointer to the first byte of elements stored in the buffer.\n  byte* pointer() const { return pointer_; }\n\n  // Size of buffer contents in bytes\n  uint64_t size() const { return size_; }\n\n protected:\n  uint64_t size_ = 0;\n  byte* pointer_ = nullptr;\n  MemoryStorageType storage_type_ = MemoryStorageType::kHost;\n  release_function_t release_func_ = nullptr;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_MEMORY_BUFFER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/metric.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_METRIC_HPP_\n#define NVIDIA_GXF_STD_METRIC_HPP_\n\n#include <string>\n#include <vector>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// This component holds data for a single metric, where a metric is any double-typed value that is\n// computed during the execution of a GXF application, and has success criteria defined by a\n// lower threshold, an upper threshold, and an aggregation function that specifies how to\n// aggregate multiple samples into a final value for the metric. Other components in a GXF app can\n// access this component in order to, for example, write the metrics to a file for downstream\n// analysis of app execution.\nclass Metric : public Component {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override { return GXF_SUCCESS; }\n\n  // Type for the aggregation functor.\n  using aggregation_function_t = std::function<double(double)>;\n\n  // Records a single sample to the metric and updates aggregated_value_.\n  Expected<void> record(double sample);\n\n  // Sets a custom function object to use when computing the aggregated_value_ over a set of\n  // samples.\n  Expected<void> setAggregationFunction(aggregation_function_t aggregation_function);\n\n  // Checks whether the aggregated_value_ lies within the expected range.\n  Expected<bool> evaluateSuccess();\n\n  // Public accessor functions for metric data.\n  Expected<double> getAggregatedValue();\n  Expected<double> getLowerThreshold();\n  Expected<double> getUpperThreshold();\n\n  // Public functions to set a few common aggregation functions.\n  // Computes the mean of a set of samples.\n  Expected<void> setMeanAggregationFunction();\n  // Computes the root-mean-square average over a set of samples.\n  Expected<void> setRootMeanSquareAggregationFunction();\n  // Finds the maximum absolute value over a set of samples.\n  Expected<void> setAbsMaxAggregationFunction();\n  // Finds the maximum over a set of samples.\n  Expected<void> setMaxAggregationFunction();\n  // Finds the minimum over a set of samples.\n  Expected<void> setMinAggregationFunction();\n  // Computes the total sum over a set of samples.\n  Expected<void> setSumAggregationFunction();\n  // Fixes the aggregated value to the last recorded sample.\n  Expected<void> setFixedAggregationFunction();\n\n  // Resets the aggregated value to an uninitialized value\n  Expected<void> reset();\n\n private:\n  Parameter<std::string> aggregation_policy_;\n  Parameter<double> lower_threshold_;\n  Parameter<double> upper_threshold_;\n\n  // Current aggregated value across all samples recorded to this metric. Updated on record().\n  Expected<double> aggregated_value_ = Unexpected{GXF_UNINITIALIZED_VALUE};\n  // Function object that is used to update the metric's aggregated value based on a given sample.\n  // The function accepts a sample and returns the updated aggregated value. Called on record().\n  aggregation_function_t aggregation_function_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_METRIC_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/monitor.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_MONITOR_HPP_\n#define NVIDIA_GXF_STD_MONITOR_HPP_\n\n#include \"gxf/core/component.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Interface for monitoring entities during runtime\nclass Monitor : public Component {\n public:\n  virtual ~Monitor() = default;\n\n  // Callback for after an entity executes\n  //         eid - ID of entity that finished execution\n  //   timestamp - execution timestamp\n  //        code - execution result\n  virtual gxf_result_t on_execute_abi(gxf_uid_t eid, uint64_t timestamp, gxf_result_t code) = 0;\n\n  // C++ API wrapper\n  Expected<void> onExecute(gxf_uid_t eid, uint64_t timestamp, gxf_result_t code);\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_MONITOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/multi_thread_scheduler.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_MUTLI_THREAD_SCHEDULER_HPP_\n#define NVIDIA_GXF_STD_MUTLI_THREAD_SCHEDULER_HPP_\n\n#include <algorithm>\n#include <atomic>\n#include <chrono>\n#include <condition_variable>\n#include <map>\n#include <memory>\n#include <mutex>\n#include <set>\n#include <string>\n#include <thread>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/std/clock.hpp\"\n#include \"gxf/std/cpu_thread.hpp\"\n#include \"gxf/std/gems/event_list/event_list.hpp\"\n#include \"gxf/std/gems/staging_queue/staging_queue.hpp\"\n#include \"gxf/std/gems/timed_job_list/timed_job_list.hpp\"\n#include \"gxf/std/scheduler.hpp\"\n#include \"gxf/std/scheduling_condition.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Forward declarations\nclass EntityExecutor;\n\nconstexpr int64_t kMsToNs = 1'000'000l;      // Convenient constant of 1 ms = 1e6 ns\nconstexpr int64_t kMaxSlipNs = 1 * kMsToNs;  // Max slip tolerance set to 1 ms\n\n/// @brief Multi-threaded scheduler\n///\n/// The scheduler requires a Clock to keep track of time. It spawns a dispatcher thread and several\n/// worker threads as configured. Dispatcher thread checks condition of entities and place them in\n/// worker queue if they are ready to run or have expected time to run.\nclass MultiThreadScheduler : public Scheduler {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n  gxf_result_t prepare_abi(EntityExecutor* executor) override;\n  gxf_result_t schedule_abi(gxf_uid_t eid) override;\n  gxf_result_t unschedule_abi(gxf_uid_t eid) override;\n  gxf_result_t runAsync_abi() override;\n  gxf_result_t stop_abi() override;\n  gxf_result_t wait_abi() override;\n  gxf_result_t event_notify_abi(gxf_uid_t eid, gxf_event_t event) override;\n\n private:\n  std::atomic<uint64_t> workerExecTime { 0 };\n  std::atomic<uint64_t> workerWaitTime { 0};\n  std::atomic<uint64_t> workerExecCount { 0 };\n  uint64_t dispatcherExecTime = 0;\n  uint64_t dispatcherWaitTime = 0;\n  uint64_t dispatcherExecCount = 0;\n  // Entrance for dispatcher threads\n  void dispatcherThreadEntrance();\n  // Entrance for async event handler thread\n  void asyncEventThreadEntrance();\n  // Entrance for worker threads\n  void workerThreadEntrance(ThreadPool* pool, int64_t thread_number);\n\n  // Checks if need to stop due to no active entities or expiration\n  void checkEndingCriteria(int64_t timestamp);\n\n  // Updates condition of specific entity id and keep track of how many entities are good to run\n  void updateCondition(gxf_uid_t eid, const SchedulingCondition& next_condition);\n\n  // stops all async threads and deactivates all the entities\n  gxf_result_t stopAllThreads();\n\n  // stops all jobs queued in dispatcher and workers\n  void stopAllJobs();\n\n  // returns the current state of the scheduler as string\n  const char* schedulerStateString();\n\n  // cache thread info for pinned job into resources_\n  void prepareResourceMap(gxf_uid_t eid);\n  // cache thread info for every job into resources_, use default thread pool for non-pinned job\n  void prepareResourceMapStrict(gxf_uid_t eid);\n  // In worker thread, check if pinned job matches the thread\n  bool isJobMatch(ThreadPool* pool, int64_t thread_number, gxf_uid_t eid);\n  // In worker thread, check if the job matches the thread and if thread matches the job\n  bool isJobMatchStrict(ThreadPool* pool, int64_t thread_number, gxf_uid_t eid);\n\n  // Parameters\n  Parameter<Handle<Clock>> clock_;\n  Parameter<int64_t> max_duration_ms_;\n  Parameter<double> check_recession_period_ms_;\n  Parameter<bool> stop_on_deadlock_;\n  Parameter<int64_t> stop_on_deadlock_timeout_;\n  Parameter<int64_t> worker_thread_number_;\n  Parameter<bool> thread_pool_allocation_auto_;\n  Parameter<bool> strict_job_thread_pinning_;\n  Parameter<std::string> worker_thread_name_id_;\n\n  EntityExecutor* executor_ = nullptr;\n\n  ThreadPool default_thread_pool_;\n\n  // thread pool set including default pool and added pools\n  std::set<ThreadPool*> thread_pool_set_;\n\n  // Resource map for thread pools and worker threads\n  std::map<gxf_uid_t, std::pair<ThreadPool*, int64_t>> resources_;\n\n  gxf_result_t thread_error_code_;\n\n  // A thread to dispatch jobs to worker pool\n  std::thread dispatcher_thread_;\n\n  // Mutex to synchronize dispatcher thread\n  std::mutex dispatcher_sync_mutex_;\n\n  // instances of dispatcher/worker threads. 0 index would be async event handler thread.\n  std::vector<std::thread> async_threads_;\n\n  // Keep track of the timestamp that the scheduler started working for checking expiration\n  int64_t start_timestamp_ = 0;\n\n  // Keep track of conditions for entities\n  std::unordered_map<gxf_uid_t, SchedulingCondition> conditions_;\n  int64_t ready_count_{0};\n  int64_t wait_time_count_{0};\n  int64_t wait_event_count_{0};\n  int64_t wait_count_{0};\n  std::mutex conditions_mutex_;\n\n  // Queue with execution time for jobs to execute\n  std::unique_ptr<TimedJobList<gxf_uid_t>> worker_jobs_;\n\n  // Queue worker threads => dispatcher thread\n  std::unique_ptr<TimedJobList<gxf_uid_t>> check_jobs_;\n\n  // synchronize entities\n  std::mutex event_notification_mutex_;\n  std::unique_ptr<EventList<gxf_uid_t>> unschedule_entities_;\n\n  // Mutex to synchronize worker thread pool\n  std::mutex thread_sync_mutex_;\n  std::condition_variable thread_sync_cv_;\n\n  // Keep track of asynchronous events\n  std::unique_ptr<EventList<gxf_uid_t>> event_notified_;\n  std::unique_ptr<EventList<gxf_uid_t>> event_waiting_;\n  std::condition_variable event_notification_cv_;\n\n  enum class State {\n    kNotStarted = 0,       // Scheduler has not started execution yet, starts during runAsync_abi()\n    kRunning,              // Execution threads are running\n    kStopping,             // Ending criteria reached, execution threads are doing pending jobs\n    kStopped,              // Scheduler has stopped\n  };\n\n  std::atomic<State> state_{State::kNotStarted};\n  std::mutex state_change_mutex_;\n  std::condition_variable work_done_cv_;\n\n  // latest timestamp from last should_stop == false\n  int64_t last_no_stop_ts_ = 0;\n  // maintain last no stop timestamp, and check if need to update should_stop\n  gxf_result_t stop_on_deadlock_timeout(const int64_t timeout,\n    const int64_t now, bool& should_stop);\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_MUTLI_THREAD_SCHEDULER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/network_context.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef GXF_STD_NETWORK_CONTEXT_HPP\n#define GXF_STD_NETWORK_CONTEXT_HPP\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/handle.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nclass NetworkContext : public Component {\n public:\n  virtual gxf_result_t init_context() = 0;\n  // Finds transmitters and receivers passes the network context to transmitter\n  // and receivers and make connection between them\n  virtual Expected<void> addRoutes(const Entity& entity) = 0;\n\n  // Closes the connection between transmitters and receivers\n  virtual Expected<void> removeRoutes(const Entity& entity) = 0;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/new_component_allocator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_NEW_COMPONENT_ALLOCATOR_HPP\n#define NVIDIA_GXF_STD_NEW_COMPONENT_ALLOCATOR_HPP\n\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/component_allocator.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Default implementation for allocating components using new/delete\ntemplate <typename T, typename Enabler = void>\nclass NewComponentAllocator\n    : public ComponentAllocator {\n public:\n  ~NewComponentAllocator() override = default;\n\n  gxf_result_t allocate_abi(void** out_pointer) override {\n    if (out_pointer == nullptr) {\n      return GXF_ARGUMENT_NULL;\n    }\n    *out_pointer = static_cast<void*>(new T());\n    if (*out_pointer == nullptr) {\n      return GXF_OUT_OF_MEMORY;\n    }\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t deallocate_abi(void* pointer) override {\n    if (pointer == nullptr) {\n      return GXF_ARGUMENT_NULL;\n    }\n    delete static_cast<T*>(pointer);\n    return GXF_SUCCESS;\n  }\n};\n\n// Special case of standard component allocator for abstract components\ntemplate <typename T>\nclass NewComponentAllocator<T, std::enable_if_t<std::is_abstract<T>::value>>\n    : public ComponentAllocator {\n public:\n  ~NewComponentAllocator() override = default;\n\n  gxf_result_t allocate_abi(void** out_pointer) override {\n    return GXF_FACTORY_ABSTRACT_CLASS;\n  }\n\n  gxf_result_t deallocate_abi(void* pointer) override {\n    return GXF_FACTORY_ABSTRACT_CLASS;\n  }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/queue.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 202-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_QUEUE_HPP\n#define NVIDIA_GXF_STD_QUEUE_HPP\n\n#include \"gxf/core/component.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Interface for storing entities in a queue\nclass Queue : public Component {\n public:\n  virtual ~Queue() = default;\n\n  // Gets the next (oldest) entity in the queue.\n  virtual gxf_result_t pop_abi(gxf_uid_t* uid) = 0;\n\n  // Adds a new entity to the queue. The receiver takes shared\n  // ownership of the entity.\n  virtual gxf_result_t push_abi(gxf_uid_t other) = 0;\n\n  // Peeks the entity at given index on the main stage.\n  virtual gxf_result_t peek_abi(gxf_uid_t* uid, int32_t index) = 0;\n\n  // The total number of entities the queue can hold.\n  virtual size_t capacity_abi() = 0;\n\n  // The total number of entities the queue currently holds.\n  virtual size_t size_abi() = 0;\n\n  Expected<Entity> pop();\n\n  Expected<void> push(const Entity& other);\n\n  Expected<Entity> peek(int32_t index = 0);\n\n  size_t capacity();\n\n  size_t size();\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/receiver.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_RECEIVER_HPP\n#define NVIDIA_GXF_STD_RECEIVER_HPP\n\n#include <set>\n#include \"gxf/std/queue.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Interface for receiving entities.\nclass Receiver : public Queue {\n public:\n  // Receives the next entity from the main stage.\n  virtual gxf_result_t receive_abi(gxf_uid_t* uid) = 0;\n\n  // The total number of entities which have recently arrived but are not yet on the main stage.\n  virtual size_t back_size_abi() = 0;\n\n  // Peeks into back stage\n  virtual gxf_result_t peek_back_abi(gxf_uid_t* uid, int32_t index) = 0;\n\n  // Moves entities which recently arrived to the main stage.\n  virtual gxf_result_t sync_abi() = 0;\n\n  virtual gxf_result_t sync_io_abi() { return GXF_SUCCESS; }\n\n  virtual gxf_result_t wait_abi() { return GXF_SUCCESS; }\n\n  Expected<Entity> receive();\n\n  size_t back_size();\n\n  Expected<void> sync();\n\n  Expected<void> sync_io();\n\n  Expected<void> wait();\n\n  Expected<Entity> peekBack(int32_t index = 0);\n\n  Expected<void> setTransmitter(Handle<Transmitter> tx);\n\n private:\n  std::set<Handle<Transmitter>> connected_transmitters_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/resources.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_RESOURCES_HPP_\n#define NVIDIA_GXF_STD_RESOURCES_HPP_\n\n#include <map>\n#include <string>\n#include <vector>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/entity.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// A base type for all kinds of resources.\n// This is used to filter out resource Component from an entity\nclass ResourceBase : public Component {};\n\nclass ThreadPool : public ResourceBase {\n public:\n  struct Thread {\n    gxf_uid_t uid;\n    gxf_uid_t cpu_thread_cid;\n    // Scalability: std::thread\n  };\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  Expected<gxf_uid_t> addThread(gxf_uid_t key, gxf_uid_t cpu_thread_cid = kUnspecifiedUid);\n  Expected<gxf_uid_t> addThread(gxf_uid_t key, const std::vector<uint32_t>& pin_cores);\n  const Expected<Thread> getThread(gxf_uid_t key) const;\n  const std::map<gxf_uid_t, Thread>& get() const;\n  int64_t size() const;\n  int64_t priority() const;\n\n  std::vector<uint32_t> coresToPin() const {\n    const auto maybe_pin_cores = pin_cores_.try_get();\n    if (maybe_pin_cores) {\n      return maybe_pin_cores.value();\n    }\n    return {};\n  }\n\n private:\n  Parameter<int64_t> initial_size_;\n  Parameter<int64_t> priority_;\n  // CPU core IDs to pin the worker threads to (empty means no core pinning)\n  Parameter<std::vector<uint32_t>> pin_cores_;\n  std::map<gxf_uid_t, Thread> thread_pool_;\n  static inline gxf_uid_t next_uid_ = 0;\n};\n\nclass GPUDevice : public ResourceBase {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  int32_t device_id() const { return dev_id_; }\n\n private:\n  Parameter<int32_t> dev_id_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_RESOURCES_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/scheduler.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <unistd.h>\n#include <memory>\n#include <vector>\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/resources.hpp\"\n#include \"gxf/std/system.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nclass EntityExecutor;  // forward declaration\n\n// An interface which extends the nvidia::gxf::System interface to create schedulers\n// which can execute codelets.\n\nclass Scheduler : public System {\n public:\n  virtual gxf_result_t prepare_abi(EntityExecutor* executor) = 0;\n};\n\n#ifdef __linux__\n\n// glibc before 2.41 does not provide wrappers for sched_setattr() and sched_getattr(),\n// so we need to define them here.\n#ifdef __x86_64__\n#define __NR_sched_setattr 314\n#define __NR_sched_getattr 315\n#endif\n\n#ifdef __aarch64__\n#define __NR_sched_setattr 274\n#define __NR_sched_getattr 275\n#endif\n\nstruct sched_attr {\n  uint32_t size;\n  uint32_t sched_policy;\n  uint64_t sched_flags;\n  int32_t sched_nice;\n  uint32_t sched_priority;\n  uint64_t sched_runtime;\n  uint64_t sched_deadline;\n  uint64_t sched_period;\n};\n\nstatic inline int\nsched_setattr(pid_t pid, const struct sched_attr* attr, unsigned flags) {\n    return syscall(__NR_sched_setattr, pid, attr, flags);\n}\n\nstatic inline int\nsched_getattr(pid_t pid, struct sched_attr* attr,\n              unsigned int size, unsigned int flags) {\n    return syscall(__NR_sched_getattr, pid, attr, size, flags);\n}\n#endif\n\n}  // namespace gxf\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/scheduling_condition.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_SCHEDULING_CONDITION_HPP\n#define NVIDIA_GXF_STD_SCHEDULING_CONDITION_HPP\n#include <stdint.h>\n\nnamespace nvidia {\nnamespace gxf {\n\n// The type of a scheduling condition\nenum class SchedulingConditionType : int32_t {\n  // Will never execute again\n  NEVER = 0,\n  // Ready to execute now\n  READY = 1,\n  // May execute again at some point in the future\n  WAIT = 2,\n  // Will execute after a certain known time interval. Negative or zero interval will result in\n  // immediate execution.\n  WAIT_TIME = 3,\n  // Waiting for an event with unknown interval time. Entity will be put in a waiting queue until\n  // event done notification is signalled\n  WAIT_EVENT = 4,\n};\n\n// A condition for which the scheduling term is waiting.\nstruct SchedulingCondition {\n  // Describes the type of the condition\n  SchedulingConditionType type;\n  // A timestamp needed for certain conditions. Might not be set always.\n  int64_t target_timestamp;\n};\n\n// Merges two scheduling conditions with an And-like logic. For example NEVER and READY will result\n// in NEVER.\nSchedulingCondition AndCombine(SchedulingCondition a, SchedulingCondition b);\n\n// Gets a string describing the scheduling type condition\nconst char* SchedulingConditionTypeStr(const SchedulingConditionType& condition_type);\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_SCHEDULING_CONDITION_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/scheduling_term.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_SCHEDULING_TERM_HPP\n#define NVIDIA_GXF_STD_SCHEDULING_TERM_HPP\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/std/scheduling_condition.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/// @brief Base class for scheduling terms\n///\n/// Scheduling terms are used by a scheduler to determine if codelets in an entity are ready for\n/// execution.\nclass SchedulingTerm : public Component {\n public:\n  virtual ~SchedulingTerm() = default;\n\n  // Get the condition on which the scheduling waits before allowing execution. If the term is\n  // waiting for a time event 'target_timestamp' will contain the target timestamp.\n  virtual gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                                 int64_t* target_timestamp) const = 0;\n\n  // Called each time after the entity of this term was executed.\n  virtual gxf_result_t onExecute_abi(int64_t dt) = 0;\n\n  // Checks if the state of the scheduling term can be updated and updates it\n  virtual gxf_result_t update_state_abi(int64_t timestamp) { return GXF_SUCCESS; }\n\n  Expected<SchedulingCondition> check(int64_t timestamp) {\n    SchedulingConditionType status;\n    int64_t target_timestamp = 0;\n    gxf_result_t result = update_state_abi(timestamp);\n    if (result != GXF_SUCCESS) { return Unexpected{result}; }\n    const gxf_result_t error = check_abi(timestamp, &status, &target_timestamp);\n    return ExpectedOrCode(error, SchedulingCondition{status, target_timestamp});\n  }\n\n  Expected<void> onExecute(int64_t timestamp) { return ExpectedOrCode(onExecute_abi(timestamp)); }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/scheduling_term_combiner.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_SCHEDULING_TERM_COMBINER_HPP\n#define NVIDIA_GXF_STD_SCHEDULING_TERM_COMBINER_HPP\n\n#include <stdint.h>\n\n#include <algorithm>\n\n#include \"gxf/std/scheduling_term.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n/**\n * @brief Base class for scheduling term combiner\n *\n * scheduling term combiners can be used to create complex execution patterns by interpreting\n * combinations of scheduling terms differently\n */\nclass SchedulingTermCombiner : public Component {\n public:\n  // Function to combine two SchedulingCondition types\n  virtual SchedulingCondition combine(SchedulingCondition a, SchedulingCondition b) = 0;\n\n  // Get the list of scheduling terms in an entity being governed by the combiner\n  virtual FixedVector<Handle<SchedulingTerm>, kMaxComponents> getTermList() const = 0;\n};\n\n/**\n * @brief OR combiners simulate the bitwise OR operation when combining scheduling conditions\n *\n */\nclass OrSchedulingTermCombiner : public SchedulingTermCombiner {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(terms_, \"terms\", \"SchedulingTerms\",\n                                 \"The list of scheduling terms to be combined using OR operation\");\n    return GXF_SUCCESS;\n  }\n\n  SchedulingCondition combine(SchedulingCondition a, SchedulingCondition b) override {\n    // \"never\" has the highest significance\n    if (a.type == SchedulingConditionType::NEVER || b.type == SchedulingConditionType::NEVER) {\n      return {SchedulingConditionType::NEVER, 0};\n    }\n\n    // If both are ready, choose the max target timestamp\n    if (a.type == SchedulingConditionType::READY && b.type == SchedulingConditionType::READY) {\n      return {SchedulingConditionType::READY, std::max(a.target_timestamp, b.target_timestamp)};\n    }\n\n    // Check if either one of them is ready\n    if (a.type == SchedulingConditionType::READY) {\n      return {SchedulingConditionType::READY, a.target_timestamp};\n    } else if (b.type == SchedulingConditionType::READY) {\n      return {SchedulingConditionType::READY, b.target_timestamp};\n    }\n\n    // \"wait event\" has the third highest significance\n    if (a.type == SchedulingConditionType::WAIT_EVENT ||\n        b.type == SchedulingConditionType::WAIT_EVENT) {\n      return {SchedulingConditionType::WAIT_EVENT, 0};\n    }\n\n    // \"wait time\" events are combined so that the maximum time is returned\n    if (a.type == SchedulingConditionType::WAIT_TIME &&\n        b.type == SchedulingConditionType::WAIT_TIME) {\n      return {SchedulingConditionType::WAIT_TIME, std::max(a.target_timestamp, b.target_timestamp)};\n    }\n\n    // Check if atleast one of them have a target time\n    if (a.type == SchedulingConditionType::WAIT_TIME) {\n      return a;\n    } else if (b.type == SchedulingConditionType::WAIT_TIME) {\n      return b;\n    }\n\n    // The only remaining case is that both are wait\n    return {SchedulingConditionType::WAIT, 0};\n  }\n\nFixedVector<Handle<SchedulingTerm>, kMaxComponents> getTermList() const {\n  return terms_;\n}\n\nParameter<FixedVector<Handle<SchedulingTerm>, kMaxComponents>> terms_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_SCHEDULING_TERM_COMBINER_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/scheduling_terms.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_SCHEDULING_TERMS_HPP_\n#define NVIDIA_GXF_STD_SCHEDULING_TERMS_HPP_\n\n#include <set>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/core/parameter_parser.hpp\"\n#include \"gxf/core/parameter_parser_std.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/clock.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/scheduling_term.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nenum class PeriodicSchedulingPolicy {\n  // scheduler will try to \"catch up\" on missed ticks\n  // eg. assume recess period of 100ms:\n  // tick 0 at 0ms   -> next_target_ = 100ms\n  // tick 1 at 250ms -> next_target_ = 200ms (next_target_ < timestamp)\n  // tick 2 at 255ms -> next_target_ = 300ms (double tick before 300ms)\n  kCatchUpMissedTicks,\n  // scheduler guarantees recess period will have passed before next tick\n  // eg. assume recess period of 100ms:\n  // tick 0 at 0ms   -> next_target_ = 100ms\n  // tick 1 at 101ms -> next_target_ = 201ms\n  // tick 2 at 350ms -> next_target_ = 450ms\n  kMinTimeBetweenTicks,\n  // scheduler will not try to \"catch up\" on missed ticks\n  // eg. assume recess period of 100ms:\n  // tick 0 at 0ms   -> next_target_ = 100ms\n  // tick 1 at 250ms -> next_target_ = 300ms (single tick before 300ms)\n  // tick 2 at 305ms -> next_target_ = 400ms\n  kNoCatchUpMissedTicks\n};\n\n  // Custom parameter parser for PeriodicSchedulingPolicy\ntemplate <>\nstruct ParameterParser<PeriodicSchedulingPolicy> {\n  static Expected<PeriodicSchedulingPolicy> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                      const char* key, const YAML::Node& node,\n                                      const std::string& prefix) {\n    const std::string value = node.as<std::string>();\n    if (strcmp(value.c_str(), \"CatchUpMissedTicks\") == 0) {\n      return PeriodicSchedulingPolicy::kCatchUpMissedTicks;\n    }\n    if (strcmp(value.c_str(), \"MinTimeBetweenTicks\") == 0) {\n      return PeriodicSchedulingPolicy::kMinTimeBetweenTicks;\n    }\n    if (strcmp(value.c_str(), \"NoCatchUpMissedTicks\") == 0) {\n      return PeriodicSchedulingPolicy::kNoCatchUpMissedTicks;\n    }\n    return Unexpected{GXF_ARGUMENT_OUT_OF_RANGE};\n  }\n};\n\n// Custom parameter wrapper for PeriodicSchedulingPolicy\ntemplate<>\nstruct ParameterWrapper<PeriodicSchedulingPolicy> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context, const PeriodicSchedulingPolicy& value) {\n    YAML::Node node(YAML::NodeType::Scalar);\n    switch (value) {\n      case PeriodicSchedulingPolicy::kCatchUpMissedTicks: {\n        node = std::string(\"CatchUpMissedTicks\");\n        break;\n      }\n      case PeriodicSchedulingPolicy::kMinTimeBetweenTicks: {\n        node = std::string(\"MinTimeBetweenTicks\");\n        break;\n      }\n      case PeriodicSchedulingPolicy::kNoCatchUpMissedTicks: {\n        node = std::string(\"NoCatchUpMissedTicks\");\n        break;\n      }\n      default:\n        return Unexpected{GXF_PARAMETER_OUT_OF_RANGE};\n    }\n    return node;\n  }\n};\n\n// A scheduling term which permits execution only after a minimum time period has passed since the\n// last execution.\nclass PeriodicSchedulingTerm : public SchedulingTerm {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t timestamp) override;\n\n  // Minimum time which needs to elapse between two executions (in nano seconds).\n  int64_t recess_period_ns() const { return recess_period_ns_; }\n\n  // Get the last run time stamp\n  Expected<int64_t> last_run_timestamp() const { return next_target_ - recess_period_ns_; }\n\n private:\n  Parameter<std::string> recess_period_;\n  Parameter<PeriodicSchedulingPolicy> policy_;\n\n  int64_t recess_period_ns_ = 0;\n  Expected<int64_t> next_target_ = Unexpected{GXF_UNINITIALIZED_VALUE};\n};\n\n// A scheduling term which permits execution only a limited number of times.\nclass CountSchedulingTerm : public SchedulingTerm {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t timestamp) override;\n  gxf_result_t update_state_abi(int64_t timestamp) override;\n\n private:\n  Parameter<int64_t> count_;\n\n  // The remaining number of permitted executions.\n  int64_t remaining_;\n\n  SchedulingConditionType current_state_;  // The current state of the scheduling term\n  int64_t last_run_timestamp_;  // timestamp when the entity was last executed\n};\n\n// A component which specifies that an entity shall be executed if the receiver for a given\n// transmitter can accept new messages.\nclass DownstreamReceptiveSchedulingTerm : public SchedulingTerm {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t timestamp) override;\n  gxf_result_t update_state_abi(int64_t timestamp) override;\n\n  // The transmitter which needs to be able to publish a message.\n  Handle<Transmitter> transmitter() const { return transmitter_.get(); }\n  // The receiver which is connected to the transmitter and which needs to have room for messages.\n  void setReceivers(std::set<Handle<Receiver>> receivers) { receivers_ = std::move(receivers); }\n\n  // Set the transmitter parameter\n  Expected<void> setTransmitter(Handle<Transmitter> value) {\n    return transmitter_.set(value);\n  }\n  // Set the min_size parameter\n  Expected<void> setMinSize(uint64_t value) {\n    return min_size_.set(value);\n  }\n\n private:\n  Parameter<Handle<Transmitter>> transmitter_;\n  Parameter<uint64_t> min_size_;\n\n  std::set<Handle<Receiver>> receivers_;  // The receiver connected to the transmitter (if any).\n  SchedulingConditionType current_state_;   // The current state of the scheduling term\n  int64_t last_state_change_;  // timestamp when the state changed the last time\n};\n\n// A scheduling term which permits execution at a user-specified timestamp. The timestamp is\n// specified on the clock provided.\nclass TargetTimeSchedulingTerm : public SchedulingTerm {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t timestamp) override;\n\n  // Needs to be called to determine how long to wait for the next execution. If it is not called,\n  // the scheduling condition will stay as WAIT.\n  gxf_result_t setNextTargetTime(int64_t target_timestamp);\n\n private:\n  Parameter<Handle<Clock>> clock_;\n\n  // The timestamp at which the most recent execution cycle began\n  int64_t last_timestamp_;\n  // The timestamp at which the next execution cycle is requested to begin\n  mutable Expected<int64_t> target_timestamp_ = Unexpected{GXF_UNINITIALIZED_VALUE};\n  // The timestamp at which the next execution cycle is locked to begin\n  mutable Expected<int64_t> locked_target_timestamp_ = Unexpected{GXF_UNINITIALIZED_VALUE};\n};\n\n// A component which specifies that an entity shall be executed when a queue has at least a certain\n// number of elements.\nclass MessageAvailableSchedulingTerm : public SchedulingTerm {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t timestamp) override;\n  gxf_result_t update_state_abi(int64_t timestamp) override;\n\n  // Set the receiver parameter\n  Expected<void> setReceiver(Handle<Receiver> value) {\n    return receiver_.set(value);\n  }\n  // Set the min_size parameter\n  Expected<void> setMinSize(size_t value) {\n    return min_size_.set(value);\n  }\n  // Set the front_stage_max_size parameter\n  Expected<void> setFrontStageMaxSize(size_t value) {\n    return front_stage_max_size_.set(value);\n  }\n\n private:\n  // Returns true if the condition imposed by min_size is true.\n  bool checkMinSize() const;\n\n  // Returns true if the condition imposed by front_stage_max_size is true.\n  bool checkFrontStageMaxSize() const;\n\n  Parameter<Handle<Receiver>> receiver_;\n  Parameter<uint64_t> min_size_;\n  Parameter<size_t> front_stage_max_size_;\n  SchedulingConditionType current_state_;   // The current state of the scheduling term\n  int64_t last_state_change_;  // timestamp when the state changed the last time\n};\n\n// Type of sampling to be used for incoming messages\nenum struct SamplingMode {\n  kSumOfAll = 0,     // Min size specified is for the sum of all messages at all receivers;\n  kPerReceiver = 1,  // Min size specified is per receiver connected;\n};\n\n// Custom parameter parser for SamplingMode\ntemplate <>\nstruct ParameterParser<SamplingMode> {\n  static Expected<SamplingMode> Parse(gxf_context_t context, gxf_uid_t component_uid,\n                                       const char* key, const YAML::Node& node,\n                                       const std::string& prefix) {\n    const std::string value = node.as<std::string>();\n    if (strcmp(value.c_str(), \"SumOfAll\") == 0) {\n      return SamplingMode::kSumOfAll;\n    }\n    if (strcmp(value.c_str(), \"PerReceiver\") == 0) {\n      return SamplingMode::kPerReceiver;\n    }\n    return Unexpected{GXF_ARGUMENT_OUT_OF_RANGE};\n  }\n};\n\n// Custom parameter wrapper for SamplingMode\ntemplate<>\nstruct ParameterWrapper<SamplingMode> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context, const SamplingMode& value) {\n  YAML::Node node(YAML::NodeType::Scalar);\n  switch (value) {\n    case SamplingMode::kSumOfAll: {\n      node = std::string(\"SumOfAll\");\n      break;\n    }\n    case SamplingMode::kPerReceiver: {\n      node = std::string(\"PerReceiver\");\n      break;\n    }\n    default:\n      return Unexpected{GXF_PARAMETER_OUT_OF_RANGE};\n  }\n    return node;\n  }\n};\n\n\n// A scheduling term which specifies that an entity can be executed when a list of provided input\n// channels combined have at least a given number of messages.\nclass MultiMessageAvailableSchedulingTerm : public SchedulingTerm {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t timestamp) override;\n  gxf_result_t update_state_abi(int64_t timestamp) override;\n\n  // Set the min_size parameter\n  Expected<void> setMinSize(size_t value) {\n    return min_size_.set(value);\n  }\n  // Set the min_sum parameter\n  Expected<void> setMinSum(size_t value) {\n    return min_sum_.set(value);\n  }\n  // Set the sampling_mode parameter\n  Expected<void> setSamplingMode(SamplingMode value) {\n    return sampling_mode_.set(value);\n  }\n  // Add a value to receivers parameter\n  Expected<void> addReceiver(Handle<Receiver> value) {\n    auto rxs = receivers_.get();\n    auto result = rxs.push_back(value);\n    if (!result) { return Unexpected(GXF_OUT_OF_MEMORY); }\n    return receivers_.set(rxs);\n  }\n  // Add a value to min_sizes parameter\n  Expected<void> addMinSize(size_t value) {\n    auto min_sizes = min_sizes_.get();\n    auto result = min_sizes.push_back(value);\n    if (!result) { return Unexpected(GXF_OUT_OF_MEMORY); }\n    return min_sizes_.set(min_sizes);\n  }\n\n private:\n  Parameter<FixedVector<Handle<Receiver>, kMaxComponents>> receivers_;\n  Parameter<size_t> min_size_;\n  Parameter<size_t> min_sum_;\n  SchedulingConditionType current_state_;   // The current state of the scheduling term\n  int64_t last_state_change_;  // timestamp when the state changed the last time\n  Parameter<FixedVector<size_t, kMaxComponents>> min_sizes_;\n  Parameter<SamplingMode> sampling_mode_;\n};\n\n// A scheduling term which tries to wait for specified number of messages in receiver.\n// When the first message in the queue mature after specified delay since arrival it would fire\n// regardless.\nclass ExpiringMessageAvailableSchedulingTerm : public SchedulingTerm {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t timestamp) override;\n\n private:\n  Parameter<int64_t> max_batch_size_;\n  Parameter<int64_t> max_delay_ns_;\n  Parameter<Handle<Receiver>> receiver_;  // The receiver to check\n  Parameter<Handle<Clock>> clock_;\n};\n\n// A scheduling term which acts as a boolean AND term to control execution of the\n// entity\nclass BooleanSchedulingTerm : public SchedulingTerm {\n public:\n  // Enable entity execution\n  Expected<void> enable_tick();\n  // Disable entity execution\n  Expected<void> disable_tick();\n  // Returns true if the tick is enabled.\n  bool checkTickEnabled() const;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t dt) override;\n\n private:\n  Parameter<bool> enable_tick_;\n};\n\n// A Behavior Tree (BT) scheduling term which is referenced by the BT entity\n// itself and the entity's parent (if any) used to schedule the entity itself or\n// its child entities (if any) in BT\nclass BTSchedulingTerm : public SchedulingTerm {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t dt) override;\n  // called by the entity's codelet (start/stop the entity itself) or the parent\n  // entity's codelet (start/stop the child) in BT\n  gxf_result_t set_condition(SchedulingConditionType type);\n\n private:\n  Parameter<bool> is_root_;\n  SchedulingConditionType scheduling_condition_type_{SchedulingConditionType::READY};\n};\n\nenum class AsynchronousEventState {\n  READY = 0,             // Init state, first tick is pending\n  WAIT,                  // Request to async service yet to be sent, nothing to do but wait\n  EVENT_WAITING,         // Request sent to an async service, pending event done notification\n  EVENT_DONE,            // Event done notification received, entity ready to be ticked\n  EVENT_NEVER,           // Entity does not want to be ticked again, end of execution\n};\n\n// A scheduling term which waits on an asynchronous event from the codelet which can happen outside\n// of the regular tick function.\nclass AsynchronousSchedulingTerm : public SchedulingTerm {\n public:\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t dt) override;\n  void setEventState(AsynchronousEventState state);\n  AsynchronousEventState getEventState() const;\n\n private:\n  AsynchronousEventState event_state_{AsynchronousEventState::READY};\n  mutable std::mutex event_state_mutex_;\n};\n\n// A scheduling term which lets an entity maintain a specific execution (min) frequency\n// The scheduling term will also monitor messages incoming via multiple receivers and\n// switch to READY state if any messages are available\nclass MessageAvailableFrequencyThrottler : public SchedulingTerm {\n public:\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t timestamp) override;\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t update_state_abi(int64_t timestamp) override;\n\n private:\n  Expected<int64_t> last_run_timestamp_ = Unexpected{GXF_UNINITIALIZED_VALUE};\n  Parameter<std::string> execution_frequency_text_;\n  Parameter<FixedVector<Handle<Receiver>, kMaxComponents>> receivers_;\n  Parameter<size_t> min_sum_;\n  Parameter<FixedVector<size_t, kMaxComponents>> min_sizes_;\n  Parameter<SamplingMode> sampling_mode_;\n  int64_t message_recess_period_;\n  int64_t execution_frequency_;\n\n  SchedulingConditionType current_state_;   // The current state of the scheduling term\n  int64_t last_state_change_;  // timestamp when the state changed the last time\n};\n\n// A scheduling term which waits until a given number of blocks are\n// available in a pool. This can be used to force a codelet to wait\n// until a minimum number of its in-flight buffers have returned from\n// downstream consumers.\nclass MemoryAvailableSchedulingTerm : public SchedulingTerm {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t check_abi(int64_t timestamp, SchedulingConditionType* type,\n                         int64_t* target_timestamp) const override;\n  gxf_result_t onExecute_abi(int64_t dt) override;\n  gxf_result_t update_state_abi(int64_t timestamp) override;\n\n private:\n  bool is_available() const;\n\n  Parameter<Handle<Allocator>> allocator_;\n  Parameter<uint64_t> min_bytes_parameter_;\n  Parameter<uint64_t> min_blocks_parameter_;\n\n  // actual minimum number of bytes, computed from parameters\n  uint64_t min_bytes_;\n\n  SchedulingConditionType current_state_;   // The current state of the scheduling term\n  int64_t last_state_change_;  // timestamp when the state changed the last time\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_SCHEDULING_TERMS_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/system.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_CORE_SYSTEM_HPP\n#define NVIDIA_GXF_CORE_SYSTEM_HPP\n\n#include \"gxf/core/component.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Component interface for systems which are run as part of the application run cycle.\nclass System : public Component {\n public:\n  virtual ~System() = default;\n\n  virtual gxf_result_t schedule_abi(gxf_uid_t eid) = 0;\n  virtual gxf_result_t unschedule_abi(gxf_uid_t eid) = 0;\n  virtual gxf_result_t runAsync_abi() = 0;\n  virtual gxf_result_t stop_abi() = 0;\n  virtual gxf_result_t wait_abi() = 0;\n  virtual gxf_result_t event_notify_abi(gxf_uid_t eid, gxf_event_t event) = 0;\n\n  Expected<void> schedule(const Entity& entity);\n  Expected<void> unschedule(const Entity& entity);\n  Expected<void> runAsync();\n  Expected<void> stop();\n  Expected<void> wait();\n  Expected<void> event_notify(gxf_uid_t eid, gxf_event_t event);\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/tensor.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cuda_fp16.h>\n\n#include <array>\n#include <cstdint>\n#include <memory>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"common/byte.hpp\"\n#include \"dlpack/dlpack.h\"\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/complex.hpp\"\n#include \"gxf/std/dlpack_utils.hpp\"\n#include \"gxf/std/memory_buffer.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Type of parameters and other primitives\nenum class PrimitiveType : int32_t {\n  kCustom,\n  kInt8,\n  kUnsigned8,\n  kInt16,\n  kUnsigned16,\n  kInt32,\n  kUnsigned32,\n  kInt64,\n  kUnsigned64,\n  kFloat32,\n  kFloat64,\n  kComplex64,\n  kComplex128,\n  kFloat16,\n};\n\n// Returns the size of each element of specific PrimitiveType as number of bytes.\n// Returns 0 for kCustom.\nuint64_t PrimitiveTypeSize(PrimitiveType primitive);\n\ntemplate <typename T>\nstruct PrimitiveTypeTraits;\n\n#define GXF_PRIMITIVE_TYPE_TRAITS(TYPE, ENUM)                                                      \\\n  template <> struct PrimitiveTypeTraits<TYPE> {                                                   \\\n    static constexpr PrimitiveType value = PrimitiveType::ENUM;                                    \\\n    static constexpr size_t size = sizeof(TYPE);                                                   \\\n  };                                                                                               \\\n\nGXF_PRIMITIVE_TYPE_TRAITS(int8_t, kInt8);\nGXF_PRIMITIVE_TYPE_TRAITS(uint8_t, kUnsigned8);\nGXF_PRIMITIVE_TYPE_TRAITS(int16_t, kInt16);\nGXF_PRIMITIVE_TYPE_TRAITS(uint16_t, kUnsigned16);\nGXF_PRIMITIVE_TYPE_TRAITS(int32_t, kInt32);\nGXF_PRIMITIVE_TYPE_TRAITS(uint32_t, kUnsigned32);\nGXF_PRIMITIVE_TYPE_TRAITS(int64_t, kInt64);\nGXF_PRIMITIVE_TYPE_TRAITS(uint64_t, kUnsigned64);\nGXF_PRIMITIVE_TYPE_TRAITS(__half, kFloat16);\nGXF_PRIMITIVE_TYPE_TRAITS(float, kFloat32);\nGXF_PRIMITIVE_TYPE_TRAITS(double, kFloat64);\nGXF_PRIMITIVE_TYPE_TRAITS(complex64, kComplex64);\nGXF_PRIMITIVE_TYPE_TRAITS(complex128, kComplex128);\n\n// Type to hold the shape of a tensor\nclass Shape {\n public:\n  // The maximum possible rank of the tensor.\n  static constexpr uint32_t kMaxRank = 8;\n\n  // Initializes an empty rank-0 tensor.\n  Shape() : rank_(0) {}\n\n  // Initializes a shape object with the given dimensions.\n  Shape(std::initializer_list<int32_t> dimensions)\n      : rank_(0) {\n    for (int32_t dimension : dimensions) {\n      if (rank_ == kMaxRank) {\n        return;\n      }\n      dimensions_[rank_++] = dimension;\n    }\n  }\n\n  // Creates shape from vector\n  Shape(const std::vector<int32_t>& dimensions)\n      : rank_(0) {\n    for (int32_t dimension : dimensions) {\n      if (rank_ == kMaxRank) {\n        return;\n      }\n      dimensions_[rank_++] = dimension;\n    }\n  }\n\n  // Creates shape from array\n  Shape(const std::array<int32_t, Shape::kMaxRank>& dims, uint32_t rank)\n    : rank_(rank), dimensions_(dims) {\n  }\n\n  // Creates shape from array with correct rank\n  template <size_t N>\n  Shape(const std::array<int32_t, N>& dims) : rank_(N) {\n    static_assert(N < kMaxRank, \"Invalid rank\");\n    for (size_t i = 0; i < N; i++) {\n      dimensions_[i] = dims[i];\n    }\n  }\n\n  // The rank of the tensor\n  uint32_t rank() const { return rank_; }\n\n  // The total number of elements in the tensor. Note: this is not the same as the number of bytes\n  // required in memory.\n  uint64_t size() const {\n    uint64_t element_count = 1;\n    for (size_t i = 0; i < rank_; i++) {\n      element_count *= dimensions_[i];\n    }\n    return rank_ == 0 ? 0 : element_count;\n  }\n\n  // Gets the i-th dimension of the tensor.\n  // Special cases:\n  //  If the rank is 0 the function always returns 0.\n  //  If 'index' is greater or equal than the rank the function returns 1.\n  int32_t dimension(uint32_t index) const {\n    if (rank_ == 0) {\n      return 0;\n    } else if (index >= rank_) {\n      return 1;\n    } else {\n      return dimensions_[index];\n    }\n  }\n\n  bool operator== (const Shape& other) const {\n    if (rank_ != other.rank_) { return false; }\n    for (uint32_t i = 0; i < rank_; ++i) {\n      if (dimensions_[i] != other.dimensions_[i]) { return false; }\n    }\n    return true;\n  }\n\n  bool operator!= (const Shape& other) const {\n    return !(*this == other);\n  }\n\n  // Check whether shape is valid\n  bool valid() const {\n    for (uint32_t i = 0; i < rank_; ++i) {\n      if (dimensions_[i] <= 0) { return false; }\n    }\n    return true;\n  }\n\n private:\n  uint32_t rank_ = 0;\n  std::array<int32_t, kMaxRank> dimensions_;\n};\n\n/**\n * @brief Class that wraps a DLManagedTensor with a memory data reference.\n *\n * This class is used to wrap a DLManagedTensor (`tensor`) with a shared pointer to the memory\n * data (`memory_ref`). This class also holds shape and strides data in the integer type needed by\n * the DLTensor object since this does not exist as contiguous int64_t on the Tensor object\n * itself. It should also be noted that DLPack `dl_strides` stored here are in number of elements\n * while those returned by `Tensor::stride` are in bytes rather than elements.\n *\n * The DLPack protocol is designed so that the producer (GXF in the case of `toDLPack` or\n * `toDLManagedTensorContext`) continues to own the data. When the borrowing framework no longer\n * needs the tensor, it should call the deleter to notify the producer that the resource is no\n * longer needed.\n *\n * See: https://dmlc.github.io/dlpack/latest/c_api.html#_CPPv415DLManagedTensor\n */\nstruct DLManagedTensorContext {\n  DLManagedTensor tensor;            ///< The DLManagedTensor to wrap.\n  std::shared_ptr<void> memory_ref;  ///< The memory data reference.\n\n  std::vector<int64_t> dl_shape;    ///< Shape of the DLTensor.\n  std::vector<int64_t> dl_strides;  ///< Strides of the DLTensor.\n};\n\n/**\n * @brief Class to wrap the deleter of a DLManagedTensor.\n *\n * This class is used with DLManagedTensorContext class to wrap the DLManagedTensor.\n *\n * A shared pointer to this class in DLManagedTensorContext class is used as the deleter of the\n * DLManagedTensorContext::memory_ref.\n *\n * When the last reference to the DLManagedTensorContext object is released,\n * DLManagedTensorContext::memory_ref will also be destroyed, which will call the deleter function\n * of the DLManagedTensor object. This allows setting release_func to nullptr when calling\n * wrapTensor within fromDLPack.\n *\n */\nclass DLManagedMemoryBuffer {\n public:\n  explicit DLManagedMemoryBuffer(DLManagedTensor* self);\n  ~DLManagedMemoryBuffer();\n\n private:\n  DLManagedTensor* self_ = nullptr;\n};\n\n// A component which holds a single tensor. Multiple tensors can be added to one\n// entity to create a map of tensors. The component name can be used as key.\nclass Tensor {\n public:\n  typedef std::array<uint64_t, Shape::kMaxRank> stride_array_t;\n\n  Tensor() = default;\n\n  ~Tensor() {\n    memory_buffer_.freeBuffer();  // FIXME(V2) error code?\n    dl_ctx_.reset();\n    element_count_ = 0;\n    shape_ = Shape();\n  }\n\n  Tensor(const Tensor&) = delete;\n\n  Tensor(Tensor&& other) {\n    *this = std::move(other);\n  }\n\n  // zero-copy initialization from an existing DLPack DLManagedTensor (C API struct)\n  explicit Tensor(const DLManagedTensor* dl_managed_tensor_ptr);\n\n  // zero-copy initialization from an existing DLManagedTensorContext (C++-style DLPack wrapper)\n  explicit Tensor(std::shared_ptr<DLManagedTensorContext> dl_ctx);\n\n  Tensor& operator=(const Tensor&) = delete;\n\n  Tensor& operator=(Tensor&& other) {\n    shape_ = other.shape_;\n    element_count_ = other.element_count_;\n    element_type_ = other.element_type_;\n    bytes_per_element_ = other.bytes_per_element_;\n    strides_ = std::move(other.strides_);\n    memory_buffer_ = std::move(other.memory_buffer_);\n    dl_ctx_ = std::move(other.dl_ctx_);\n\n    return *this;\n  }\n\n  // The type of memory where the tensor data is stored.\n  MemoryStorageType storage_type() const { return memory_buffer_.storage_type(); }\n\n  // The shape of the dimensions holds the rank and the dimensions.\n  const Shape& shape() const { return shape_; }\n\n  // The rank of the tensor.\n  uint32_t rank() const { return shape_.rank(); }\n\n  // The scalar type of elements stored in the tensor\n  PrimitiveType element_type() const { return element_type_; }\n\n  // Number of bytes stored per element\n  uint64_t bytes_per_element() const { return bytes_per_element_; }\n\n  // Total number of elements stored in the tensor.\n  uint64_t element_count() const { return element_count_; }\n\n  // Size of tensor contents in bytes\n  size_t size() const { return memory_buffer_.size(); }\n\n  // Raw pointer to the first byte of elements stored in the tensor.\n  byte* pointer() const { return memory_buffer_.pointer(); }\n\n  // Move the memory buffer\n  MemoryBuffer move_buffer() { return std::move(memory_buffer_); }\n\n  // Gets a pointer to the first element stored in this tensor. Requested type must match the\n  // tensor element type.\n  template <typename T>\n  Expected<T*> data() {\n    if (element_type_ != PrimitiveType::kCustom && PrimitiveTypeTraits<T>::value != element_type_) {\n      return Unexpected{GXF_INVALID_DATA_FORMAT};\n    }\n    return reinterpret_cast<T*>(memory_buffer_.pointer());\n  }\n\n  // Gets a pointer to the first element stored in this tensor. Requested type must match the\n  // tensor element type.\n  template <typename T>\n  Expected<const T*> data() const {\n    if (element_type_ != PrimitiveType::kCustom && PrimitiveTypeTraits<T>::value != element_type_) {\n      return Unexpected{GXF_INVALID_DATA_FORMAT};\n    }\n    return reinterpret_cast<const T*>(memory_buffer_.pointer());\n  }\n\n  // Changes the shape and type of the tensor. Uses a primitive type and dense memory layot.\n  // Memory will be allocated with the given allocator.\n  template <typename T>\n  Expected<void> reshape(const Shape& shape, MemoryStorageType storage_type,\n                         Handle<Allocator> allocator) {\n    return reshapeCustom(shape, PrimitiveTypeTraits<T>::value, PrimitiveTypeTraits<T>::size,\n                         Unexpected{GXF_UNINITIALIZED_VALUE}, storage_type, allocator);\n  }\n  // Changes the shape and type of the tensor. Memory will be allocated with the given allocator\n  // strides: The number of bytes that each slide takes for each dimension (alignment).\n  //          Use ComputeStrides() to calculate it.\n  Expected<void> reshapeCustom(const Shape& shape,\n                               PrimitiveType element_type, uint64_t bytes_per_element,\n                               Expected<stride_array_t> strides,\n                               MemoryStorageType storage_type, Handle<Allocator> allocator);\n\n  // Type of the callback function to release memory passed to the tensor using the\n  // wrapMemory method\n  using release_function_t = MemoryBuffer::release_function_t;\n\n  // Wrap existing memory inside the tensor. A callback function of type release_function_t\n  // may be passed that will be called when the Tensor wants to release the memory.\n  Expected<void> wrapMemory(const Shape& shape,\n                            PrimitiveType element_type, uint64_t bytes_per_element,\n                            Expected<stride_array_t> strides,\n                            MemoryStorageType storage_type, void* pointer,\n                            release_function_t release_func, bool reset_dlpack = true);\n\n  // Wraps an existing memory buffer element into the current tensor\n  Expected<void> wrapMemoryBuffer(const Shape& shape,\n                                        PrimitiveType element_type, uint64_t bytes_per_element,\n                                        Expected<stride_array_t> strides,\n                                        MemoryBuffer memory_buffer);\n\n  // Permute the axes of a tensor.\n  // Number of axes must match rank of tensor.\n  // Note that since no data is moved, permuting a tensor can be\n  // detrimental to performance (data not accessed from the same cache line).\n  Expected<void> permute(const std::initializer_list<int32_t>& axes);\n\n  // Attempts to reshape the array in memory by modifying the strides\n  // Assumes tensor data is stored in row-major order.\n  // The product of the element of the new shape must equal the product\n  // of the tensor's dimensions\n  // Adapted from Numpy:\n  // https://github.com/numpy/numpy/blob/45bc13e6d922690eea43b9d807d476e0f243f836/\n  // numpy/core/src/multiarray/shape.c#L371\n  Expected<void> noCopyReshape(const std::initializer_list<int32_t>& new_shape);\n\n  // Insert singleton dimension at specified dimension\n  Expected<void> insertSingletonDim(uint32_t dimension);\n\n  // Is tensor memory access contiguous?\n  Expected<bool> isContiguous();\n\n  // The size of data in bytes\n  uint64_t bytes_size() {\n    return shape_.dimension(0) * strides_[0];\n  }\n\n  // The stride of specified rank in bytes\n  uint64_t stride(uint32_t index) const {\n    if (index >= shape_.rank()) {\n      return 0;\n    }\n    return strides_[index];\n  }\n\n  // Get a new DLManagedTensor* corresponding to the Tensor data.\n  // An alternative is to use toDLManagedTensorContext instead which returns a shared pointer\n  // that will automatically clean up resources once its reference count goes to zero.\n  Expected<DLManagedTensor*> toDLPack();\n\n  /**\n   * @brief Get the internal DLManagedTensorContext of the Tensor.\n   *\n   * @return A shared pointer to the Tensor's DLManagedTensorContext.\n   */\n  Expected<std::shared_ptr<DLManagedTensorContext>&> toDLManagedTensorContext() {\n    if (dl_ctx_ == nullptr) {\n      auto status = initializeDLContext();\n      if (!status) {\n        GXF_LOG_ERROR(\n            \"Failed to initialize DLManagedTensorContext with code: %s, returning nullptr\",\n            GxfResultStr(status.error()));\n        ForwardError(status);\n      }\n    }\n    return dl_ctx_;\n  }\n\n  // Wrap an existing DLPack managed tensor as a Tensor.\n  Expected<void> fromDLPack(const DLManagedTensor* dl_managed_tensor_ptr);\n\n  // Wrap an existing shared C++ DLManagedTensorContext as a Tensor.\n  Expected<void> fromDLPack(std::shared_ptr<DLManagedTensorContext> dl_ctx);\n\n protected:\n  // This member will be populated when memory_buffer_ is initialized or updated, allowing it to\n  // be used to provide the DLPack interface.\n  std::shared_ptr<DLManagedTensorContext> dl_ctx_;  ///< The DLManagedTensorContext object.\n\n private:\n  Shape shape_;\n  uint64_t element_count_ = 0;\n  PrimitiveType element_type_ = PrimitiveType::kUnsigned8;\n  uint64_t bytes_per_element_ = 1;\n  stride_array_t strides_;\n  MemoryBuffer memory_buffer_;\n\n  /**\n   * @brief Get DLDevice object from the GXF Tensor.\n   *\n   * @return DLDevice object.\n   */\n  Expected<DLDevice> device() const;\n\n  // populate DLPack data structures corresponding to the current Tensor\n  Expected<void> initializeDLContext();\n\n  // If dl_ctx_ has already been initialized, reset it and initialize a new one\n  Expected<void> updateDLContext();\n\n  // call wrapTensor using data from an existing DLManagedTensor\n  Expected<void> wrapDLPack(const DLManagedTensor* dl_managed_tensor_ptr,\n                            MemoryBuffer::release_function_t release_func = nullptr);\n};\n\n// Helper function to compute strides from Tensor shape, element size and non-trivial\n// alignment step size for row dimension.\n// The third rank from the end is assumed to be the row dimension.\nExpected<Tensor::stride_array_t> ComputeRowStrides(const Shape& shape, uint32_t row_step_size,\n                                                   const uint32_t bytes_per_element);\n\n// Helper function to compute trivial strides from Tensor shape and element size\nTensor::stride_array_t ComputeTrivialStrides(const Shape& shape, const uint32_t bytes_per_element);\n\n// Helper function to compute strides from steps (minimal number of bytes per slice on each rank)\nTensor::stride_array_t ComputeStrides(const Shape& shape,\n                                      const Tensor::stride_array_t& stride_steps);\n\n// Type to description a tensor used by 'CreateTensorMap'\nstruct TensorDescription {\n  std::string name;\n  MemoryStorageType storage_type;\n  Shape shape;\n  PrimitiveType element_type;\n  uint64_t bytes_per_element;\n  // array providing number of bytes for each slice on each rank\n  Expected<Tensor::stride_array_t> strides = Unexpected{GXF_UNINITIALIZED_VALUE};\n};\n\n// Creates a new entity with a collection of named tensors\nExpected<Entity> CreateTensorMap(gxf_context_t context, Handle<Allocator> pool,\n                                 std::initializer_list<TensorDescription> descriptions,\n                                 bool activate = true);\n\n// Determine tensor shape from a DLTensor struct\nExpected<Shape> ShapeFromDLTensor(const DLTensor* dl_tensor);\n\n// Determine tensor strides from a DLTensor struct\nExpected<Tensor::stride_array_t> StridesFromDLTensor(const DLTensor* dl_tensor);\n\n// Determine the tensor memory storage type from a DLTensor struct\nExpected<MemoryStorageType> MemoryStorageTypeFromDLTensor(const DLTensor* dl_tensor);\n\n// Determine the tensor primitive data type from a DLDataType struct\nExpected<PrimitiveType> PrimitiveTypeFromDLDataType(const DLDataType& dtype);\n\n// Convert PrimitiveType to its corresponding DLDataType\nExpected<DLDataType> PrimitiveTypeToDLDataType(const PrimitiveType& element_type,\n                                               uint16_t lanes = 1);\n\n}  // namespace gxf\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/timestamp.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_TIMESTAMP_HPP\n#define NVIDIA_GXF_STD_TIMESTAMP_HPP\n\n#include <cstdint>\n#include <utility>\n\n#include \"common/fixed_vector.hpp\"\n#include \"gxf/core/expected.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Contains timing information for the data in a message. All times are relative to the global GXF\n// clock and in nanoseconds.\nstruct Timestamp {\n  // The timestamp in nanoseconds at which the message was published into the system.\n  int64_t pubtime;\n  // The timestamp in nanoseconds at the message was acquired. This usually refers to the timestamp\n  // of the original sensor data which created the message.\n  int64_t acqtime;\n};\n\nenum class TimeDomainID : uint8_t {\n  // Specifies various sources of timestamp\n  TSC = 0,\n  NTP,\n  PTP,\n  TIME_DOMAIN_COUNT   // Used to determine the maximum no of timestamp sources available\n};\n\nstatic constexpr ssize_t MAX_TIME_DOMAINS = static_cast<ssize_t>(TimeDomainID::TIME_DOMAIN_COUNT);\n\n// Associate timestamp with its source\nusing MultiSourceTimestamp = FixedVector<std::pair<Timestamp, TimeDomainID>, MAX_TIME_DOMAINS>;\n\n// This function retrieves the timestamp corresponding to the time domain ID\nExpected<Timestamp> getTimestamp(MultiSourceTimestamp const& timestamps,\n                                 TimeDomainID const& timeDomainId);\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_TIMESTAMP_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/topic.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_TOPIC_HPP\n#define NVIDIA_GXF_STD_TOPIC_HPP\n\n#include <string>\n#include <vector>\n\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Class to add transmitters/receivers to a topic channel.\nclass Topic : public Component {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n\n  // Get topic name.\n  std::string getTopicName() const {\n    return topic_name_.get().empty() ? std::string(name()) : topic_name_;\n  }\n\n  // Get all transmitters in this topic.\n  std::vector<Handle<Transmitter>> getTransmitters() const { return transmitters_.get(); }\n\n  // Get all receivers in this topic.\n  std::vector<Handle<Receiver>> getReceivers() const { return receivers_.get(); }\n\n private:\n  Parameter<std::string> topic_name_;\n  Parameter<std::vector<Handle<Receiver>>> receivers_;\n  Parameter<std::vector<Handle<Transmitter>>> transmitters_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/transmitter.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_TRANSMITTER_HPP\n#define NVIDIA_GXF_STD_TRANSMITTER_HPP\n\n#include \"gxf/std/queue.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Interface for publishing entities.\nclass Transmitter : public Queue {\n public:\n  // Publishes an entity\n  virtual gxf_result_t publish_abi(gxf_uid_t uid) = 0;\n\n  // The total number of entities which have previously been published and were moved out of the\n  // main stage.\n  virtual size_t back_size_abi() = 0;\n\n  // Moves entities which were published recently out of the main stage.\n  virtual gxf_result_t sync_abi() = 0;\n\n  virtual gxf_result_t sync_io_abi() { return GXF_SUCCESS; }\n\n  virtual gxf_result_t pop_io_abi(gxf_uid_t* uid) { return GXF_NOT_IMPLEMENTED; }\n\n  Expected<void> publish(const Entity& other);\n\n  Expected<void> publish(Entity& other, const int64_t acq_timestamp);\n\n  size_t back_size();\n\n  Expected<void> sync();\n\n  Expected<void> sync_io();\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/unbounded_allocator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_STD_UNBOUNDED_ALLOCATOR_HPP_\n#define NVIDIA_GXF_STD_UNBOUNDED_ALLOCATOR_HPP_\n\n#include <mutex>\n#include <set>\n\n#include \"gxf/std/allocator.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// An allocator which uses cudaMalloc/cudaMallocHost dynamically without a pool. Does not provide\n// bounded execution times.\nclass UnboundedAllocator : public Allocator {\n public:\n  UnboundedAllocator() = default;\n  ~UnboundedAllocator() = default;\n\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n\n  gxf_result_t is_available_abi(uint64_t size) override;\n  gxf_result_t allocate_abi(uint64_t size, int32_t storage_type, void** pointer) override;\n  gxf_result_t free_abi(void* pointer) override;\n\n private:\n  // Mutex to protect cuda_blocks_\n  std::mutex mutex_;\n  // Remember the blocks so that we know how to delete them\n  std::set<void*> cuda_blocks_;\n  std::set<void*> cuda_host_blocks_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_UNBOUNDED_ALLOCATOR_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/vault.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_VAULT_HPP_\n#define NVIDIA_GXF_STD_VAULT_HPP_\n\n#include <condition_variable>\n#include <memory>\n#include <mutex>\n#include <vector>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Receives messages, stores them and provides thread-safe access to them.\nclass Vault : public Codelet {\n public:\n  using CallbackType = std::function<void(void)>;\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t start() override;\n  gxf_result_t tick() override;\n  gxf_result_t deinitialize() override;\n\n  gxf_result_t setCallback(CallbackType callback);\n\n  // Waits until at least the given number of entities have arrived, stores them in the vault,\n  // and returns their UIDs.\n  std::vector<gxf_uid_t> storeBlocking(size_t count);\n\n  // Waits until at least the given number of entities have arrived, stores them in the vault,\n  // and returns their UIDs or times out within a targeted duration (nanoseconds).\n  std::vector<gxf_uid_t> storeBlockingFor(size_t count, int64_t duration_ns);\n\n  // Tries to grab at most specified number of entities and return without waiting.\n  std::vector<gxf_uid_t> store(size_t max_count);\n\n  // Removes the given entities from the vault\n  void free(const std::vector<gxf_uid_t>& entities);\n\n private:\n  // Stores entities assuming lock\n  std::vector<gxf_uid_t> storeImpl(size_t max_count);\n\n  Parameter<Handle<Receiver>> source_;\n  Parameter<uint64_t> max_waiting_count_;\n  Parameter<bool> drop_waiting_;\n  Parameter<int64_t> callback_address_;\n  Parameter<bool> enable_callback_;\n\n  std::vector<Entity> entities_waiting_;\n  std::vector<Entity> entities_in_vault_;\n\n  std::mutex mutex_;\n  std::condition_variable condition_variable_;\n  bool alive_;\n  std::unique_ptr<CallbackType> callback_{ nullptr };\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/std/yaml_file_loader.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STD_YAML_FILE_LOADER_HPP_\n#define NVIDIA_GXF_STD_YAML_FILE_LOADER_HPP_\n\n#include <map>\n#include <memory>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"common/fixed_vector.hpp\"\n#include \"common/yaml_parser.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"yaml-cpp/yaml.h\"\n\nnamespace YAML { class Node; }\n\nnamespace nvidia {\nnamespace gxf {\n\nclass ParameterStorage;\n\n// Loads a GXF file and creates entities\n// Components will be initialized in the order they are specified in the YAML file and\n// deinitialized in reverse-initialization order\nclass YamlFileLoader {\n public:\n  // Sets the mandatory parameter storage where parameters loaded from YAML are stored.\n  void setParameterStorage(std::shared_ptr<ParameterStorage> parameter_storage) {\n    parameter_storage_ = parameter_storage;\n  }\n\n  void setFileRoot(const std::string& root) { root_ = root; }\n\n  // Load GXF Graph from file.\n  Expected<void> loadFromFile(gxf_context_t context, const std::string& filename,\n                              const std::string& entity_prefix,\n                              const char* parameters_override_string[],\n                              const uint32_t num_overrides, gxf_uid_t parent_eid = kNullUid,\n                              const YAML::Node& prerequisites = YAML::Node(YAML::NodeType::Null));\n\n  // Load GXF Graph from string.\n  Expected<void> loadFromString(gxf_context_t context, const std::string& text,\n                                const std::string& entity_prefix,\n                                const char* parameters_override_string[],\n                                const uint32_t num_overrides);\n\n  // Load GXF Graph from parsed YAML graph.\n  Expected<void> load(gxf_context_t context, const FixedVectorBase<YAML::Node>& nodes,\n                      std::string entity_prefix, gxf_uid_t parent_eid,\n                      const char* parameters_override_string[],\n                      const uint32_t num_overrides,\n                      const YAML::Node& prerequisites);\n\n  // Save GXF Graph to YAML file.\n  Expected<void> saveToFile(gxf_context_t context, const std::string& filename);\n\n private:\n  // Finds existing entity or creates a new one\n  Expected<gxf_uid_t> findOrCreateEntity(\n      gxf_context_t context, const Expected<std::string>& entity_name);\n\n  // Creates a new component in an entity by typename\n  Expected<gxf_uid_t> addComponent(gxf_context_t context, gxf_uid_t eid, const char* type);\n\n  // Finds a component in an entity by name\n  Expected<gxf_uid_t> findComponent(gxf_context_t context, gxf_uid_t eid, const char* name);\n\n  // Sets the parameters of a component\n  Expected<void> setParameters(gxf_context_t context, gxf_uid_t handle,\n                               const std::string& prefix, const YAML::Node& parameters);\n\n  // Add a component to an entity's interface mapping\n  Expected<void> addComponentToInterface(gxf_context_t context, gxf_uid_t eid,\n                                         const std::string& entity_prefix,\n                                         const std::string& interface_name,\n                                         const std::string& tag);\n\n  Expected<void> populateEntityGroups(gxf_context_t context,\n                                      const FixedVectorBase<YAML::Node>& nodes,\n                                      const std::string& entity_prefix);\n\n  std::shared_ptr<ParameterStorage> parameter_storage_ = nullptr;\n\n  std::string root_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STD_YAML_FILE_LOADER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/stream/stream_nvsci.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STREAM_STREAM_NVSCI_HPP_\n#define NVIDIA_GXF_STREAM_STREAM_NVSCI_HPP_\n\n#include <cuda_runtime.h>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/gxf.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\nenum class SyncType {\n  GXF_STREAM_SIGNALER_NONE = 0,       // Invalid value for signaler\n  GXF_STREAM_SIGNALER_CUDA,           // Indicates CUDA is the signaler.\n                                      // Used to indicate when some CUDA job is done.\n  GXF_STREAM_WAITER_NONE,             // Invalid value for waiter\n  GXF_STREAM_WAITER_CUDA,             // Indicates CUDA is the waiter.\n                                      // Used to wait on some CUDA job to be completed.\n};\n\nstruct Stream : public Component {\n  virtual ~Stream() = default;\n\n  // Allocates a nvsci sync object.\n  virtual gxf_result_t allocate_sync_object(SyncType signalerList,\n                                            SyncType waiterList, void** syncObj) {\n    return GXF_NOT_IMPLEMENTED;\n  }\n\n  // Signal semaphore with cuda stream\n  virtual gxf_result_t signalSemaphore() {\n    return GXF_NOT_IMPLEMENTED;\n  }\n\n  // Wait semaphore with cuda stream\n  virtual gxf_result_t waitSemaphore() {\n    return GXF_NOT_IMPLEMENTED;\n  }\n\n  // Set cuda stream for the give sync type\n  virtual gxf_result_t setCudaStream(SyncType syncType, cudaStream_t stream) {\n    return GXF_NOT_IMPLEMENTED;\n  }\n\n  // Imports the semaphore which will be used for signalling/waiting based on the sync type\n  // Streams preserves the semaphore internally and is not exposed to the clients\n  virtual gxf_result_t importSemaphore(SyncType syncType) {\n    return GXF_NOT_IMPLEMENTED;\n  }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STREAM_STREAM_NVSCI_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/stream/stream_nvscisync.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STREAM_STREAM_NVSCISYNC_HPP_\n#define NVIDIA_GXF_STREAM_STREAM_NVSCISYNC_HPP_\n\n#include <nvscisync.h>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/stream/stream_nvsci.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\nclass StreamSync: public Stream {\n public:\n  StreamSync() = default;\n  ~StreamSync() = default;\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n  gxf_result_t allocate_sync_object(SyncType signaler, SyncType waiter, void** syncObj) override;\n  gxf_result_t signalSemaphore() override;\n  gxf_result_t waitSemaphore() override;\n  gxf_result_t setCudaStream(SyncType syncType, cudaStream_t stream) override;\n  gxf_result_t importSemaphore(SyncType syncType) override;\n\n private:\n  gxf_result_t importSemaphore(cudaExternalSemaphore_t* semaphore, SyncType syncType);\n\n  NvSciSyncModule         sync_module_{nullptr};\n  NvSciSyncAttrList       attr_list_{nullptr};\n  NvSciSyncAttrList       reconciled_attr_list_{nullptr};\n  NvSciSyncObj            sync_obj_{nullptr};\n  NvSciSyncFence*         fence_;\n  cudaExternalSemaphore_t signaler_semaphore_;\n  cudaExternalSemaphore_t waiter_semaphore_;\n  cudaStream_t            signaler_cuda_stream_{};\n  cudaStream_t            waiter_cuda_stream_{};\n  bool                    is_signaler_semaphore_imported_{false};\n  bool                    is_waiter_semaphore_imported_{false};\n  int32_t                 num_gpus_{0};\n\n  Parameter<int32_t> signaler_device_id_;\n  Parameter<int32_t> waiter_device_id_;\n  Parameter<int32_t> signaler_;\n  Parameter<int32_t> waiter_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STREAM_STREAM_NVSCISYNC_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/stream/stream_sync_id.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STREAM_STREAM_SYNC_ID_HPP_\n#define NVIDIA_GXF_STREAM_STREAM_SYNC_ID_HPP_\n\n#include \"gxf/core/gxf.h\"\n\n\nnamespace nvidia {\nnamespace gxf {\n\n// The Structure indicates stream sync component ID.\n// Message entity carrying StreamSyncId indicates the ID of stream sync. The handle could\n// be deduced by Handle<StreamSync>::Create(context, stream_sync_cid).\nstruct StreamSyncId {\n  // component id of StreamSync\n  gxf_uid_t stream_sync_cid;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STREAM_STREAM_SYNC_ID_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/stream/tests/test_gxf_stream_sync_cuda_helper.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_STREAM_TESTS_TEST_STREAM_CUDA_HELPER_HPP\n#define NVIDIA_GXF_STREAM_TESTS_TEST_STREAM_CUDA_HELPER_HPP\n\n#include <cublas_v2.h>\n#include <limits>\n#include <numeric>\n#include <string>\n#include <utility>\n\n#include \"common/assert.hpp\"\n#include \"gxf/cuda/cuda_common.hpp\"\n#include \"gxf/cuda/cuda_stream.hpp\"\n#include \"gxf/cuda/cuda_stream_id.hpp\"\n#include \"gxf/cuda/cuda_stream_pool.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n#include \"gxf/stream/stream_nvsci.hpp\"\n#include \"gxf/stream/stream_nvscisync.hpp\"\n#include \"gxf/stream/stream_sync_id.hpp\"\n\n#define CHECK_CUBLUS_ERROR(cu_result, fmt, ...)                         \\\n  do {                                                                  \\\n    cublasStatus_t err = (cu_result);                                   \\\n    if (err != CUBLAS_STATUS_SUCCESS) {                                 \\\n      GXF_LOG_ERROR(fmt \", cublas_error: %d\", ##__VA_ARGS__, (int)err); \\\n      return Unexpected{GXF_FAILURE};                                   \\\n    }                                                                   \\\n  } while (0)\n\nnamespace nvidia {\nnamespace gxf {\nnamespace stream {\nnamespace test {\n\nconstexpr static const char* kStreamName0 = \"CudaStream0\";\nconstexpr static int kDefaultDevId = 0;\nstatic const Shape kInitTensorShape{1024, 2048};\n\n// The base class with cuda stream utils\nclass StreamBasedOpsNew : public Codelet {\n public:\n  static Expected<Handle<CudaStream>> getStream(Entity& message) {\n    auto stream_id = message.get<CudaStreamId>();\n    GXF_ASSERT(stream_id, \"failed to find cudastreamid\");\n    auto stream =\n        Handle<CudaStream>::Create(stream_id.value().context(), stream_id.value()->stream_cid);\n    GXF_ASSERT(stream, \"create cudastream from cid failed\");\n    GXF_ASSERT(stream.value(), \"cudastream handle is null\");\n    return stream;\n  }\n\n  static Expected<void> addStream(Entity& message, Handle<CudaStream>& stream,\n                                  const char* name = nullptr) {\n    auto stream_id = message.add<CudaStreamId>(name);\n    GXF_ASSERT(stream_id, \"failed to add cudastreamid\");\n    stream_id.value()->stream_cid = stream.cid();\n    GXF_ASSERT(stream_id.value()->stream_cid != kNullUid, \"stream_cid is null\");\n    return Success;\n  }\n\n  static Expected<Handle<StreamSync>> getStreamSync(Entity& message) {\n    auto streamSync = message.get<StreamSyncId>();\n    GXF_ASSERT(streamSync, \"Failed to find StreamSync\");\n    auto stream_sync = Handle<StreamSync>::Create(streamSync.value().context(),\n                                                  streamSync.value()->stream_sync_cid);\n    GXF_ASSERT(stream_sync, \"Create StreamSync from cid failed\");\n    GXF_ASSERT(stream_sync.value(), \"StreamSync handle is null\");\n    return stream_sync;\n  }\n\n  static Expected<void> addStreamSync(Entity& message, Handle<StreamSync>& streamSync,\n                                      const char* name = nullptr) {\n    auto stream_sync_id = message.add<StreamSyncId>(name);\n    GXF_ASSERT(stream_sync_id, \"Failed to add StreamSyncId\");\n    stream_sync_id.value()->stream_sync_cid = streamSync.cid();\n    GXF_ASSERT(stream_sync_id.value()->stream_sync_cid != kNullUid, \"stream_sync_cid is null\");\n    return Success;\n  }\n\n  static Expected<Handle<Tensor>> addTensor(Entity& message, Handle<Allocator> pool,\n                                            const TensorDescription& description) {\n    GXF_ASSERT(pool, \"pool is not set\");\n    auto tensor = message.add<Tensor>(description.name.c_str());\n    GXF_ASSERT(tensor, \"failed to add message tensor\");\n    const uint64_t bytes_per_element = description.element_type == PrimitiveType::kCustom\n                                           ? description.bytes_per_element\n                                           : PrimitiveTypeSize(description.element_type);\n\n    auto result = tensor.value()->reshapeCustom(description.shape, description.element_type,\n                                                bytes_per_element, description.strides,\n                                                description.storage_type, pool);\n    GXF_ASSERT(result, \"reshape tensor:%s failed\", description.name.c_str());\n    return tensor;\n  }\n};\n\n// Generate cuda tensor map with cudastreams for transmitter cuda_tx,\n// Generate host tensor map for transmitter host_tx\nclass StreamTensorGeneratorNew : public StreamBasedOpsNew {\n public:\n  gxf_result_t initialize() override {\n    GXF_ASSERT(stream_pool_.get(), \"stream pool is not set\");\n    auto stream = stream_pool_->allocateStream();\n    GXF_ASSERT(stream, \"allocating stream failed\");\n    stream_ = std::move(stream.value());\n    GXF_ASSERT(stream_->stream(), \"allocated stream is not initialized.\");\n\n    stream_sync = stream_sync_.get();\n\n    void *syncObj{nullptr};\n    GXF_ASSERT_SUCCESS(stream_sync->allocate_sync_object(static_cast<SyncType>(signaler_.get()),\n                                                         static_cast<SyncType>(waiter_.get()),\n                                                         reinterpret_cast<void**>(&syncObj)));\n    GXF_ASSERT(syncObj, \"syncObj is null\");\n\n    GXF_ASSERT_SUCCESS(stream_sync->setCudaStream(static_cast<SyncType>(signaler_.get()),\n                                                  stream_->stream().value()));\n\n    GXF_ASSERT_SUCCESS(stream_sync->setCudaStream(static_cast<SyncType>(waiter_.get()),\n                                                  stream_->stream().value()));\n\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t tick() override {\n    Expected<Entity> maybe_dev_msg = Entity::New(context());\n    GXF_ASSERT(maybe_dev_msg, \"New dev message failed\");\n    auto& dev_msg = maybe_dev_msg.value();\n\n    Expected<Entity> maybe_host_msg = Entity::New(context());\n    GXF_ASSERT(maybe_host_msg, \"New host message failed\");\n    auto& host_msg = maybe_host_msg.value();\n\n    auto ret = addStream(dev_msg, stream_, kStreamName0);\n    GXF_ASSERT(ret, \"stream tensor generator adding stream failed\");\n\n    auto sync_ret = addStreamSync(dev_msg, stream_sync, \"nvidia::gxf::StreamSync\");\n    GXF_ASSERT_TRUE(sync_ret.has_value());\n    auto maybe_stream_sync = getStreamSync(dev_msg);\n    GXF_ASSERT_TRUE(maybe_stream_sync.has_value());\n\n    ret = createTensors(dev_msg, host_msg, stream_->stream().value());\n    GXF_ASSERT(ret, \"creating tensors failed\");\n    GXF_ASSERT(stream_, \"stream is not allocated\");\n\n    maybe_stream_sync.value()->signalSemaphore();\n\n    ret = cuda_tx_->publish(dev_msg);\n    if (!ret) {\n      GXF_LOG_ERROR(\"stream tensor generator publishing cuda tensors failed\");\n      return ToResultCode(ret);\n    }\n\n    if (host_tx_.get()) {\n      ret = host_tx_->publish(host_msg);\n      if (!ret) {\n        GXF_LOG_ERROR(\"stream tensor generator publishing cuda tensors failed\");\n        return ToResultCode(ret);\n      }\n    }\n\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(cuda_tx_, \"cuda_tx\", \"transmitter of cuda tensors\", \"\");\n    result &= registrar->parameter(host_tx_, \"host_tx\", \"transmitter of host tensors\", \"\",\n                                   Handle<Transmitter>());\n    result &= registrar->parameter(cuda_tensor_pool_, \"cuda_tensor_pool\", \"Cuda Tensor Pool\", \"\");\n    result &= registrar->parameter(host_tensor_pool_, \"host_tensor_pool\", \"Host Tensor Pool\", \"\");\n    result &= registrar->parameter(stream_pool_, \"stream_pool\", \"Cuda Stream Pool\", \"\");\n    result &= registrar->parameter(stream_sync_, \"stream_sync\",\n                                   \"Synchronization object across CUDA codelets\", \"\");\n    result &= registrar->parameter(signaler_, \"signaler\", \"Signaler type\",\n                                   \"Defines the signaler type. Cuda Signaler (2)\",\n                                   static_cast<int32_t>(SyncType::GXF_STREAM_SIGNALER_CUDA));\n    result &= registrar->parameter(waiter_, \"waiter\", \"Waiter type\",\n                                   \"Defines the waiter type. Cuda Waiter (6)\",\n                                   static_cast<int32_t>(SyncType::GXF_STREAM_WAITER_CUDA));\n    return ToResultCode(result);\n  }\n\n private:\n  Expected<void> createTensors(Entity& dev_msg, Entity& host_msg, cudaStream_t stream) {\n    Shape shape = kInitTensorShape;\n    for (size_t i = 0; i < 2; ++i) {\n      TensorDescription dev_desc{\"cuda_tensor\", MemoryStorageType::kDevice, shape,\n                                 PrimitiveType::kFloat32};\n      auto cuda_tensor_ret = addTensor(dev_msg, cuda_tensor_pool_, dev_desc);\n      GXF_ASSERT(cuda_tensor_ret, \"Generator dev message adding tensor failed.\");\n      auto& cuda_tensor = cuda_tensor_ret.value();\n      Expected<float*> cuda_data = cuda_tensor->data<float>();\n      if (!cuda_data) { return ForwardError(cuda_data); }\n\n      TensorDescription host_desc{\"host_tensor\", MemoryStorageType::kHost, shape,\n                                  PrimitiveType::kFloat32};\n      auto host_tensor_ret = addTensor(host_msg, host_tensor_pool_, host_desc);\n      GXF_ASSERT(host_tensor_ret, \"Generator host message adding tensor failed.\");\n      auto& host_tensor = host_tensor_ret.value();\n      Expected<float*> host_data = host_tensor->data<float>();\n      if (!host_data) { return ForwardError(host_data); }\n\n      for (size_t j = 0; j < shape.size(); ++j) {\n        host_data.value()[j] = (j % 100) + 1.0f;\n      }\n\n      cudaError_t error = cudaMemcpyAsync(cuda_data.value(), host_data.value(), cuda_tensor->size(),\n                                          cudaMemcpyHostToDevice, stream);\n      CHECK_CUDA_ERROR(error, \"StreamTensorGeneratorNew cuda memory cpy H2D failed.\");\n    }\n\n    return Success;\n  }\n\n  Parameter<Handle<Transmitter>> cuda_tx_;\n  Parameter<Handle<Transmitter>> host_tx_;\n  Parameter<Handle<Allocator>> cuda_tensor_pool_;\n  Parameter<Handle<Allocator>> host_tensor_pool_;\n  Parameter<Handle<CudaStreamPool>> stream_pool_;\n  Parameter<Handle<StreamSync>> stream_sync_;\n  Parameter<int32_t> signaler_;\n  Parameter<int32_t> waiter_;\n\n  Handle<CudaStream> stream_;\n  Handle<StreamSync> stream_sync;\n};\n\n// Dot product execution base class\nclass DotProductExeNew {\n private:\n  Handle<Receiver> rx_;\n  Handle<Transmitter> tx_;\n  Handle<Allocator> tensor_pool_;\n\n public:\n  DotProductExeNew() = default;\n  virtual ~DotProductExeNew() = default;\n  void setEnv(const Handle<Receiver>& rx, const Handle<Transmitter>& tx,\n              const Handle<Allocator>& pool) {\n    rx_ = rx;\n    tx_ = tx;\n    tensor_pool_ = pool;\n  }\n\n  virtual Expected<void> dotproduct_i(float* in0, float* in1, float* out, int32_t row,\n                                      int32_t column, Entity& in_msg, Entity& out_msg) = 0;\n\n  Expected<void> executeNew(Entity in_msg, const char* out_tensor_name = \"\") {\n    GXF_ASSERT(rx_ && tx_ && tensor_pool_, \"dotproduct received empty in_msg\");\n\n    // get tensors\n    auto in_tensors = in_msg.findAllHeap<Tensor>();\n    GXF_ASSERT(in_tensors, \"failed to find Tensors in in_msg\");\n    GXF_ASSERT(in_tensors->size() == 2, \"doesn't find Tensors in in_msg\");\n    GXF_ASSERT(in_tensors->at(0).value()->rank() == 2, \"Input tensor rank is not 2\");\n    int32_t column = in_tensors->at(0).value()->shape().dimension(1);\n    int32_t row = in_tensors->at(0).value()->shape().dimension(0);\n    MemoryStorageType mem_type = in_tensors->at(0).value()->storage_type();\n    PrimitiveType data_type = in_tensors->at(0).value()->element_type();\n\n    float* in_data[2] = {nullptr};\n    for (size_t i = 0; i < in_tensors->size(); ++i) {\n      GXF_ASSERT(in_tensors->at(i).value(), \"Input Tensor Handle is empty\");\n      GXF_ASSERT(in_tensors->at(i).value()->rank() == 2, \"Input tensor rank is not 2\");\n      in_data[i] = ValuePointer<float>(in_tensors->at(i).value()->pointer());\n    }\n\n    Expected<Entity> output = Entity::New(in_msg.context());\n    GXF_ASSERT(output, \"Creating dotproduct output tensor failed.\");\n    Shape out_shape{row};\n    GXF_ASSERT(out_shape.rank() == 1 && out_shape.size() == static_cast<uint64_t>(row),\n               \"output_shape is not correct\");\n        TensorDescription outDesc{out_tensor_name, mem_type, out_shape, data_type};\n    auto out_tensor = StreamBasedOpsNew::addTensor(output.value(), tensor_pool_, outDesc);\n    GXF_ASSERT(out_tensor && out_tensor.value(), \"cuda dotproduct output tensor is not found\");\n    float* out_data = ValuePointer<float>(out_tensor.value()->pointer());\n        auto ret =\n        dotproduct_i(in_data[0], in_data[1], out_data, row, column, in_msg, output.value());\n    GXF_ASSERT(ret, \"dotproduct execute with implementation failed\");\n        ret = tx_->publish(output.value());\n    GXF_ASSERT(ret, \"dotproduct publishing tensors failed\");\n        return ret;\n  }\n};\n\n// Cublas Dot product Operators\nclass CublasDotProductNew : public StreamBasedOpsNew {\n public:\n  // Culblas dot production execution class\n  class CublasDotProductExe : public DotProductExeNew {\n   private:\n    cublasHandle_t handle_ = nullptr;\n    CublasDotProductNew* codelet_ = nullptr;\n\n   public:\n    CublasDotProductExe(CublasDotProductNew* codelet) : codelet_(codelet) {}\n    ~CublasDotProductExe() {\n      if (handle_) { cublasDestroy(handle_); }\n    }\n    Expected<void> dotproduct_i(float* in0, float* in1, float* out, int32_t row, int32_t column,\n                                Entity& in_msg, Entity& out_msg) override {\n      // locate stream\n      auto maybe_stream = StreamBasedOpsNew::getStream(in_msg);\n      GXF_ASSERT(maybe_stream && maybe_stream.value(), \"get stream from in_msg failed\");\n      auto& stream = maybe_stream.value();\n      auto ret = StreamBasedOpsNew::addStream(out_msg, stream);\n      GXF_ASSERT(ret, \"adding cudastream into dotproduct output message failed.\");\n\n      int gpu_id = stream->dev_id();\n      if (gpu_id >= 0) {\n        CHECK_CUDA_ERROR(cudaSetDevice(gpu_id), \"failed to set deviceid: %d\", gpu_id);\n      }\n\n      if (!handle_) {\n        CHECK_CUBLUS_ERROR(cublasCreate(&handle_), \"failed to create cublas handle\");\n      }\n      auto custream_id = stream->stream();\n      GXF_ASSERT(custream_id, \"cudastream id is invalid\");\n      CHECK_CUBLUS_ERROR(cublasSetStream(handle_, custream_id.value()), \"cublas set stream failed\");\n\n      for (int i = 0; i < row; ++i) {\n        CHECK_CUBLUS_ERROR(\n            cublasSdot(handle_, column, in0 + column * i, 1, in1 + column * i, 1, out + i),\n            \"cublasSdot failed on row :%d\", i);\n      }\n\n      return ret;\n    }\n  };\n\n  CublasDotProductNew() : exec_(this) {}\n\n  gxf_result_t initialize() override {\n    GXF_ASSERT(tensor_pool_.get() && rx_.get() && tx_.get(), \"params not set\");\n    exec_.setEnv(rx_.get(), tx_.get(), tensor_pool_.get());\n    return GXF_SUCCESS;\n  }\n\n  // gxf_result_t start() override { return ToResultCode(initOpsEvent()); }\n\n  gxf_result_t tick() override {\n    Expected<Entity> in_msg = rx_->receive();\n\n    auto maybe_stream_sync = getStreamSync(in_msg.value());\n    GXF_ASSERT_TRUE(maybe_stream_sync.has_value());\n\n    maybe_stream_sync.value()->waitSemaphore();\n\n    auto ret = exec_.executeNew(in_msg.value(), \"cublasdotproduct_tensor\");\n    return ToResultCode(ret);\n  }\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(tx_, \"tx\", \"transmitter of tensors\", \"\");\n    result &= registrar->parameter(rx_, \"rx\", \"receiver of tensors\", \"\");\n    result &= registrar->parameter(tensor_pool_, \"tensor_pool\", \"Tensor Pool\", \"\");\n    return ToResultCode(result);\n  }\n\n private:\n  Parameter<Handle<Transmitter>> tx_;\n  Parameter<Handle<Receiver>> rx_;\n  Parameter<Handle<Allocator>> tensor_pool_;\n\n  CublasDotProductExe exec_;\n};\n\n// CPU Dot product Operators\nclass HostDotProductNew : public Codelet {\n public:\n  // CPU dot production execution class\n  class HostDotProductExeNew : public DotProductExeNew {\n   public:\n    HostDotProductExeNew() = default;\n    ~HostDotProductExeNew() = default;\n    Expected<void> dotproduct_i(float* in0, float* in1, float* out, int32_t row, int32_t column,\n                                Entity& in_msg, Entity& out_msg) override {\n      for (int i = 0; i < row; ++i) {\n        float sum = 0.0;\n        float* x = in0 + column * i;\n        float* y = in1 + column * i;\n        for (int j = 0; j < column; ++j) { sum += x[j] * y[j]; }\n        out[i] = sum;\n      }\n      return Success;\n    }\n  };\n\n  gxf_result_t initialize() override {\n    GXF_ASSERT(tensor_pool_.get() && rx_.get() && tx_.get(), \"params not set\");\n    exec_.setEnv(rx_.get(), tx_.get(), tensor_pool_.get());\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t tick() override {\n    Expected<Entity> in_msg = rx_->receive();\n\n\n    auto ret = exec_.executeNew(in_msg.value(), \"host_dotproduct_tensor\");\n    return ToResultCode(ret);\n  }\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(tx_, \"tx\", \"transmitter of tensors\", \"\");\n    result &= registrar->parameter(rx_, \"rx\", \"receiver of tensors\", \"\");\n    result &= registrar->parameter(tensor_pool_, \"tensor_pool\", \"Tensor Pool\", \"\");\n    return ToResultCode(result);\n  }\n\n private:\n  Parameter<Handle<Transmitter>> tx_;\n  Parameter<Handle<Receiver>> rx_;\n  Parameter<Handle<Allocator>> tensor_pool_;\n\n  HostDotProductExeNew exec_;\n};\n\n// Stream based Memory copy from device to host\nclass MemCpy2HostNew : public StreamBasedOpsNew {\n public:\n  gxf_result_t tick() override {\n    auto in = rx_->receive();\n    GXF_ASSERT(in, \"rx received empty message\");\n    auto maybe_tensor = in.value().get<Tensor>();\n    GXF_ASSERT(maybe_tensor, \"tensor not found\");\n    auto& in_tensor = maybe_tensor.value();\n    byte* in_data = in_tensor->pointer();\n\n    Expected<Entity> out_msg = Entity::New(context());\n    TensorDescription out_desc{in_tensor.name(), MemoryStorageType::kHost, in_tensor->shape(),\n                               in_tensor->element_type()};\n    auto maybe_out_tensor = addTensor(out_msg.value(), tensor_pool_, out_desc);\n    GXF_ASSERT(maybe_out_tensor, \"Memcpy host message adding tensor failed.\");\n    auto& out_tensor = maybe_out_tensor.value();\n    byte* out_data = out_tensor->pointer();\n\n    auto maybe_stream = getStream(in.value());\n    GXF_ASSERT(maybe_stream && maybe_stream.value(), \"get stream from in failed\");\n    auto& stream = maybe_stream.value();\n    auto ret = addStream(out_msg.value(), stream);\n    GXF_ASSERT(ret, \"adding cudastream into memcpy output message failed.\");\n\n    // wrap cuda operations since CHECK_CUDA_ERROR return Expected<void>\n    ret = [&, this]() -> Expected<void> {\n      int gpu_id = stream->dev_id();\n      if (gpu_id >= 0) {\n        CHECK_CUDA_ERROR(cudaSetDevice(gpu_id), \"failed to set deviceid: %d\", gpu_id);\n      }\n      cudaError_t error = cudaMemcpyAsync(out_data, in_data, in_tensor->size(),\n                                          cudaMemcpyDeviceToHost, stream->stream().value());\n      CHECK_CUDA_ERROR(error, \"CUDA memory cpy to host failed.\");\n      return Success;\n    }();\n    GXF_ASSERT(ret, \"CUDA memory cpy to host failed.\");\n\n    ret = tx_->publish(out_msg.value());\n    GXF_ASSERT(ret, \"memcpy_to_host publishing tensors failed\");\n\n    return ToResultCode(ret);\n  }\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(tx_, \"tx\", \"transmitter of tensors\", \"\");\n    result &= registrar->parameter(rx_, \"rx\", \"receiver of tensors\", \"\");\n    result &= registrar->parameter(tensor_pool_, \"tensor_pool\", \"Tensor output pool\", \"\");\n    return ToResultCode(result);\n  }\n\n private:\n  Parameter<Handle<Transmitter>> tx_;\n  Parameter<Handle<Receiver>> rx_;\n  Parameter<Handle<Allocator>> tensor_pool_;\n};\n\n// Equal verification\nclass VerifyEqualNew : public Codelet {\n public:\n  gxf_result_t tick() override {\n    GXF_LOG_DEBUG(\"verifying frame: %d\", count_++);\n    auto in0 = rx0_->receive();\n    GXF_ASSERT(in0, \"rx0 received empty message\");\n    auto in1 = rx1_->receive();\n    GXF_ASSERT(in1, \"rx1 received empty message\");\n\n    // get tensors\n    auto maybe_tensor0 = in0.value().get<Tensor>();\n    GXF_ASSERT(maybe_tensor0, \"tensor0 not found\");\n    auto maybe_tensor1 = in1.value().get<Tensor>();\n    GXF_ASSERT(maybe_tensor1, \"tensor1 not found\");\n    auto& tensor0 = maybe_tensor0.value();\n    auto& tensor1 = maybe_tensor1.value();\n    GXF_ASSERT(std::string(tensor0.name()) != std::string(tensor1.name()),\n               \"2 tensor name should not same\");\n    GXF_ASSERT(tensor0->shape() == tensor1->shape(), \"2 tensors' shape not matched\");\n    GXF_ASSERT(tensor0->element_type() == tensor1->element_type(),\n               \"2 tensor's element type not matched\");\n    GXF_ASSERT(tensor0->storage_type() == tensor1->storage_type(),\n               \"2 tensor's storage_type not matched\");\n    GXF_ASSERT(tensor0->storage_type() == MemoryStorageType::kHost,\n               \"very tensor storage_type is not from host\");\n\n    float* data0 = tensor0->data<float>().value();\n    float* data1 = tensor1->data<float>().value();\n    uint64_t count = tensor0->element_count();\n    for (uint64_t i = 0; i < count; ++i) {\n      GXF_ASSERT(fequal(data0[i], data1[i]), \"data0[%d]: %f but data1: %f.\", static_cast<int>(i),\n                 data0[i], data1[i]);\n    }\n\n    return GXF_SUCCESS;\n  }\n\n  gxf_result_t registerInterface(Registrar* registrar) override {\n    Expected<void> result;\n    result &= registrar->parameter(rx0_, \"rx0\", \"receiver0 of tensors\", \"\");\n    result &= registrar->parameter(rx1_, \"rx1\", \"receiver1 of tensors\", \"\");\n    return ToResultCode(result);\n  }\n\n private:\n  bool fequal(float a, float b) {\n    if (fabs(a - b) <= std::numeric_limits<float>::epsilon()) return true;\n    return false;\n  }\n  Parameter<Handle<Receiver>> rx0_;\n  Parameter<Handle<Receiver>> rx1_;\n  int32_t count_ = 0;\n};\n\n}  // namespace test\n\n}  // namespace stream\n\n}  // namespace gxf\n\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_STREAM_TESTS_TEST_STREAM_CUDA_HELPER_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/ucx/ucx_common.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_GXF_UCX_UCX_COMMON_HPP_\n#define NVIDIA_GXF_UCX_UCX_COMMON_HPP_\n\n#include <arpa/inet.h> /* inet_addr */\n\n#include <ucp/api/ucp.h>\n#include <queue>\n#include \"gxf/std/allocator.hpp\"\n\n#define DEFAULT_UCX_PORT           13337U\n#define TEST_AM_ID             0\n#define WORKER_PROGRESS_ITERATIONS 5\nnamespace nvidia {\nnamespace gxf {\n\ntypedef struct _ucx_am_data_desc {\n    volatile int    complete;\n    void            *desc;\n    volatile size_t msg_length;\n    volatile size_t header_length;\n    void            *recv_buf;\n    void            *header;\n    volatile size_t num_of_comps;\n    bool            receiving_message;\n    ucs_memory_type_t mem_type;\n} ucx_am_data_desc;\n/**\n * Stream request context. Holds a value to indicate whether or not the\n * request is completed.\n */\ntypedef struct test_req {\n    int complete;\n    char *header;\n} test_req_t;\n\n  typedef enum {\n    INIT,\n    CONNECTED,\n    RESET,\n    CLOSED,\n    RECEIVE_MESSAGE\n  } ConnState;\n\n#define CHECK_RETURN_FAILURE(value) \\\n    if ((value) != GXF_SUCCESS) return (value);\n\ntypedef struct UcxTransmitterSendContext {\n    Entity                  entity;\n    ucp_worker_h            ucp_worker;\n    void*                   request;\n    test_req_t*             ctx;\n    int                     index;\n} UcxTransmitterSendContext_;\n/**\n * Set an address for the server to listen on - INADDR_ANY on a well known port.\n */\nvoid set_sock_addr(const char* address_str, int port, struct sockaddr_storage* saddr);\n\nvoid ep_close(ucp_worker_h ucp_worker, ucp_ep_h ep, uint32_t mode);\n\ngxf_result_t init_worker(ucp_context_h ucp_context, ucp_worker_h* ucp_worker);\n\nucs_memory_type_t ucx_mem_type(MemoryStorageType gxf_mem_type);\n\nucs_status_t request_wait(ucp_worker_h ucp_worker, void* request,\n                                 test_req_t* ctx);\n\ngxf_result_t request_finalize(ucp_worker_h ucp_worker, void* request,\n                                           test_req_t* ctx);\n\nucs_status_t request_wait_once(ucp_worker_h ucp_worker, void* request,\n                                 test_req_t* ctx);\n\nucs_status_t process_request(ucp_worker_h ucp_worker, void* req);\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_EXTENSIONS_UCX_UCX_COMMON_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/ucx/ucx_component_serializer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_UCX_COMPONENT_SERIALIZER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_UCX_COMPONENT_SERIALIZER_HPP_\n\n#include \"common/endian.hpp\"\n#include \"gxf/multimedia/audio.hpp\"\n#include \"gxf/multimedia/video.hpp\"\n#include \"gxf/serialization/component_serializer.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/eos.hpp\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Serializer that supports serializaing Timestamps, Tensors, Video Buffer,\n// Audio Buffer and integer components\n// Valid for sharing data between devices with the same endianness\nclass UcxComponentSerializer : public ComponentSerializer {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override { return GXF_SUCCESS; }\n\n private:\n  // Configures all serializer functions\n  Expected<void> configureSerializers();\n  // Configures all deserializer functions\n  Expected<void> configureDeserializers();\n  // Serializes a nvidia::gxf::Timestamp\n  Expected<size_t> serializeTimestamp(Timestamp timestamp, Endpoint* endpoint);\n  // Deserializes a nvidia::gxf::Timestamp\n  Expected<Timestamp> deserializeTimestamp(Endpoint* endpoint);\n  // Serializes a nvidia::gxf::Tensor\n  Expected<size_t> serializeTensor(const Tensor& tensor, Endpoint* endpoint);\n  // Deserializes a nvidia::gxf::Tensor\n  Expected<Tensor> deserializeTensor(Endpoint* endpoint);\n  // Serializes a nvidia::gxf::VideoBuffer\n  Expected<size_t> serializeVideoBuffer(const VideoBuffer& videoBuffer, Endpoint* endpoint);\n  // Deserializes a nvidia::gxf::VideoBuffer\n  Expected<VideoBuffer> deserializeVideoBuffer(Endpoint* endpoint);\n  // Serializes a nvidia::gxf::AudioBuffer\n  Expected<size_t> serializeAudioBuffer(const AudioBuffer& audioBuffer, Endpoint* endpoint);\n  // Deserializes a nvidia::gxf::AudioBuffer\n  Expected<AudioBuffer> deserializeAudioBuffer(Endpoint* endpoint);\n  // Serializes a nvidia::gxf::EndOfStream\n  Expected<size_t> serializeEndOfStream(EndOfStream& eos, Endpoint* endpoint);\n  // Deserializes a nvidia::gxf::EndOfStream\n  Expected<EndOfStream> deserializeEndOfStream(Endpoint* endpoint);\n  // Serializes an integer\n  template <typename T>\n  Expected<size_t> serializeInteger(T value, Endpoint* endpoint) {\n    if (!endpoint) {\n      return Unexpected{GXF_ARGUMENT_NULL};\n    }\n    T encoded = EncodeLittleEndian(value);\n    return endpoint->writeTrivialType(&encoded);\n  }\n  // Deserializes an integer\n  template <typename T>\n  Expected<T> deserializeInteger(Endpoint* endpoint) {\n    if (!endpoint) {\n      return Unexpected{GXF_ARGUMENT_NULL};\n    }\n    T encoded;\n    return endpoint->readTrivialType(&encoded)\n        .and_then([&]() { return DecodeLittleEndian(encoded); });\n  }\n\n  Parameter<Handle<Allocator>> allocator_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_STD_COMPONENT_SERIALIZER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/ucx/ucx_context.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_UCX_UCX_ROUTER_HPP_\n#define NVIDIA_GXF_UCX_UCX_ROUTER_HPP_\n\n#include <sys/epoll.h>\n#include <sys/eventfd.h>\n#include <ucp/api/ucp.h>\n#include <unistd.h>\n#include <list>\n#include <memory>\n#include <queue>\n\n#include \"gxf/std/network_context.hpp\"\n#include \"gxf/std/queue.hpp\"\n\n#include \"ucx_common.hpp\"\n#include \"ucx_receiver.hpp\"\n#include \"ucx_transmitter.hpp\"\n\n#define kMaxRxContexts 1024\n\nnamespace nvidia {\nnamespace gxf {\n\ntypedef struct ucx_server_ctx {\n    volatile ucp_conn_request_h conn_request;\n    ucp_listener_h              listener;\n    ucp_worker_h                listener_worker;\n    int                         listener_fd;  ///< epoll fd used only when enable_async_ == true\n} ucx_server_ctx_t;\n\n\n\ntypedef struct UcxReceiverContext {\n    UcxReceiver             *rx;\n    ucx_server_ctx_t        server_context;\n    ucp_ep_h                ep;\n    ConnState               conn_state;\n    ucx_am_data_desc        am_data_desc;\n    FixedVector<std::shared_ptr<ucx_am_data_desc>, kMaxRxContexts> headers;\n    ucp_worker_h            ucp_worker;\n    int                     worker_fd;  ///< epoll fd used only when enable_async_ == true\n    int                     index;\n} UcxReceiverContext_;\n\ntypedef struct UcxTransmitterContext {\n    UcxTransmitter          *tx;\n    ucp_ep_h                ep;\n    ucp_worker_h            ucp_worker;\n    bool                    connection_closed;\n    int                     index;\n} UcxTransmitterContext_;\n\ntypedef struct ConnManager {\n    int total;\n    int connected;\n    int closed;\n} ConnManager_;\n\n\n// The class which initializes UCX context and connection worker\nclass UcxContext : public NetworkContext {\n public:\n    gxf_result_t registerInterface(Registrar* registrar) override;\n    gxf_result_t initialize() override;\n    gxf_result_t deinitialize() override;\n    Expected<void> addRoutes(const Entity& entity) override;\n    Expected<void> removeRoutes(const Entity& entity) override;\n\n private:\n    gxf_result_t init_context();\n    gxf_result_t init_tx(Handle<UcxTransmitter> tx);\n    gxf_result_t init_rx(Handle<UcxReceiver> rx);\n\n    /// @brief start server for the case with enable_async_ == false\n    void start_server();\n\n    gxf_result_t am_desc_to_iov(std::shared_ptr<UcxReceiverContext> rx_context);\n    void destroy_rx_contexts();\n    void destroy_tx_contexts();\n\n    // The remaining methods are specific to the case with enable_async_ == true\n\n    /// @brief start server for the case with enable_async_ == true\n    void start_server_async_queue();\n    /// @brief method run by tx_thread_ when enable_async_ == true\n    void poll_queue();\n    gxf_result_t wait_for_event();   ///< primary function called by start_server_async_queue\n    gxf_result_t epoll_add_worker(std::shared_ptr<UcxReceiverContext> rx_context,\n            bool is_listener);\n    gxf_result_t progress_work(std::shared_ptr<UcxReceiverContext> rx_context);\n    gxf_result_t server_create_ep(std::shared_ptr<UcxReceiverContext> rx_context);\n    gxf_result_t init_connection(std::shared_ptr<UcxReceiverContext> rx_context);\n    gxf_result_t handle_connections_after_recv();\n    gxf_result_t gxf_arm_worker(std::shared_ptr<UcxReceiverContext> rx_context,\n            bool is_listener);\n    void copy_header_to_am_desc(std::shared_ptr<UcxReceiverContext> rx_context);\n\n    bool close_server_loop_;\n    FixedVector<std::shared_ptr<UcxReceiverContext>, kMaxRxContexts> rx_contexts_;\n    FixedVector<std::shared_ptr<UcxTransmitterContext>, kMaxRxContexts> tx_contexts_;\n    ucp_context_h ucp_context_;\n    Parameter<Handle<EntitySerializer>> entity_serializer_;\n    Parameter<bool> reconnect_;\n    Resource<Handle<GPUDevice>> gpu_device_;\n    Parameter<bool> cpu_data_only_;\n    Parameter<bool> enable_async_;\n    int32_t dev_id_ = 0;\n\n    std::thread t_;  ///< server thread used only when enable_async_ == false\n\n    // The following data members are only used when enable_async_ == true\n    ConnManager rx_conns_ = {0};\n    std::thread rx_thread_;  ///< async receiver thread used only when enable_async_ == true\n    std::thread tx_thread_;  ///< async transmitter thread used only when enable_async_ == true\n    std::list<UcxTransmitterSendContext_> pending_send_requests_;\n    std::mutex mtx_;\n    std::condition_variable cv_;\n    bool areTransmittersDone = false;\n    int epoll_fd_;\n    int efd_signal_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/ucx/ucx_entity_serializer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_UCX_ENTITY_SERIALIZER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_UCX_ENTITY_SERIALIZER_HPP_\n\n#define kMaxTempComponents 1024\n\n#include <unordered_map>\n\n#include \"common/fixed_vector.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/serialization/component_serializer.hpp\"\n#include \"gxf/serialization/entity_serializer.hpp\"\n#include \"gxf/serialization/tid_hash.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Serializes and deserializes entities with the provided component serializers\n// Little-endian is used over big-endian for better performance on x86 and arm platforms\n// Entities are serialized in the following format:\n//\n//   | Entity Header | Component Header | Component Metadata | Component Header | ... | ... |\n//\n// Components will be serialized in the order they are added to the entity\n// Components without serializers will be skipped\n// Each component will be preceded by a component header and the name of the component\n// The component itself will be serialized with a component serializer\n// An entity header will be added at the beginning\nclass UcxEntitySerializer : public EntitySerializer {\n public:\n  #pragma pack(push, 1)\n  // Header preceding entities\n  struct EntityHeader {\n    uint64_t serialized_size;  // Size of the serialized entity in bytes\n    uint64_t sequence_number;  // Sequence number of the message\n    uint64_t component_count;  // Number of components in the entity\n  };\n  #pragma pack(pop)\n\n  #pragma pack(push, 1)\n  // Header preceding components\n  struct ComponentHeader {\n    uint64_t serialized_size;  // Size of the serialized component in bytes\n    gxf_tid_t tid;             // Type ID of the component\n    char name[kMaxComponentNameSize];            // Component name\n  };\n  #pragma pack(pop)\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override { return GXF_SUCCESS; }\n  gxf_result_t deinitialize() override { return GXF_SUCCESS; }\n\n  gxf_result_t serialize_entity_abi(gxf_uid_t eid, Endpoint* endpoint, uint64_t* size) override;\n  gxf_result_t deserialize_entity_abi(gxf_uid_t eid, Endpoint* endpoint) override;\n  Expected<Entity> deserialize_entity_header_abi(Endpoint* endpoint) override;\n\n  Expected<void> add_serializer(const Handle<ComponentSerializer>& serializer);\n\n private:\n  // Structure used to organize serializable components\n  struct ComponentEntry;\n\n  using ComponentSerializerList = FixedVector<Handle<ComponentSerializer>, kMaxTempComponents>;\n\n  // Populates a list of component entries using a list of component handles\n  Expected<FixedVector<ComponentEntry, kMaxTempComponents>> createComponentEntries(\n      const FixedVector<UntypedHandle, kMaxComponents>& components);\n  // Serializes a list of components and writes them to an endpoint\n  // Returns the total number of bytes serialized\n  Expected<size_t> serializeComponents(const FixedVector<ComponentEntry,\n                                       kMaxTempComponents>& entries,\n                                       Endpoint* endpoint);\n  // Reads from an endpoint and deserializes a list of components\n  Expected<void> deserializeComponents(size_t component_count, Entity entity, Endpoint* endpoint);\n  // Searches for a component serializer that supports the given type ID\n  // Uses the first valid serializer found and caches it for subsequent lookups\n  // Returns an Unexpected if no valid serializer is found\n  Expected<Handle<ComponentSerializer>> findComponentSerializer(gxf_tid_t tid);\n\n  Parameter<ComponentSerializerList> component_serializers_;\n  Parameter<bool> verbose_warning_;\n\n  // Table that caches type ID with a valid component serializer\n  std::unordered_map<gxf_tid_t, Handle<ComponentSerializer>, TidHash> serializer_cache_;\n  // Sequence number for outgoing messages\n  uint64_t outgoing_sequence_number_{0};\n  // Sequence number for incoming messages\n  uint64_t incoming_sequence_number_{0};\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_STD_ENTITY_SERIALIZER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/ucx/ucx_receiver.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <ucp/api/ucp.h>\n#include <deque>\n#include <list>\n#include <memory>\n#include <queue>\n#include <string>\n#include <thread>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"gxf/std/gems/staging_queue/staging_queue.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/resources.hpp\"\n\n#include \"ucx_common.hpp\"\n#include \"ucx_serialization_buffer.hpp\"\n\n#ifndef NVIDIA_GXF_UCX_UCX_RECEIVER_HPP_\n#define NVIDIA_GXF_UCX_UCX_RECEIVER_HPP_\n\nnamespace nvidia {\nnamespace gxf {\n\n\n// Codelet that functions as a receiver on a UCX connection\nclass UcxReceiver : public Receiver {\n public:\n  using queue_t = ::gxf::staging_queue::StagingQueue<Entity>;\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n  gxf_result_t init_context(ucp_worker_h  ucp_worker,\n                            ucx_am_data_desc* am_data_desc, int fd,\n                            bool cpu_data_only, bool enable_async);\n\n  gxf_result_t pop_abi(gxf_uid_t* uid) override;\n\n  gxf_result_t push_abi(gxf_uid_t other) override;\n\n  gxf_result_t peek_abi(gxf_uid_t* uid, int32_t index) override;\n\n  gxf_result_t peek_back_abi(gxf_uid_t* uid, int32_t index) override;\n\n  size_t capacity_abi() override;\n\n  size_t size_abi() override;\n\n  gxf_result_t receive_abi(gxf_uid_t* uid) override;\n\n  size_t back_size_abi() override;\n\n  gxf_result_t sync_abi() override;\n\n  gxf_result_t sync_io_abi() override;\n\n  gxf_result_t wait_abi() override;\n  const char* get_addr();\n\n  int get_port();\n\n  Expected<void> set_serialization_buffer(Handle<UcxSerializationBuffer> buffer);\n\n  Expected<void> set_port(int port);\n\n  Parameter<uint64_t> capacity_;\n  Parameter<uint64_t> policy_;\n  Parameter<std::string> address_;\n  Parameter<uint32_t> port_;\n  Resource<Handle<GPUDevice>> gpu_device_;\n  Parameter<Handle<UcxSerializationBuffer>> buffer_;\n\n private:\n  gxf_result_t receive_message();\n\n  /// @brief finalize request when enable_async_ == false\n  gxf_result_t request_finalize_sync(ucp_worker_h ucp_worker, test_req_t* request,\n                                     test_req_t* ctx);\n\n  ucp_worker_h     ucp_worker_;\n  ucx_am_data_desc* am_data_desc_;\n  std::unique_ptr<queue_t> queue_;\n  int32_t dev_id_ = 0;\n  int efd_signal_;\n  bool cpu_data_only_ = false;\n  std::list<std::pair<void*, test_req_t*>> requests;\n  int enable_async_ = true;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_EXTENSIONS_UCX_UCX_RECEIVER_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/ucx/ucx_serialization_buffer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_GXF_SERIALIZATION_UCX_SERIALIZATION_BUFFER_HPP_\n#define NVIDIA_GXF_SERIALIZATION_UCX_SERIALIZATION_BUFFER_HPP_\n\n#include <mutex>\n#include <vector>\n\n#include \"gxf/serialization/endpoint.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/memory_buffer.hpp\"\n\n#include \"ucp/api/ucp.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Buffer to hold serialized data\nclass UcxSerializationBuffer : public Endpoint {\n public:\n  gxf_result_t registerInterface(Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override { return ToResultCode(buffer_.freeBuffer()); }\n\n  gxf_result_t write_abi(const void* data, size_t size, size_t* bytes_written) override;\n  gxf_result_t read_abi(void* data, size_t size, size_t* bytes_read) override;\n\n  gxf_result_t write_ptr_abi(const void* pointer, size_t size, MemoryStorageType type) override;\n\n  // Resizes the buffer\n  gxf::Expected<void> resize(size_t size);\n\n  // Set allocator\n  Expected<void> set_allocator(Handle<Allocator> allocator);\n\n  // Returns a read-only pointer to buffer data\n  const byte* data() const { return buffer_.pointer(); }\n  const ucp_dt_iov_t* iov_buffer() const { return iov_buffers_.data(); }\n  const size_t iov_buffer_size() const { return iov_buffers_.size(); }\n  const ucs_memory_type_t mem_type() const {return mem_type_; }\n  // Returns the capacity of the buffer\n  size_t capacity() const { return buffer_.size(); }\n  // Returns the number of bytes written to the buffer\n  size_t size() const;\n  // Resets buffer for sequential access\n  void reset();\n  uint64_t sequence_number_ = 0;\n\n private:\n  Parameter<Handle<Allocator>> allocator_;\n  Parameter<size_t> buffer_size_;\n  // Vector for ucp_dt_iov_t\n  std::vector<ucp_dt_iov_t> iov_buffers_;\n  // UCX memory type of component\n  ucs_memory_type_t mem_type_;\n  // Data buffer\n  MemoryBuffer buffer_;\n  // Offset for sequential writes\n  size_t write_offset_;\n  // Offset for sequential reads\n  size_t read_offset_;\n  // Mutex to guard concurrent access\n  mutable std::mutex mutex_;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_SERIALIZATION_UCX_SERIALIZATION_BUFFER_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/gxf/ucx/ucx_transmitter.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <condition_variable>\n#include <list>\n#include <memory>\n#include <queue>\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#include \"ucp/api/ucp.h\"\n\n#include \"gxf/std/gems/staging_queue/staging_queue.hpp\"\n#include \"gxf/std/resources.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n#include \"ucx_common.hpp\"\n#include \"ucx_entity_serializer.hpp\"\n#include \"ucx_serialization_buffer.hpp\"\n\n#ifndef NVIDIA_GXF_UCX_UCX_TRANSMITTER_HPP_\n#define NVIDIA_GXF_UCX_UCX_TRANSMITTER_HPP_\n\nnamespace nvidia {\nnamespace gxf {\n\n// Codelet that functions as a transmitter in a UCX connection\nclass UcxTransmitter : public Transmitter {\n public:\n  using queue_t = ::gxf::staging_queue::StagingQueue<Entity>;\n\n  gxf_result_t init_context(ucp_context_h ucp_context,\n                            Handle<EntitySerializer> serializer,\n                            ucp_worker_h ucp_worker,\n                            ucp_ep_h* ep,\n                            bool* connection_closed_p,\n                            bool reconnect,\n                            bool cpu_data_only,\n                            bool enable_async,\n                            std::list<UcxTransmitterSendContext_>* send_queue,\n                            std::condition_variable* cv,\n                            std::mutex* mtx);\n\n  gxf_result_t registerInterface(Registrar* registrar) override;\n\n  gxf_result_t initialize() override;\n\n  gxf_result_t deinitialize() override;\n\n  gxf_result_t pop_abi(gxf_uid_t* uid) override;\n\n  gxf_result_t pop_io_abi(gxf_uid_t* uid) override;\n\n  gxf_result_t push_abi(gxf_uid_t other) override;\n\n  gxf_result_t peek_abi(gxf_uid_t* uid, int32_t index) override;\n\n  size_t capacity_abi() override;\n\n  size_t size_abi() override;\n\n  gxf_result_t publish_abi(gxf_uid_t uid) override;\n\n  size_t back_size_abi() override;\n\n  gxf_result_t sync_abi() override;\n\n  gxf_result_t sync_io_abi() override;\n\n  Expected<void> set_port(int port);\n\n  Expected<void> set_serialization_buffer(Handle<UcxSerializationBuffer> buffer);\n\n  Parameter<uint64_t> capacity_;\n  Parameter<uint64_t> policy_;\n\n private:\n  gxf_result_t send_am(Entity& entity);\n  gxf_result_t create_client_connection();\n  gxf_result_t create_client_connection_with_retries();\n  gxf_result_t check_connection_and_connect();\n\n  ucp_worker_h  ucp_worker_;\n  ucp_ep_h* ep_;\n  Parameter<std::string> receiver_address_;\n  Parameter<std::string> local_address_;\n  Parameter<uint32_t> port_;\n  Parameter<uint32_t> local_port_;\n  Parameter<uint32_t> maximum_connection_retries_;\n  Resource<Handle<GPUDevice>> gpu_device_;\n  int32_t dev_id_ = 0;\n  Parameter<Handle<UcxSerializationBuffer>> buffer_;\n\n  Handle<EntitySerializer> entity_serializer_;\n  std::unique_ptr<queue_t> queue_;\n  bool* connection_closed_p_;\n  bool reconnect_;\n  bool cpu_data_only_;\n  std::list<UcxTransmitterSendContext_>* send_queue_;\n  std::condition_variable* cv_;\n  std::mutex* mtx_;\n  int index = 0;\n  int* id_;\n  int enable_async_ = true;\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n\n#endif  // NVIDIA_GXF_EXTENSIONS_UCX_UCX_TRANSMITTER_HPP\n"
  },
  {
    "path": "isaac_ros_gxf/gxf/core/include/third_party/LICENSE.txt",
    "content": "================================================================================\nThis project uses the following software and the following licensing terms\napply to all the extensions and its associated files used to interact with the\ngraph composer tools suite that includes composer, registry, container builder\nand gxe.\n\n================================================================================\npython 3.6.9 (Provided under following license)\n================================================================================\n\nPython was created in the early 1990s by Guido van Rossum at Stichting Mathematisch Centrum (CWI, see https://www.cwi.nl/) in the Netherlands as a successor of a language called ABC. Guido remains Python’s principal author, although it includes many contributions from others.\n\nIn 1995, Guido continued his work on Python at the Corporation for National Research Initiatives (CNRI, see https://www.cnri.reston.va.us/) in Reston, Virginia where he released several versions of the software.\n\nIn May 2000, Guido and the Python core development team moved to BeOpen.com to form the BeOpen PythonLabs team. In October of the same year, the PythonLabs team moved to Digital Creations (now Zope Corporation; see https://www.zope.org/). In 2001, the Python Software Foundation (PSF, see https://www.python.org/psf/) was formed, a non-profit organization created specifically to own Python-related Intellectual Property. Zope Corporation is a sponsoring member of the PSF.\n\nAll Python releases are Open Source (see https://opensource.org/ for the Open Source Definition). Historically, most, but not all, Python releases have also been GPL-compatible; the table below summarizes the various releases.\n\n\n    Release         Derived     Year        Owner       GPL-\n                    from                                compatible? (1)\n\n    0.9.0 thru 1.2              1991-1995   CWI         yes\n    1.3 thru 1.5.2  1.2         1995-1999   CNRI        yes\n    1.6             1.5.2       2000        CNRI        no\n    2.0             1.6         2000        BeOpen.com  no\n    1.6.1           1.6         2001        CNRI        yes (2)\n    2.1             2.0+1.6.1   2001        PSF         no\n    2.0.1           2.0+1.6.1   2001        PSF         yes\n    2.1.1           2.1+2.0.1   2001        PSF         yes\n    2.1.2           2.1.1       2002        PSF         yes\n    2.1.3           2.1.2       2002        PSF         yes\n    2.2 and above   2.1.1       2001-now    PSF         yes\n\nNote\n\nGPL-compatible doesn’t mean that we’re distributing Python under the GPL. All Python licenses, unlike the GPL, let you distribute a modified version without making your changes open source. The GPL-compatible licenses make it possible to combine Python with other software that is released under the GPL; the others don’t.\n\nThanks to the many outside volunteers who have worked under Guido’s direction to make these releases possible.\nTerms and conditions for accessing or otherwise using Python\nPSF LICENSE AGREEMENT FOR PYTHON 3.6.13\n\n1. This LICENSE AGREEMENT is between the Python Software Foundation (\"PSF\"), and\n   the Individual or Organization (\"Licensee\") accessing and otherwise using Python\n   3.6.13 software in source or binary form and its associated documentation.\n\n2. Subject to the terms and conditions of this License Agreement, PSF hereby\n   grants Licensee a nonexclusive, royalty-free, world-wide license to reproduce,\n   analyze, test, perform and/or display publicly, prepare derivative works,\n   distribute, and otherwise use Python 3.6.13 alone or in any derivative\n   version, provided, however, that PSF's License Agreement and PSF's notice of\n   copyright, i.e., \"Copyright © 2001-2021 Python Software Foundation; All Rights\n   Reserved\" are retained in Python 3.6.13 alone or in any derivative version\n   prepared by Licensee.\n\n3. In the event Licensee prepares a derivative work that is based on or\n   incorporates Python 3.6.13 or any part thereof, and wants to make the\n   derivative work available to others as provided herein, then Licensee hereby\n   agrees to include in any such work a brief summary of the changes made to Python\n   3.6.13.\n\n4. PSF is making Python 3.6.13 available to Licensee on an \"AS IS\" basis.\n   PSF MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.  BY WAY OF\n   EXAMPLE, BUT NOT LIMITATION, PSF MAKES NO AND DISCLAIMS ANY REPRESENTATION OR\n   WARRANTY OF MERCHANTABILITY OR FITNESS FOR ANY PARTICULAR PURPOSE OR THAT THE\n   USE OF PYTHON 3.6.13 WILL NOT INFRINGE ANY THIRD PARTY RIGHTS.\n\n5. PSF SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF PYTHON 3.6.13\n   FOR ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR LOSS AS A RESULT OF\n   MODIFYING, DISTRIBUTING, OR OTHERWISE USING PYTHON 3.6.13, OR ANY DERIVATIVE\n   THEREOF, EVEN IF ADVISED OF THE POSSIBILITY THEREOF.\n\n6. This License Agreement will automatically terminate upon a material breach of\n   its terms and conditions.\n\n7. Nothing in this License Agreement shall be deemed to create any relationship\n   of agency, partnership, or joint venture between PSF and Licensee.  This License\n   Agreement does not grant permission to use PSF trademarks or trade name in a\n   trademark sense to endorse or promote products or services of Licensee, or any\n   third party.\n\n8. By copying, installing or otherwise using Python 3.6.13, Licensee agrees\n   to be bound by the terms and conditions of this License Agreement.\n\nBEOPEN.COM LICENSE AGREEMENT FOR PYTHON 2.0\n\nBEOPEN PYTHON OPEN SOURCE LICENSE AGREEMENT VERSION 1\n\n1. This LICENSE AGREEMENT is between BeOpen.com (\"BeOpen\"), having an office at\n   160 Saratoga Avenue, Santa Clara, CA 95051, and the Individual or Organization\n   (\"Licensee\") accessing and otherwise using this software in source or binary\n   form and its associated documentation (\"the Software\").\n\n2. Subject to the terms and conditions of this BeOpen Python License Agreement,\n   BeOpen hereby grants Licensee a non-exclusive, royalty-free, world-wide license\n   to reproduce, analyze, test, perform and/or display publicly, prepare derivative\n   works, distribute, and otherwise use the Software alone or in any derivative\n   version, provided, however, that the BeOpen Python License is retained in the\n   Software, alone or in any derivative version prepared by Licensee.\n\n3. BeOpen is making the Software available to Licensee on an \"AS IS\" basis.\n   BEOPEN MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.  BY WAY OF\n   EXAMPLE, BUT NOT LIMITATION, BEOPEN MAKES NO AND DISCLAIMS ANY REPRESENTATION OR\n   WARRANTY OF MERCHANTABILITY OR FITNESS FOR ANY PARTICULAR PURPOSE OR THAT THE\n   USE OF THE SOFTWARE WILL NOT INFRINGE ANY THIRD PARTY RIGHTS.\n\n4. BEOPEN SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF THE SOFTWARE FOR\n   ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR LOSS AS A RESULT OF USING,\n   MODIFYING OR DISTRIBUTING THE SOFTWARE, OR ANY DERIVATIVE THEREOF, EVEN IF\n   ADVISED OF THE POSSIBILITY THEREOF.\n\n5. This License Agreement will automatically terminate upon a material breach of\n   its terms and conditions.\n\n6. This License Agreement shall be governed by and interpreted in all respects\n   by the law of the State of California, excluding conflict of law provisions.\n   Nothing in this License Agreement shall be deemed to create any relationship of\n   agency, partnership, or joint venture between BeOpen and Licensee.  This License\n   Agreement does not grant permission to use BeOpen trademarks or trade names in a\n   trademark sense to endorse or promote products or services of Licensee, or any\n   third party.  As an exception, the \"BeOpen Python\" logos available at\n   http://www.pythonlabs.com/logos.html may be used according to the permissions\n   granted on that web page.\n\n7. By copying, installing or otherwise using the software, Licensee agrees to be\n   bound by the terms and conditions of this License Agreement.\n\nCNRI LICENSE AGREEMENT FOR PYTHON 1.6.1\n\n1. This LICENSE AGREEMENT is between the Corporation for National Research\n   Initiatives, having an office at 1895 Preston White Drive, Reston, VA 20191\n   (\"CNRI\"), and the Individual or Organization (\"Licensee\") accessing and\n   otherwise using Python 1.6.1 software in source or binary form and its\n   associated documentation.\n\n2. Subject to the terms and conditions of this License Agreement, CNRI hereby\n   grants Licensee a nonexclusive, royalty-free, world-wide license to reproduce,\n   analyze, test, perform and/or display publicly, prepare derivative works,\n   distribute, and otherwise use Python 1.6.1 alone or in any derivative version,\n   provided, however, that CNRI's License Agreement and CNRI's notice of copyright,\n   i.e., \"Copyright © 1995-2001 Corporation for National Research Initiatives; All\n   Rights Reserved\" are retained in Python 1.6.1 alone or in any derivative version\n   prepared by Licensee.  Alternately, in lieu of CNRI's License Agreement,\n   Licensee may substitute the following text (omitting the quotes): \"Python 1.6.1\n   is made available subject to the terms and conditions in CNRI's License\n   Agreement.  This Agreement together with Python 1.6.1 may be located on the\n   Internet using the following unique, persistent identifier (known as a handle):\n   1895.22/1013.  This Agreement may also be obtained from a proxy server on the\n   Internet using the following URL: http://hdl.handle.net/1895.22/1013.\";\n\n3. In the event Licensee prepares a derivative work that is based on or\n   incorporates Python 1.6.1 or any part thereof, and wants to make the derivative\n   work available to others as provided herein, then Licensee hereby agrees to\n   include in any such work a brief summary of the changes made to Python 1.6.1.\n\n4. CNRI is making Python 1.6.1 available to Licensee on an \"AS IS\" basis.  CNRI\n   MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.  BY WAY OF EXAMPLE,\n   BUT NOT LIMITATION, CNRI MAKES NO AND DISCLAIMS ANY REPRESENTATION OR WARRANTY\n   OF MERCHANTABILITY OR FITNESS FOR ANY PARTICULAR PURPOSE OR THAT THE USE OF\n   PYTHON 1.6.1 WILL NOT INFRINGE ANY THIRD PARTY RIGHTS.\n\n5. CNRI SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF PYTHON 1.6.1 FOR\n   ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR LOSS AS A RESULT OF\n   MODIFYING, DISTRIBUTING, OR OTHERWISE USING PYTHON 1.6.1, OR ANY DERIVATIVE\n   THEREOF, EVEN IF ADVISED OF THE POSSIBILITY THEREOF.\n\n6. This License Agreement will automatically terminate upon a material breach of\n   its terms and conditions.\n\n7. This License Agreement shall be governed by the federal intellectual property\n   law of the United States, including without limitation the federal copyright\n   law, and, to the extent such U.S. federal law does not apply, by the law of the\n   Commonwealth of Virginia, excluding Virginia's conflict of law provisions.\n   Notwithstanding the foregoing, with regard to derivative works based on Python\n   1.6.1 that incorporate non-separable material that was previously distributed\n   under the GNU General Public License (GPL), the law of the Commonwealth of\n   Virginia shall govern this License Agreement only as to issues arising under or\n   with respect to Paragraphs 4, 5, and 7 of this License Agreement.  Nothing in\n   this License Agreement shall be deemed to create any relationship of agency,\n   partnership, or joint venture between CNRI and Licensee.  This License Agreement\n   does not grant permission to use CNRI trademarks or trade name in a trademark\n   sense to endorse or promote products or services of Licensee, or any third\n   party.\n\n8. By clicking on the \"ACCEPT\" button where indicated, or by copying, installing\n   or otherwise using Python 1.6.1, Licensee agrees to be bound by the terms and\n   conditions of this License Agreement.\n\nCWI LICENSE AGREEMENT FOR PYTHON 0.9.0 THROUGH 1.2\n\nCopyright © 1991 - 1995, Stichting Mathematisch Centrum Amsterdam, The\nNetherlands.  All rights reserved.\n\nPermission to use, copy, modify, and distribute this software and its\ndocumentation for any purpose and without fee is hereby granted, provided that\nthe above copyright notice appear in all copies and that both that copyright\nnotice and this permission notice appear in supporting documentation, and that\nthe name of Stichting Mathematisch Centrum or CWI not be used in advertising or\npublicity pertaining to distribution of the software without specific, written\nprior permission.\n\nSTICHTING MATHEMATISCH CENTRUM DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS\nSOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO\nEVENT SHALL STICHTING MATHEMATISCH CENTRUM BE LIABLE FOR ANY SPECIAL, INDIRECT\nOR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE,\nDATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS\nACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS\nSOFTWARE.\n\nLicenses and Acknowledgements for Incorporated Software\n\nThis section is an incomplete, but growing list of licenses and acknowledgements for third-party software incorporated in the Python distribution.\nMersenne Twister\n\nThe _random module includes code based on a download from http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/MT2002/emt19937ar.html. The following are the verbatim comments from the original code:\n\nA C-program for MT19937, with initialization improved 2002/1/26.\nCoded by Takuji Nishimura and Makoto Matsumoto.\n\nBefore using, initialize the state by using init_genrand(seed)\nor init_by_array(init_key, key_length).\n\nCopyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions\nare met:\n\n 1. Redistributions of source code must retain the above copyright\n    notice, this list of conditions and the following disclaimer.\n\n 2. Redistributions in binary form must reproduce the above copyright\n    notice, this list of conditions and the following disclaimer in the\n    documentation and/or other materials provided with the distribution.\n\n 3. The names of its contributors may not be used to endorse or promote\n    products derived from this software without specific prior written\n    permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR\nA PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR\nCONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,\nEXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,\nPROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\nPROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\nLIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\nNEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\nSOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n\nAny feedback is very welcome.\nhttp://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/emt.html\nemail: m-mat @ math.sci.hiroshima-u.ac.jp (remove space)\n\nSockets\n\nThe socket module uses the functions, getaddrinfo(), and getnameinfo(), which are coded in separate source files from the WIDE Project, http://www.wide.ad.jp/.\n\nCopyright (C) 1995, 1996, 1997, and 1998 WIDE Project.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions\nare met:\n1. Redistributions of source code must retain the above copyright\n   notice, this list of conditions and the following disclaimer.\n2. Redistributions in binary form must reproduce the above copyright\n   notice, this list of conditions and the following disclaimer in the\n   documentation and/or other materials provided with the distribution.\n3. Neither the name of the project nor the names of its contributors\n   may be used to endorse or promote products derived from this software\n   without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND\nANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\nARE DISCLAIMED.  IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS\nOR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)\nHOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\nLIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY\nOUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF\nSUCH DAMAGE.\n\nFloating point exception control\n\nThe source for the fpectl module includes the following notice:\n\n  ---------------------------------------------------------------------\n /                       Copyright (c) 1996.                           \\\n|          The Regents of the University of California.                 |\n|                        All rights reserved.                           |\n|                                                                       |\n|   Permission to use, copy, modify, and distribute this software for   |\n|   any purpose without fee is hereby granted, provided that this en-   |\n|   tire notice is included in all copies of any software which is or   |\n|   includes  a  copy  or  modification  of  this software and in all   |\n|   copies of the supporting documentation for such software.           |\n|                                                                       |\n|   This  work was produced at the University of California, Lawrence   |\n|   Livermore National Laboratory under  contract  no.  W-7405-ENG-48   |\n|   between  the  U.S.  Department  of  Energy and The Regents of the   |\n|   University of California for the operation of UC LLNL.              |\n|                                                                       |\n|                              DISCLAIMER                               |\n|                                                                       |\n|   This  software was prepared as an account of work sponsored by an   |\n|   agency of the United States Government. Neither the United States   |\n|   Government  nor the University of California nor any of their em-   |\n|   ployees, makes any warranty, express or implied, or  assumes  any   |\n|   liability  or  responsibility  for the accuracy, completeness, or   |\n|   usefulness of any information,  apparatus,  product,  or  process   |\n|   disclosed,   or  represents  that  its  use  would  not  infringe   |\n|   privately-owned rights. Reference herein to any specific  commer-   |\n|   cial  products,  process,  or  service  by trade name, trademark,   |\n|   manufacturer, or otherwise, does not  necessarily  constitute  or   |\n|   imply  its endorsement, recommendation, or favoring by the United   |\n|   States Government or the University of California. The views  and   |\n|   opinions  of authors expressed herein do not necessarily state or   |\n|   reflect those of the United States Government or  the  University   |\n|   of  California,  and shall not be used for advertising or product   |\n \\  endorsement purposes.                                              /\n  ---------------------------------------------------------------------\n\nAsynchronous socket services\n\nThe asynchat and asyncore modules contain the following notice:\n\nCopyright 1996 by Sam Rushing\n\n                        All Rights Reserved\n\nPermission to use, copy, modify, and distribute this software and\nits documentation for any purpose and without fee is hereby\ngranted, provided that the above copyright notice appear in all\ncopies and that both that copyright notice and this permission\nnotice appear in supporting documentation, and that the name of Sam\nRushing not be used in advertising or publicity pertaining to\ndistribution of the software without specific, written prior\npermission.\n\nSAM RUSHING DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,\nINCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN\nNO EVENT SHALL SAM RUSHING BE LIABLE FOR ANY SPECIAL, INDIRECT OR\nCONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS\nOF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,\nNEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN\nCONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.\n\nCookie management\n\nThe http.cookies module contains the following notice:\n\nCopyright 2000 by Timothy O'Malley <timo@alum.mit.edu>\n\n               All Rights Reserved\n\nPermission to use, copy, modify, and distribute this software\nand its documentation for any purpose and without fee is hereby\ngranted, provided that the above copyright notice appear in all\ncopies and that both that copyright notice and this permission\nnotice appear in supporting documentation, and that the name of\nTimothy O'Malley  not be used in advertising or publicity\npertaining to distribution of the software without specific, written\nprior permission.\n\nTimothy O'Malley DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS\nSOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY\nAND FITNESS, IN NO EVENT SHALL Timothy O'Malley BE LIABLE FOR\nANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES\nWHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,\nWHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS\nACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR\nPERFORMANCE OF THIS SOFTWARE.\n\nExecution tracing\n\nThe trace module contains the following notice:\n\nportions copyright 2001, Autonomous Zones Industries, Inc., all rights...\nerr...  reserved and offered to the public under the terms of the\nPython 2.2 license.\nAuthor: Zooko O'Whielacronx\nhttp://zooko.com/\nmailto:zooko@zooko.com\n\nCopyright 2000, Mojam Media, Inc., all rights reserved.\nAuthor: Skip Montanaro\n\nCopyright 1999, Bioreason, Inc., all rights reserved.\nAuthor: Andrew Dalke\n\nCopyright 1995-1997, Automatrix, Inc., all rights reserved.\nAuthor: Skip Montanaro\n\nCopyright 1991-1995, Stichting Mathematisch Centrum, all rights reserved.\n\n\nPermission to use, copy, modify, and distribute this Python software and\nits associated documentation for any purpose without fee is hereby\ngranted, provided that the above copyright notice appears in all copies,\nand that both that copyright notice and this permission notice appear in\nsupporting documentation, and that the name of neither Automatrix,\nBioreason or Mojam Media be used in advertising or publicity pertaining to\ndistribution of the software without specific, written prior permission.\n\nUUencode and UUdecode functions\n\nThe uu module contains the following notice:\n\nCopyright 1994 by Lance Ellinghouse\nCathedral City, California Republic, United States of America.\n                       All Rights Reserved\nPermission to use, copy, modify, and distribute this software and its\ndocumentation for any purpose and without fee is hereby granted,\nprovided that the above copyright notice appear in all copies and that\nboth that copyright notice and this permission notice appear in\nsupporting documentation, and that the name of Lance Ellinghouse\nnot be used in advertising or publicity pertaining to distribution\nof the software without specific, written prior permission.\nLANCE ELLINGHOUSE DISCLAIMS ALL WARRANTIES WITH REGARD TO\nTHIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND\nFITNESS, IN NO EVENT SHALL LANCE ELLINGHOUSE CENTRUM BE LIABLE\nFOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES\nWHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN\nACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT\nOF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.\n\nModified by Jack Jansen, CWI, July 1995:\n- Use binascii module to do the actual line-by-line conversion\n  between ascii and binary. This results in a 1000-fold speedup. The C\n  version is still 5 times faster, though.\n- Arguments more compliant with Python standard\n\nXML Remote Procedure Calls\n\nThe xmlrpc.client module contains the following notice:\n\n    The XML-RPC client interface is\n\nCopyright (c) 1999-2002 by Secret Labs AB\nCopyright (c) 1999-2002 by Fredrik Lundh\n\nBy obtaining, using, and/or copying this software and/or its\nassociated documentation, you agree that you have read, understood,\nand will comply with the following terms and conditions:\n\nPermission to use, copy, modify, and distribute this software and\nits associated documentation for any purpose and without fee is\nhereby granted, provided that the above copyright notice appears in\nall copies, and that both that copyright notice and this permission\nnotice appear in supporting documentation, and that the name of\nSecret Labs AB or the author not be used in advertising or publicity\npertaining to distribution of the software without specific, written\nprior permission.\n\nSECRET LABS AB AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD\nTO THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANT-\nABILITY AND FITNESS.  IN NO EVENT SHALL SECRET LABS AB OR THE AUTHOR\nBE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY\nDAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,\nWHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS\nACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE\nOF THIS SOFTWARE.\n\ntest_epoll\n\nThe test_epoll module contains the following notice:\n\nCopyright (c) 2001-2006 Twisted Matrix Laboratories.\n\nPermission is hereby granted, free of charge, to any person obtaining\na copy of this software and associated documentation files (the\n\"Software\"), to deal in the Software without restriction, including\nwithout limitation the rights to use, copy, modify, merge, publish,\ndistribute, sublicense, and/or sell copies of the Software, and to\npermit persons to whom the Software is furnished to do so, subject to\nthe following conditions:\n\nThe above copyright notice and this permission notice shall be\nincluded in all copies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND,\nEXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF\nMERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND\nNONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE\nLIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION\nOF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION\nWITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.\n\nSelect kqueue\n\nThe select module contains the following notice for the kqueue interface:\n\nCopyright (c) 2000 Doug White, 2006 James Knight, 2007 Christian Heimes\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions\nare met:\n1. Redistributions of source code must retain the above copyright\n   notice, this list of conditions and the following disclaimer.\n2. Redistributions in binary form must reproduce the above copyright\n   notice, this list of conditions and the following disclaimer in the\n   documentation and/or other materials provided with the distribution.\n\nTHIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND\nANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\nARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS\nOR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)\nHOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\nLIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY\nOUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF\nSUCH DAMAGE.\n\nSipHash24\n\nThe file Python/pyhash.c contains Marek Majkowski’ implementation of Dan Bernstein’s SipHash24 algorithm. The contains the following note:\n\n<MIT License>\nCopyright (c) 2013  Marek Majkowski <marek@popcount.org>\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in\nall copies or substantial portions of the Software.\n</MIT License>\n\nOriginal location:\n   https://github.com/majek/csiphash/\n\nSolution inspired by code from:\n   Samuel Neves (supercop/crypto_auth/siphash24/little)\n   djb (supercop/crypto_auth/siphash24/little2)\n   Jean-Philippe Aumasson (https://131002.net/siphash/siphash24.c)\n\nstrtod and dtoa\n\nThe file Python/dtoa.c, which supplies C functions dtoa and strtod for conversion of C doubles to and from strings, is derived from the file of the same name by David M. Gay, currently available from http://www.netlib.org/fp/. The original file, as retrieved on March 16, 2009, contains the following copyright and licensing notice:\n\n/****************************************************************\n *\n * The author of this software is David M. Gay.\n *\n * Copyright (c) 1991, 2000, 2001 by Lucent Technologies.\n *\n * Permission to use, copy, modify, and distribute this software for any\n * purpose without fee is hereby granted, provided that this entire notice\n * is included in all copies of any software which is or includes a copy\n * or modification of this software and in all copies of the supporting\n * documentation for such software.\n *\n * THIS SOFTWARE IS BEING PROVIDED \"AS IS\", WITHOUT ANY EXPRESS OR IMPLIED\n * WARRANTY.  IN PARTICULAR, NEITHER THE AUTHOR NOR LUCENT MAKES ANY\n * REPRESENTATION OR WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY\n * OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.\n *\n ***************************************************************/\n\nOpenSSL\n\nThe modules hashlib, posix, ssl, crypt use the OpenSSL library for added performance if made available by the operating system. Additionally, the Windows and Mac OS X installers for Python may include a copy of the OpenSSL libraries, so we include a copy of the OpenSSL license here:\n\n LICENSE ISSUES\n ==============\n\n The OpenSSL toolkit stays under a dual license, i.e. both the conditions of\n the OpenSSL License and the original SSLeay license apply to the toolkit.\n See below for the actual license texts. Actually both licenses are BSD-style\n Open Source licenses. In case of any license issues related to OpenSSL\n please contact openssl-core@openssl.org.\n\n OpenSSL License\n ---------------\n\n   /* ====================================================================\n    * Copyright (c) 1998-2008 The OpenSSL Project.  All rights reserved.\n    *\n    * Redistribution and use in source and binary forms, with or without\n    * modification, are permitted provided that the following conditions\n    * are met:\n    *\n    * 1. Redistributions of source code must retain the above copyright\n    *    notice, this list of conditions and the following disclaimer.\n    *\n    * 2. Redistributions in binary form must reproduce the above copyright\n    *    notice, this list of conditions and the following disclaimer in\n    *    the documentation and/or other materials provided with the\n    *    distribution.\n    *\n    * 3. All advertising materials mentioning features or use of this\n    *    software must display the following acknowledgment:\n    *    \"This product includes software developed by the OpenSSL Project\n    *    for use in the OpenSSL Toolkit. (http://www.openssl.org/)\";\n    *\n    * 4. The names \"OpenSSL Toolkit\" and \"OpenSSL Project\" must not be used to\n    *    endorse or promote products derived from this software without\n    *    prior written permission. For written permission, please contact\n    *    openssl-core@openssl.org.\n    *\n    * 5. Products derived from this software may not be called \"OpenSSL\"\n    *    nor may \"OpenSSL\" appear in their names without prior written\n    *    permission of the OpenSSL Project.\n    *\n    * 6. Redistributions of any form whatsoever must retain the following\n    *    acknowledgment:\n    *    \"This product includes software developed by the OpenSSL Project\n    *    for use in the OpenSSL Toolkit (http://www.openssl.org/)\";\n    *\n    * THIS SOFTWARE IS PROVIDED BY THE OpenSSL PROJECT ``AS IS'' AND ANY\n    * EXPRESSED OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n    * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\n    * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE OpenSSL PROJECT OR\n    * ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n    * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT\n    * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n    * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)\n    * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,\n    * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n    * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED\n    * OF THE POSSIBILITY OF SUCH DAMAGE.\n    * ====================================================================\n    *\n    * This product includes cryptographic software written by Eric Young\n    * (eay@cryptsoft.com).  This product includes software written by Tim\n    * Hudson (tjh@cryptsoft.com).\n    *\n    */\n\nOriginal SSLeay License\n-----------------------\n\n   /* Copyright (C) 1995-1998 Eric Young (eay@cryptsoft.com)\n    * All rights reserved.\n    *\n    * This package is an SSL implementation written\n    * by Eric Young (eay@cryptsoft.com).\n    * The implementation was written so as to conform with Netscapes SSL.\n    *\n    * This library is free for commercial and non-commercial use as long as\n    * the following conditions are aheared to.  The following conditions\n    * apply to all code found in this distribution, be it the RC4, RSA,\n    * lhash, DES, etc., code; not just the SSL code.  The SSL documentation\n    * included with this distribution is covered by the same copyright terms\n    * except that the holder is Tim Hudson (tjh@cryptsoft.com).\n    *\n    * Copyright remains Eric Young's, and as such any Copyright notices in\n    * the code are not to be removed.\n    * If this package is used in a product, Eric Young should be given attribution\n    * as the author of the parts of the library used.\n    * This can be in the form of a textual message at program startup or\n    * in documentation (online or textual) provided with the package.\n    *\n    * Redistribution and use in source and binary forms, with or without\n    * modification, are permitted provided that the following conditions\n    * are met:\n    * 1. Redistributions of source code must retain the copyright\n    *    notice, this list of conditions and the following disclaimer.\n    * 2. Redistributions in binary form must reproduce the above copyright\n    *    notice, this list of conditions and the following disclaimer in the\n    *    documentation and/or other materials provided with the distribution.\n    * 3. All advertising materials mentioning features or use of this software\n    *    must display the following acknowledgement:\n    *    \"This product includes cryptographic software written by\n    *     Eric Young (eay@cryptsoft.com)\"\n    *    The word 'cryptographic' can be left out if the rouines from the library\n    *    being used are not cryptographic related :-).\n    * 4. If you include any Windows specific code (or a derivative thereof) from\n    *    the apps directory (application code) you must include an acknowledgement:\n    *    \"This product includes software written by Tim Hudson (tjh@cryptsoft.com)\"\n    *\n    * THIS SOFTWARE IS PROVIDED BY ERIC YOUNG ``AS IS'' AND\n    * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n    * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n    * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE\n    * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\n    * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS\n    * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)\n    * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n    * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY\n    * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF\n    * SUCH DAMAGE.\n    *\n    * The licence and distribution terms for any publically available version or\n    * derivative of this code cannot be changed.  i.e. this code cannot simply be\n    * copied and put under another distribution licence\n    * [including the GNU Public Licence.]\n    */\n\nexpat\n\nThe pyexpat extension is built using an included copy of the expat sources unless the build is configured --with-system-expat:\n\nCopyright (c) 1998, 1999, 2000 Thai Open Source Software Center Ltd\n                               and Clark Cooper\n\nPermission is hereby granted, free of charge, to any person obtaining\na copy of this software and associated documentation files (the\n\"Software\"), to deal in the Software without restriction, including\nwithout limitation the rights to use, copy, modify, merge, publish,\ndistribute, sublicense, and/or sell copies of the Software, and to\npermit persons to whom the Software is furnished to do so, subject to\nthe following conditions:\n\nThe above copyright notice and this permission notice shall be included\nin all copies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND,\nEXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF\nMERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.\nIN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY\nCLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,\nTORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE\nSOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.\n\nlibffi\n\nThe _ctypes extension is built using an included copy of the libffi sources unless the build is configured --with-system-libffi:\n\nCopyright (c) 1996-2008  Red Hat, Inc and others.\n\nPermission is hereby granted, free of charge, to any person obtaining\na copy of this software and associated documentation files (the\n``Software''), to deal in the Software without restriction, including\nwithout limitation the rights to use, copy, modify, merge, publish,\ndistribute, sublicense, and/or sell copies of the Software, and to\npermit persons to whom the Software is furnished to do so, subject to\nthe following conditions:\n\nThe above copyright notice and this permission notice shall be included\nin all copies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED ``AS IS'', WITHOUT WARRANTY OF ANY KIND,\nEXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF\nMERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND\nNONINFRINGEMENT.  IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT\nHOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,\nWHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER\nDEALINGS IN THE SOFTWARE.\n\nzlib\n\nThe zlib extension is built using an included copy of the zlib sources if the zlib version found on the system is too old to be used for the build:\n\nCopyright (C) 1995-2011 Jean-loup Gailly and Mark Adler\n\nThis software is provided 'as-is', without any express or implied\nwarranty.  In no event will the authors be held liable for any damages\narising from the use of this software.\n\nPermission is granted to anyone to use this software for any purpose,\nincluding commercial applications, and to alter it and redistribute it\nfreely, subject to the following restrictions:\n\n1. The origin of this software must not be misrepresented; you must not\n   claim that you wrote the original software. If you use this software\n   in a product, an acknowledgment in the product documentation would be\n   appreciated but is not required.\n\n2. Altered source versions must be plainly marked as such, and must not be\n   misrepresented as being the original software.\n\n3. This notice may not be removed or altered from any source distribution.\n\nJean-loup Gailly        Mark Adler\njloup@gzip.org          madler@alumni.caltech.edu\n\ncfuhash\n\nThe implementation of the hash table used by the tracemalloc is based on the cfuhash project:\n\nCopyright (c) 2005 Don Owens\nAll rights reserved.\n\nThis code is released under the BSD license:\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions\nare met:\n\n  * Redistributions of source code must retain the above copyright\n    notice, this list of conditions and the following disclaimer.\n\n  * Redistributions in binary form must reproduce the above\n    copyright notice, this list of conditions and the following\n    disclaimer in the documentation and/or other materials provided\n    with the distribution.\n\n  * Neither the name of the author nor the names of its\n    contributors may be used to endorse or promote products derived\n    from this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\nFOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\nCOPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\nINCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\n(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)\nHOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,\nSTRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\nARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED\nOF THE POSSIBILITY OF SUCH DAMAGE.\n\nlibmpdec\n\nThe _decimal module is built using an included copy of the libmpdec library unless the build is configured --with-system-libmpdec:\n\nCopyright (c) 2008-2016 Stefan Krah. All rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions\nare met:\n\n1. Redistributions of source code must retain the above copyright\n   notice, this list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright\n   notice, this list of conditions and the following disclaimer in the\n   documentation and/or other materials provided with the distribution.\n\nTHIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS \"AS IS\" AND\nANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\nARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS\nOR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)\nHOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\nLIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY\nOUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF\nSUCH DAMAGE.\n\n\n================================================================================\npybind11 2.5.0 (Provided under following license)\n================================================================================\n\nCopyright (c) 2016 Wenzel Jakob <wenzel.jakob@epfl.ch>, All rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\n   list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\n   this list of conditions and the following disclaimer in the documentation\n   and/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its contributors\n   may be used to endorse or promote products derived from this software\n   without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\nANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\nWARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\nPlease also refer to the file CONTRIBUTING.md, which clarifies licensing of\nexternal contributions to this project including patches, pull requests, etc.\n\n================================================================================\n\nresult (Provided under following license)\n================================================================================\n\nCopyright (C) 2015-2020 Danilo Bargen and contributors\n\nPermission is hereby granted, free of charge, to any person obtaining a copy of\nthis software and associated documentation files (the \"Software\"), to deal in\nthe Software without restriction, including without limitation the rights to\nuse, copy, modify, merge, publish, distribute, sublicense, and/or sell copies\nof the Software, and to permit persons to whom the Software is furnished to do\nso, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n\n================================================================================\n\nrequests 2.25.1 (Provided under following license)\n================================================================================\n   Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n================================================================================\n\ntoml 0.10.2 (Provided under following license)\n================================================================================\nThe MIT License\n\nCopyright 2013-2019 William Pearson\nCopyright 2015-2016 Julien Enselme\nCopyright 2016 Google Inc.\nCopyright 2017 Samuel Vasko\nCopyright 2017 Nate Prewitt\nCopyright 2017 Jack Evans\nCopyright 2019 Filippo Broggini\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in\nall copies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN\nTHE SOFTWARE.\n================================================================================\n\npyyaml 5.3.1 (Provided under following license)\n================================================================================\n\nCopyright (c) 2017-2020 Ingy döt Net\nCopyright (c) 2006-2016 Kirill Simonov\n\nPermission is hereby granted, free of charge, to any person obtaining a copy of\nthis software and associated documentation files (the \"Software\"), to deal in\nthe Software without restriction, including without limitation the rights to\nuse, copy, modify, merge, publish, distribute, sublicense, and/or sell copies\nof the Software, and to permit persons to whom the Software is furnished to do\nso, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n\n================================================================================\n\npackaging 20.9 (Provided under following license)\n================================================================================\n\nThis software is made available under the terms of *either* of the licenses\nfound in LICENSE.APACHE or LICENSE.BSD. Contributions to this software is made\nunder the terms of *both* these licenses.\n\nApache 2.0\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\nBSD\n\nCopyright (c) Donald Stufft and individual contributors.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n    1. Redistributions of source code must retain the above copyright notice,\n       this list of conditions and the following disclaimer.\n\n    2. Redistributions in binary form must reproduce the above copyright\n       notice, this list of conditions and the following disclaimer in the\n       documentation and/or other materials provided with the distribution.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\nANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\nWARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n================================================================================\nurllib3 1.26.4 (Provided under following license)\n================================================================================\n\nMIT License\n\nCopyright (c) 2008-2020 Andrey Petrov and contributors (see CONTRIBUTORS.txt)\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n\n================================================================================\n\nidna 2.6 (Provided under following license)\n================================================================================\n\nCopyright (c) 2013-2017, Kim Davies. All rights reserved.\n\nRedistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:\n\n    Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.\n    Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.\n    Neither the name of the copyright holder nor the names of the contributors may be used to endorse or promote products derived from this software without specific prior written permission.\n    THIS SOFTWARE IS PROVIDED BY THE CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\nPortions of the codec implementation and unit tests are derived from the Python standard library, which carries the Python Software Foundation License:\n\n    Copyright (c) 2001-2014 Python Software Foundation; All Rights Reserved\n\nPortions of the unit tests are derived from the Unicode standard, which is subject to the Unicode, Inc. License Agreement:\n\n    Copyright (c) 1991-2014 Unicode, Inc. All rights reserved. Distributed under the Terms of Use in <http://www.unicode.org/copyright.html>.\n\n    Permission is hereby granted, free of charge, to any person obtaining a copy of the Unicode data files and any associated documentation (the \"Data Files\") or Unicode software and any associated documentation (the \"Software\") to deal in the Data Files or Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, and/or sell copies of the Data Files or Software, and to permit persons to whom the Data Files or Software are furnished to do so, provided that\n\n    (a) this copyright and permission notice appear with all copies of the Data Files or Software,\n\n    (b) this copyright and permission notice appear in associated documentation, and\n\n    (c) there is clear notice in each modified Data File or in the Software as well as in the documentation associated with the Data File(s) or Software that the data or software has been modified.\n\n    THE DATA FILES AND SOFTWARE ARE PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR HOLDERS INCLUDED IN THIS NOTICE BE LIABLE FOR ANY CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THE DATA FILES OR SOFTWARE.\n\n    Except as contained in this notice, the name of a copyright holder shall not be used in advertising or otherwise to promote the sale, use or other dealings in these Data Files or Software without prior written authorization of the copyright holder.\n\n================================================================================\n\ncertifi 2020.12.5 (Provided under following license)\n================================================================================\n\nThis packge contains a modified version of ca-bundle.crt:\n\nca-bundle.crt -- Bundle of CA Root Certificates\n\nCertificate data from Mozilla as of: Thu Nov  3 19:04:19 2011#\nThis is a bundle of X.509 certificates of public Certificate Authorities\n(CA). These were automatically extracted from Mozilla's root certificates\nfile (certdata.txt).  This file can be found in the mozilla source tree:\nhttp://mxr.mozilla.org/mozilla/source/security/nss/lib/ckfw/builtins/certdata.txt?raw=1#\nIt contains the certificates in PEM format and therefore\ncan be directly used with curl / libcurl / php_curl, or with\nan Apache+mod_ssl webserver for SSL client authentication.\nJust configure this file as the SSLCACertificateFile.#\n\n***** BEGIN LICENSE BLOCK *****\nThis Source Code Form is subject to the terms of the Mozilla Public License,\nv. 2.0. If a copy of the MPL was not distributed with this file, You can obtain\none at http://mozilla.org/MPL/2.0/.\n\n***** END LICENSE BLOCK *****\n@(#) $RCSfile: certdata.txt,v $ $Revision: 1.80 $ $Date: 2011/11/03 15:11:58 $\n\n================================================================================\n\nchardet 3.0.4 (Provided under following license)\n================================================================================\n\n\t\t  GNU LESSER GENERAL PUBLIC LICENSE\n\t\t       Version 2.1, February 1999\n\n Copyright (C) 1991, 1999 Free Software Foundation, Inc.\n     51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA\n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n[This is the first released version of the Lesser GPL.  It also counts\n as the successor of the GNU Library Public License, version 2, hence\n the version number 2.1.]\n\n\t\t\t    Preamble\n\n  The licenses for most software are designed to take away your\nfreedom to share and change it.  By contrast, the GNU General Public\nLicenses are intended to guarantee your freedom to share and change\nfree software--to make sure the software is free for all its users.\n\n  This license, the Lesser General Public License, applies to some\nspecially designated software packages--typically libraries--of the\nFree Software Foundation and other authors who decide to use it.  You\ncan use it too, but we suggest you first think carefully about whether\nthis license or the ordinary General Public License is the better\nstrategy to use in any particular case, based on the explanations below.\n\n  When we speak of free software, we are referring to freedom of use,\nnot price.  Our General Public Licenses are designed to make sure that\nyou have the freedom to distribute copies of free software (and charge\nfor this service if you wish); that you receive source code or can get\nit if you want it; that you can change the software and use pieces of\nit in new free programs; and that you are informed that you can do\nthese things.\n\n  To protect your rights, we need to make restrictions that forbid\ndistributors to deny you these rights or to ask you to surrender these\nrights.  These restrictions translate to certain responsibilities for\nyou if you distribute copies of the library or if you modify it.\n\n  For example, if you distribute copies of the library, whether gratis\nor for a fee, you must give the recipients all the rights that we gave\nyou.  You must make sure that they, too, receive or can get the source\ncode.  If you link other code with the library, you must provide\ncomplete object files to the recipients, so that they can relink them\nwith the library after making changes to the library and recompiling\nit.  And you must show them these terms so they know their rights.\n\n  We protect your rights with a two-step method: (1) we copyright the\nlibrary, and (2) we offer you this license, which gives you legal\npermission to copy, distribute and/or modify the library.\n\n  To protect each distributor, we want to make it very clear that\nthere is no warranty for the free library.  Also, if the library is\nmodified by someone else and passed on, the recipients should know\nthat what they have is not the original version, so that the original\nauthor's reputation will not be affected by problems that might be\nintroduced by others.\n\f\n  Finally, software patents pose a constant threat to the existence of\nany free program.  We wish to make sure that a company cannot\neffectively restrict the users of a free program by obtaining a\nrestrictive license from a patent holder.  Therefore, we insist that\nany patent license obtained for a version of the library must be\nconsistent with the full freedom of use specified in this license.\n\n  Most GNU software, including some libraries, is covered by the\nordinary GNU General Public License.  This license, the GNU Lesser\nGeneral Public License, applies to certain designated libraries, and\nis quite different from the ordinary General Public License.  We use\nthis license for certain libraries in order to permit linking those\nlibraries into non-free programs.\n\n  When a program is linked with a library, whether statically or using\na shared library, the combination of the two is legally speaking a\ncombined work, a derivative of the original library.  The ordinary\nGeneral Public License therefore permits such linking only if the\nentire combination fits its criteria of freedom.  The Lesser General\nPublic License permits more lax criteria for linking other code with\nthe library.\n\n  We call this license the \"Lesser\" General Public License because it\ndoes Less to protect the user's freedom than the ordinary General\nPublic License.  It also provides other free software developers Less\nof an advantage over competing non-free programs.  These disadvantages\nare the reason we use the ordinary General Public License for many\nlibraries.  However, the Lesser license provides advantages in certain\nspecial circumstances.\n\n  For example, on rare occasions, there may be a special need to\nencourage the widest possible use of a certain library, so that it becomes\na de-facto standard.  To achieve this, non-free programs must be\nallowed to use the library.  A more frequent case is that a free\nlibrary does the same job as widely used non-free libraries.  In this\ncase, there is little to gain by limiting the free library to free\nsoftware only, so we use the Lesser General Public License.\n\n  In other cases, permission to use a particular library in non-free\nprograms enables a greater number of people to use a large body of\nfree software.  For example, permission to use the GNU C Library in\nnon-free programs enables many more people to use the whole GNU\noperating system, as well as its variant, the GNU/Linux operating\nsystem.\n\n  Although the Lesser General Public License is Less protective of the\nusers' freedom, it does ensure that the user of a program that is\nlinked with the Library has the freedom and the wherewithal to run\nthat program using a modified version of the Library.\n\n  The precise terms and conditions for copying, distribution and\nmodification follow.  Pay close attention to the difference between a\n\"work based on the library\" and a \"work that uses the library\".  The\nformer contains code derived from the library, whereas the latter must\nbe combined with the library in order to run.\n\f\n\t\t  GNU LESSER GENERAL PUBLIC LICENSE\n   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION\n\n  0. This License Agreement applies to any software library or other\nprogram which contains a notice placed by the copyright holder or\nother authorized party saying it may be distributed under the terms of\nthis Lesser General Public License (also called \"this License\").\nEach licensee is addressed as \"you\".\n\n  A \"library\" means a collection of software functions and/or data\nprepared so as to be conveniently linked with application programs\n(which use some of those functions and data) to form executables.\n\n  The \"Library\", below, refers to any such software library or work\nwhich has been distributed under these terms.  A \"work based on the\nLibrary\" means either the Library or any derivative work under\ncopyright law: that is to say, a work containing the Library or a\nportion of it, either verbatim or with modifications and/or translated\nstraightforwardly into another language.  (Hereinafter, translation is\nincluded without limitation in the term \"modification\".)\n\n  \"Source code\" for a work means the preferred form of the work for\nmaking modifications to it.  For a library, complete source code means\nall the source code for all modules it contains, plus any associated\ninterface definition files, plus the scripts used to control compilation\nand installation of the library.\n\n  Activities other than copying, distribution and modification are not\ncovered by this License; they are outside its scope.  The act of\nrunning a program using the Library is not restricted, and output from\nsuch a program is covered only if its contents constitute a work based\non the Library (independent of the use of the Library in a tool for\nwriting it).  Whether that is true depends on what the Library does\nand what the program that uses the Library does.\n\n  1. You may copy and distribute verbatim copies of the Library's\ncomplete source code as you receive it, in any medium, provided that\nyou conspicuously and appropriately publish on each copy an\nappropriate copyright notice and disclaimer of warranty; keep intact\nall the notices that refer to this License and to the absence of any\nwarranty; and distribute a copy of this License along with the\nLibrary.\n\n  You may charge a fee for the physical act of transferring a copy,\nand you may at your option offer warranty protection in exchange for a\nfee.\n\f\n  2. You may modify your copy or copies of the Library or any portion\nof it, thus forming a work based on the Library, and copy and\ndistribute such modifications or work under the terms of Section 1\nabove, provided that you also meet all of these conditions:\n\n    a) The modified work must itself be a software library.\n\n    b) You must cause the files modified to carry prominent notices\n    stating that you changed the files and the date of any change.\n\n    c) You must cause the whole of the work to be licensed at no\n    charge to all third parties under the terms of this License.\n\n    d) If a facility in the modified Library refers to a function or a\n    table of data to be supplied by an application program that uses\n    the facility, other than as an argument passed when the facility\n    is invoked, then you must make a good faith effort to ensure that,\n    in the event an application does not supply such function or\n    table, the facility still operates, and performs whatever part of\n    its purpose remains meaningful.\n\n    (For example, a function in a library to compute square roots has\n    a purpose that is entirely well-defined independent of the\n    application.  Therefore, Subsection 2d requires that any\n    application-supplied function or table used by this function must\n    be optional: if the application does not supply it, the square\n    root function must still compute square roots.)\n\nThese requirements apply to the modified work as a whole.  If\nidentifiable sections of that work are not derived from the Library,\nand can be reasonably considered independent and separate works in\nthemselves, then this License, and its terms, do not apply to those\nsections when you distribute them as separate works.  But when you\ndistribute the same sections as part of a whole which is a work based\non the Library, the distribution of the whole must be on the terms of\nthis License, whose permissions for other licensees extend to the\nentire whole, and thus to each and every part regardless of who wrote\nit.\n\nThus, it is not the intent of this section to claim rights or contest\nyour rights to work written entirely by you; rather, the intent is to\nexercise the right to control the distribution of derivative or\ncollective works based on the Library.\n\nIn addition, mere aggregation of another work not based on the Library\nwith the Library (or with a work based on the Library) on a volume of\na storage or distribution medium does not bring the other work under\nthe scope of this License.\n\n  3. You may opt to apply the terms of the ordinary GNU General Public\nLicense instead of this License to a given copy of the Library.  To do\nthis, you must alter all the notices that refer to this License, so\nthat they refer to the ordinary GNU General Public License, version 2,\ninstead of to this License.  (If a newer version than version 2 of the\nordinary GNU General Public License has appeared, then you can specify\nthat version instead if you wish.)  Do not make any other change in\nthese notices.\n\f\n  Once this change is made in a given copy, it is irreversible for\nthat copy, so the ordinary GNU General Public License applies to all\nsubsequent copies and derivative works made from that copy.\n\n  This option is useful when you wish to copy part of the code of\nthe Library into a program that is not a library.\n\n  4. You may copy and distribute the Library (or a portion or\nderivative of it, under Section 2) in object code or executable form\nunder the terms of Sections 1 and 2 above provided that you accompany\nit with the complete corresponding machine-readable source code, which\nmust be distributed under the terms of Sections 1 and 2 above on a\nmedium customarily used for software interchange.\n\n  If distribution of object code is made by offering access to copy\nfrom a designated place, then offering equivalent access to copy the\nsource code from the same place satisfies the requirement to\ndistribute the source code, even though third parties are not\ncompelled to copy the source along with the object code.\n\n  5. A program that contains no derivative of any portion of the\nLibrary, but is designed to work with the Library by being compiled or\nlinked with it, is called a \"work that uses the Library\".  Such a\nwork, in isolation, is not a derivative work of the Library, and\ntherefore falls outside the scope of this License.\n\n  However, linking a \"work that uses the Library\" with the Library\ncreates an executable that is a derivative of the Library (because it\ncontains portions of the Library), rather than a \"work that uses the\nlibrary\".  The executable is therefore covered by this License.\nSection 6 states terms for distribution of such executables.\n\n  When a \"work that uses the Library\" uses material from a header file\nthat is part of the Library, the object code for the work may be a\nderivative work of the Library even though the source code is not.\nWhether this is true is especially significant if the work can be\nlinked without the Library, or if the work is itself a library.  The\nthreshold for this to be true is not precisely defined by law.\n\n  If such an object file uses only numerical parameters, data\nstructure layouts and accessors, and small macros and small inline\nfunctions (ten lines or less in length), then the use of the object\nfile is unrestricted, regardless of whether it is legally a derivative\nwork.  (Executables containing this object code plus portions of the\nLibrary will still fall under Section 6.)\n\n  Otherwise, if the work is a derivative of the Library, you may\ndistribute the object code for the work under the terms of Section 6.\nAny executables containing that work also fall under Section 6,\nwhether or not they are linked directly with the Library itself.\n\f\n  6. As an exception to the Sections above, you may also combine or\nlink a \"work that uses the Library\" with the Library to produce a\nwork containing portions of the Library, and distribute that work\nunder terms of your choice, provided that the terms permit\nmodification of the work for the customer's own use and reverse\nengineering for debugging such modifications.\n\n  You must give prominent notice with each copy of the work that the\nLibrary is used in it and that the Library and its use are covered by\nthis License.  You must supply a copy of this License.  If the work\nduring execution displays copyright notices, you must include the\ncopyright notice for the Library among them, as well as a reference\ndirecting the user to the copy of this License.  Also, you must do one\nof these things:\n\n    a) Accompany the work with the complete corresponding\n    machine-readable source code for the Library including whatever\n    changes were used in the work (which must be distributed under\n    Sections 1 and 2 above); and, if the work is an executable linked\n    with the Library, with the complete machine-readable \"work that\n    uses the Library\", as object code and/or source code, so that the\n    user can modify the Library and then relink to produce a modified\n    executable containing the modified Library.  (It is understood\n    that the user who changes the contents of definitions files in the\n    Library will not necessarily be able to recompile the application\n    to use the modified definitions.)\n\n    b) Use a suitable shared library mechanism for linking with the\n    Library.  A suitable mechanism is one that (1) uses at run time a\n    copy of the library already present on the user's computer system,\n    rather than copying library functions into the executable, and (2)\n    will operate properly with a modified version of the library, if\n    the user installs one, as long as the modified version is\n    interface-compatible with the version that the work was made with.\n\n    c) Accompany the work with a written offer, valid for at\n    least three years, to give the same user the materials\n    specified in Subsection 6a, above, for a charge no more\n    than the cost of performing this distribution.\n\n    d) If distribution of the work is made by offering access to copy\n    from a designated place, offer equivalent access to copy the above\n    specified materials from the same place.\n\n    e) Verify that the user has already received a copy of these\n    materials or that you have already sent this user a copy.\n\n  For an executable, the required form of the \"work that uses the\nLibrary\" must include any data and utility programs needed for\nreproducing the executable from it.  However, as a special exception,\nthe materials to be distributed need not include anything that is\nnormally distributed (in either source or binary form) with the major\ncomponents (compiler, kernel, and so on) of the operating system on\nwhich the executable runs, unless that component itself accompanies\nthe executable.\n\n  It may happen that this requirement contradicts the license\nrestrictions of other proprietary libraries that do not normally\naccompany the operating system.  Such a contradiction means you cannot\nuse both them and the Library together in an executable that you\ndistribute.\n\f\n  7. You may place library facilities that are a work based on the\nLibrary side-by-side in a single library together with other library\nfacilities not covered by this License, and distribute such a combined\nlibrary, provided that the separate distribution of the work based on\nthe Library and of the other library facilities is otherwise\npermitted, and provided that you do these two things:\n\n    a) Accompany the combined library with a copy of the same work\n    based on the Library, uncombined with any other library\n    facilities.  This must be distributed under the terms of the\n    Sections above.\n\n    b) Give prominent notice with the combined library of the fact\n    that part of it is a work based on the Library, and explaining\n    where to find the accompanying uncombined form of the same work.\n\n  8. You may not copy, modify, sublicense, link with, or distribute\nthe Library except as expressly provided under this License.  Any\nattempt otherwise to copy, modify, sublicense, link with, or\ndistribute the Library is void, and will automatically terminate your\nrights under this License.  However, parties who have received copies,\nor rights, from you under this License will not have their licenses\nterminated so long as such parties remain in full compliance.\n\n  9. You are not required to accept this License, since you have not\nsigned it.  However, nothing else grants you permission to modify or\ndistribute the Library or its derivative works.  These actions are\nprohibited by law if you do not accept this License.  Therefore, by\nmodifying or distributing the Library (or any work based on the\nLibrary), you indicate your acceptance of this License to do so, and\nall its terms and conditions for copying, distributing or modifying\nthe Library or works based on it.\n\n  10. Each time you redistribute the Library (or any work based on the\nLibrary), the recipient automatically receives a license from the\noriginal licensor to copy, distribute, link with or modify the Library\nsubject to these terms and conditions.  You may not impose any further\nrestrictions on the recipients' exercise of the rights granted herein.\nYou are not responsible for enforcing compliance by third parties with\nthis License.\n\f\n  11. If, as a consequence of a court judgment or allegation of patent\ninfringement or for any other reason (not limited to patent issues),\nconditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License.  If you cannot\ndistribute so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you\nmay not distribute the Library at all.  For example, if a patent\nlicense would not permit royalty-free redistribution of the Library by\nall those who receive copies directly or indirectly through you, then\nthe only way you could satisfy both it and this License would be to\nrefrain entirely from distribution of the Library.\n\nIf any portion of this section is held invalid or unenforceable under any\nparticular circumstance, the balance of the section is intended to apply,\nand the section as a whole is intended to apply in other circumstances.\n\nIt is not the purpose of this section to induce you to infringe any\npatents or other property right claims or to contest validity of any\nsuch claims; this section has the sole purpose of protecting the\nintegrity of the free software distribution system which is\nimplemented by public license practices.  Many people have made\ngenerous contributions to the wide range of software distributed\nthrough that system in reliance on consistent application of that\nsystem; it is up to the author/donor to decide if he or she is willing\nto distribute software through any other system and a licensee cannot\nimpose that choice.\n\nThis section is intended to make thoroughly clear what is believed to\nbe a consequence of the rest of this License.\n\n  12. If the distribution and/or use of the Library is restricted in\ncertain countries either by patents or by copyrighted interfaces, the\noriginal copyright holder who places the Library under this License may add\nan explicit geographical distribution limitation excluding those countries,\nso that distribution is permitted only in or among countries not thus\nexcluded.  In such case, this License incorporates the limitation as if\nwritten in the body of this License.\n\n  13. The Free Software Foundation may publish revised and/or new\nversions of the Lesser General Public License from time to time.\nSuch new versions will be similar in spirit to the present version,\nbut may differ in detail to address new problems or concerns.\n\nEach version is given a distinguishing version number.  If the Library\nspecifies a version number of this License which applies to it and\n\"any later version\", you have the option of following the terms and\nconditions either of that version or of any later version published by\nthe Free Software Foundation.  If the Library does not specify a\nlicense version number, you may choose any version ever published by\nthe Free Software Foundation.\n\f\n  14. If you wish to incorporate parts of the Library into other free\nprograms whose distribution conditions are incompatible with these,\nwrite to the author to ask for permission.  For software which is\ncopyrighted by the Free Software Foundation, write to the Free\nSoftware Foundation; we sometimes make exceptions for this.  Our\ndecision will be guided by the two goals of preserving the free status\nof all derivatives of our free software and of promoting the sharing\nand reuse of software generally.\n\n\t\t\t    NO WARRANTY\n\n  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO\nWARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.\nEXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR\nOTHER PARTIES PROVIDE THE LIBRARY \"AS IS\" WITHOUT WARRANTY OF ANY\nKIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\nPURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE\nLIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME\nTHE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.\n\n  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN\nWRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY\nAND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU\nFOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR\nCONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE\nLIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING\nRENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A\nFAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF\nSUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH\nDAMAGES.\n\n\t\t     END OF TERMS AND CONDITIONS\n\f\n           How to Apply These Terms to Your New Libraries\n\n  If you develop a new library, and you want it to be of the greatest\npossible use to the public, we recommend making it free software that\neveryone can redistribute and change.  You can do so by permitting\nredistribution under these terms (or, alternatively, under the terms of the\nordinary General Public License).\n\n  To apply these terms, attach the following notices to the library.  It is\nsafest to attach them to the start of each source file to most effectively\nconvey the exclusion of warranty; and each file should have at least the\n\"copyright\" line and a pointer to where the full notice is found.\n\n    <one line to give the library's name and a brief idea of what it does.>\n    Copyright (C) <year>  <name of author>\n\n    This library is free software; you can redistribute it and/or\n    modify it under the terms of the GNU Lesser General Public\n    License as published by the Free Software Foundation; either\n    version 2.1 of the License, or (at your option) any later version.\n\n    This library is distributed in the hope that it will be useful,\n    but WITHOUT ANY WARRANTY; without even the implied warranty of\n    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU\n    Lesser General Public License for more details.\n\n    You should have received a copy of the GNU Lesser General Public\n    License along with this library; if not, write to the Free Software\n    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA\n\nAlso add information on how to contact you by electronic and paper mail.\n\nYou should also get your employer (if you work as a programmer) or your\nschool, if any, to sign a \"copyright disclaimer\" for the library, if\nnecessary.  Here is a sample; alter the names:\n\n  Yoyodyne, Inc., hereby disclaims all copyright interest in the\n  library `Frob' (a library for tweaking knobs) written by James Random Hacker.\n\n  <signature of Ty Coon>, 1 April 1990\n  Ty Coon, President of Vice\n\nThat's all there is to it!\n\n================================================================================\n\nGLib  (Provided under following license)\n================================================================================\n                  GNU LESSER GENERAL PUBLIC LICENSE\n                       Version 2.1, February 1999\n\n Copyright (C) 1991, 1999 Free Software Foundation, Inc.\n 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA\n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n[This is the first released version of the Lesser GPL.  It also counts\n as the successor of the GNU Library Public License, version 2, hence\n the version number 2.1.]\n\n                            Preamble\n\n  The licenses for most software are designed to take away your\nfreedom to share and change it.  By contrast, the GNU General Public\nLicenses are intended to guarantee your freedom to share and change\nfree software--to make sure the software is free for all its users.\n\n  This license, the Lesser General Public License, applies to some\nspecially designated software packages--typically libraries--of the\nFree Software Foundation and other authors who decide to use it.  You\ncan use it too, but we suggest you first think carefully about whether\nthis license or the ordinary General Public License is the better\nstrategy to use in any particular case, based on the explanations below.\n\n  When we speak of free software, we are referring to freedom of use,\nnot price.  Our General Public Licenses are designed to make sure that\nyou have the freedom to distribute copies of free software (and charge\nfor this service if you wish); that you receive source code or can get\nit if you want it; that you can change the software and use pieces of\nit in new free programs; and that you are informed that you can do\nthese things.\n\n  To protect your rights, we need to make restrictions that forbid\ndistributors to deny you these rights or to ask you to surrender these\nrights.  These restrictions translate to certain responsibilities for\nyou if you distribute copies of the library or if you modify it.\n\n  For example, if you distribute copies of the library, whether gratis\nor for a fee, you must give the recipients all the rights that we gave\nyou.  You must make sure that they, too, receive or can get the source\ncode.  If you link other code with the library, you must provide\ncomplete object files to the recipients, so that they can relink them\nwith the library after making changes to the library and recompiling\nit.  And you must show them these terms so they know their rights.\n\n  We protect your rights with a two-step method: (1) we copyright the\nlibrary, and (2) we offer you this license, which gives you legal\npermission to copy, distribute and/or modify the library.\n\n  To protect each distributor, we want to make it very clear that\nthere is no warranty for the free library.  Also, if the library is\nmodified by someone else and passed on, the recipients should know\nthat what they have is not the original version, so that the original\nauthor's reputation will not be affected by problems that might be\nintroduced by others.\n\n  Finally, software patents pose a constant threat to the existence of\nany free program.  We wish to make sure that a company cannot\neffectively restrict the users of a free program by obtaining a\nrestrictive license from a patent holder.  Therefore, we insist that\nany patent license obtained for a version of the library must be\nconsistent with the full freedom of use specified in this license.\n\n  Most GNU software, including some libraries, is covered by the\nordinary GNU General Public License.  This license, the GNU Lesser\nGeneral Public License, applies to certain designated libraries, and\nis quite different from the ordinary General Public License.  We use\nthis license for certain libraries in order to permit linking those\nlibraries into non-free programs.\n\n  When a program is linked with a library, whether statically or using\na shared library, the combination of the two is legally speaking a\ncombined work, a derivative of the original library.  The ordinary\nGeneral Public License therefore permits such linking only if the\nentire combination fits its criteria of freedom.  The Lesser General\nPublic License permits more lax criteria for linking other code with\nthe library.\n\n  We call this license the \"Lesser\" General Public License because it\ndoes Less to protect the user's freedom than the ordinary General\nPublic License.  It also provides other free software developers Less\nof an advantage over competing non-free programs.  These disadvantages\nare the reason we use the ordinary General Public License for many\nlibraries.  However, the Lesser license provides advantages in certain\nspecial circumstances.\n\n  For example, on rare occasions, there may be a special need to\nencourage the widest possible use of a certain library, so that it becomes\na de-facto standard.  To achieve this, non-free programs must be\nallowed to use the library.  A more frequent case is that a free\nlibrary does the same job as widely used non-free libraries.  In this\ncase, there is little to gain by limiting the free library to free\nsoftware only, so we use the Lesser General Public License.\n\n  In other cases, permission to use a particular library in non-free\nprograms enables a greater number of people to use a large body of\nfree software.  For example, permission to use the GNU C Library in\nnon-free programs enables many more people to use the whole GNU\noperating system, as well as its variant, the GNU/Linux operating\nsystem.\n\n  Although the Lesser General Public License is Less protective of the\nusers' freedom, it does ensure that the user of a program that is\nlinked with the Library has the freedom and the wherewithal to run\nthat program using a modified version of the Library.\n\n  The precise terms and conditions for copying, distribution and\nmodification follow.  Pay close attention to the difference between a\n\"work based on the library\" and a \"work that uses the library\".  The\nformer contains code derived from the library, whereas the latter must\nbe combined with the library in order to run.\n\n                  GNU LESSER GENERAL PUBLIC LICENSE\n   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION\n\n  0. This License Agreement applies to any software library or other\nprogram which contains a notice placed by the copyright holder or\nother authorized party saying it may be distributed under the terms of\nthis Lesser General Public License (also called \"this License\").\nEach licensee is addressed as \"you\".\n\n  A \"library\" means a collection of software functions and/or data\nprepared so as to be conveniently linked with application programs\n(which use some of those functions and data) to form executables.\n\n  The \"Library\", below, refers to any such software library or work\nwhich has been distributed under these terms.  A \"work based on the\nLibrary\" means either the Library or any derivative work under\ncopyright law: that is to say, a work containing the Library or a\nportion of it, either verbatim or with modifications and/or translated\nstraightforwardly into another language.  (Hereinafter, translation is\nincluded without limitation in the term \"modification\".)\n\n  \"Source code\" for a work means the preferred form of the work for\nmaking modifications to it.  For a library, complete source code means\nall the source code for all modules it contains, plus any associated\ninterface definition files, plus the scripts used to control compilation\nand installation of the library.\n\n  Activities other than copying, distribution and modification are not\ncovered by this License; they are outside its scope.  The act of\nrunning a program using the Library is not restricted, and output from\nsuch a program is covered only if its contents constitute a work based\non the Library (independent of the use of the Library in a tool for\nwriting it).  Whether that is true depends on what the Library does\nand what the program that uses the Library does.\n\n  1. You may copy and distribute verbatim copies of the Library's\ncomplete source code as you receive it, in any medium, provided that\nyou conspicuously and appropriately publish on each copy an\nappropriate copyright notice and disclaimer of warranty; keep intact\nall the notices that refer to this License and to the absence of any\nwarranty; and distribute a copy of this License along with the\nLibrary.\n\n  You may charge a fee for the physical act of transferring a copy,\nand you may at your option offer warranty protection in exchange for a\nfee.\n\n  2. You may modify your copy or copies of the Library or any portion\nof it, thus forming a work based on the Library, and copy and\ndistribute such modifications or work under the terms of Section 1\nabove, provided that you also meet all of these conditions:\n\n    a) The modified work must itself be a software library.\n\n    b) You must cause the files modified to carry prominent notices\n    stating that you changed the files and the date of any change.\n\n    c) You must cause the whole of the work to be licensed at no\n    charge to all third parties under the terms of this License.\n\n    d) If a facility in the modified Library refers to a function or a\n    table of data to be supplied by an application program that uses\n    the facility, other than as an argument passed when the facility\n    is invoked, then you must make a good faith effort to ensure that,\n    in the event an application does not supply such function or\n    table, the facility still operates, and performs whatever part of\n    its purpose remains meaningful.\n\n    (For example, a function in a library to compute square roots has\n    a purpose that is entirely well-defined independent of the\n    application.  Therefore, Subsection 2d requires that any\n    application-supplied function or table used by this function must\n    be optional: if the application does not supply it, the square\n    root function must still compute square roots.)\n\nThese requirements apply to the modified work as a whole.  If\nidentifiable sections of that work are not derived from the Library,\nand can be reasonably considered independent and separate works in\nthemselves, then this License, and its terms, do not apply to those\nsections when you distribute them as separate works.  But when you\ndistribute the same sections as part of a whole which is a work based\non the Library, the distribution of the whole must be on the terms of\nthis License, whose permissions for other licensees extend to the\nentire whole, and thus to each and every part regardless of who wrote\nit.\n\nThus, it is not the intent of this section to claim rights or contest\nyour rights to work written entirely by you; rather, the intent is to\nexercise the right to control the distribution of derivative or\ncollective works based on the Library.\n\nIn addition, mere aggregation of another work not based on the Library\nwith the Library (or with a work based on the Library) on a volume of\na storage or distribution medium does not bring the other work under\nthe scope of this License.\n\n  3. You may opt to apply the terms of the ordinary GNU General Public\nLicense instead of this License to a given copy of the Library.  To do\nthis, you must alter all the notices that refer to this License, so\nthat they refer to the ordinary GNU General Public License, version 2,\ninstead of to this License.  (If a newer version than version 2 of the\nordinary GNU General Public License has appeared, then you can specify\nthat version instead if you wish.)  Do not make any other change in\nthese notices.\n\n  Once this change is made in a given copy, it is irreversible for\nthat copy, so the ordinary GNU General Public License applies to all\nsubsequent copies and derivative works made from that copy.\n\n  This option is useful when you wish to copy part of the code of\nthe Library into a program that is not a library.\n\n  4. You may copy and distribute the Library (or a portion or\nderivative of it, under Section 2) in object code or executable form\nunder the terms of Sections 1 and 2 above provided that you accompany\nit with the complete corresponding machine-readable source code, which\nmust be distributed under the terms of Sections 1 and 2 above on a\nmedium customarily used for software interchange.\n\n  If distribution of object code is made by offering access to copy\nfrom a designated place, then offering equivalent access to copy the\nsource code from the same place satisfies the requirement to\ndistribute the source code, even though third parties are not\ncompelled to copy the source along with the object code.\n\n  5. A program that contains no derivative of any portion of the\nLibrary, but is designed to work with the Library by being compiled or\nlinked with it, is called a \"work that uses the Library\".  Such a\nwork, in isolation, is not a derivative work of the Library, and\ntherefore falls outside the scope of this License.\n\n  However, linking a \"work that uses the Library\" with the Library\ncreates an executable that is a derivative of the Library (because it\ncontains portions of the Library), rather than a \"work that uses the\nlibrary\".  The executable is therefore covered by this License.\nSection 6 states terms for distribution of such executables.\n\n  When a \"work that uses the Library\" uses material from a header file\nthat is part of the Library, the object code for the work may be a\nderivative work of the Library even though the source code is not.\nWhether this is true is especially significant if the work can be\nlinked without the Library, or if the work is itself a library.  The\nthreshold for this to be true is not precisely defined by law.\n\n  If such an object file uses only numerical parameters, data\nstructure layouts and accessors, and small macros and small inline\nfunctions (ten lines or less in length), then the use of the object\nfile is unrestricted, regardless of whether it is legally a derivative\nwork.  (Executables containing this object code plus portions of the\nLibrary will still fall under Section 6.)\n\n  Otherwise, if the work is a derivative of the Library, you may\ndistribute the object code for the work under the terms of Section 6.\nAny executables containing that work also fall under Section 6,\nwhether or not they are linked directly with the Library itself.\n\n  6. As an exception to the Sections above, you may also combine or\nlink a \"work that uses the Library\" with the Library to produce a\nwork containing portions of the Library, and distribute that work\nunder terms of your choice, provided that the terms permit\nmodification of the work for the customer's own use and reverse\nengineering for debugging such modifications.\n\n  You must give prominent notice with each copy of the work that the\nLibrary is used in it and that the Library and its use are covered by\nthis License.  You must supply a copy of this License.  If the work\nduring execution displays copyright notices, you must include the\ncopyright notice for the Library among them, as well as a reference\ndirecting the user to the copy of this License.  Also, you must do one\nof these things:\n\n    a) Accompany the work with the complete corresponding\n    machine-readable source code for the Library including whatever\n    changes were used in the work (which must be distributed under\n    Sections 1 and 2 above); and, if the work is an executable linked\n    with the Library, with the complete machine-readable \"work that\n    uses the Library\", as object code and/or source code, so that the\n    user can modify the Library and then relink to produce a modified\n    executable containing the modified Library.  (It is understood\n    that the user who changes the contents of definitions files in the\n    Library will not necessarily be able to recompile the application\n    to use the modified definitions.)\n\n    b) Use a suitable shared library mechanism for linking with the\n    Library.  A suitable mechanism is one that (1) uses at run time a\n    copy of the library already present on the user's computer system,\n    rather than copying library functions into the executable, and (2)\n    will operate properly with a modified version of the library, if\n    the user installs one, as long as the modified version is\n    interface-compatible with the version that the work was made with.\n\n    c) Accompany the work with a written offer, valid for at\n    least three years, to give the same user the materials\n    specified in Subsection 6a, above, for a charge no more\n    than the cost of performing this distribution.\n\n    d) If distribution of the work is made by offering access to copy\n    from a designated place, offer equivalent access to copy the above\n    specified materials from the same place.\n\n    e) Verify that the user has already received a copy of these\n    materials or that you have already sent this user a copy.\n\n  For an executable, the required form of the \"work that uses the\nLibrary\" must include any data and utility programs needed for\nreproducing the executable from it.  However, as a special exception,\nthe materials to be distributed need not include anything that is\nnormally distributed (in either source or binary form) with the major\ncomponents (compiler, kernel, and so on) of the operating system on\nwhich the executable runs, unless that component itself accompanies\nthe executable.\n\n  It may happen that this requirement contradicts the license\nrestrictions of other proprietary libraries that do not normally\naccompany the operating system.  Such a contradiction means you cannot\nuse both them and the Library together in an executable that you\ndistribute.\n\n  7. You may place library facilities that are a work based on the\nLibrary side-by-side in a single library together with other library\nfacilities not covered by this License, and distribute such a combined\nlibrary, provided that the separate distribution of the work based on\nthe Library and of the other library facilities is otherwise\npermitted, and provided that you do these two things:\n\n    a) Accompany the combined library with a copy of the same work\n    based on the Library, uncombined with any other library\n    facilities.  This must be distributed under the terms of the\n    Sections above.\n\n    b) Give prominent notice with the combined library of the fact\n    that part of it is a work based on the Library, and explaining\n    where to find the accompanying uncombined form of the same work.\n\n  8. You may not copy, modify, sublicense, link with, or distribute\nthe Library except as expressly provided under this License.  Any\nattempt otherwise to copy, modify, sublicense, link with, or\ndistribute the Library is void, and will automatically terminate your\nrights under this License.  However, parties who have received copies,\nor rights, from you under this License will not have their licenses\nterminated so long as such parties remain in full compliance.\n\n  9. You are not required to accept this License, since you have not\nsigned it.  However, nothing else grants you permission to modify or\ndistribute the Library or its derivative works.  These actions are\nprohibited by law if you do not accept this License.  Therefore, by\nmodifying or distributing the Library (or any work based on the\nLibrary), you indicate your acceptance of this License to do so, and\nall its terms and conditions for copying, distributing or modifying\nthe Library or works based on it.\n\n  10. Each time you redistribute the Library (or any work based on the\nLibrary), the recipient automatically receives a license from the\noriginal licensor to copy, distribute, link with or modify the Library\nsubject to these terms and conditions.  You may not impose any further\nrestrictions on the recipients' exercise of the rights granted herein.\nYou are not responsible for enforcing compliance by third parties with\nthis License.\n\n  11. If, as a consequence of a court judgment or allegation of patent\ninfringement or for any other reason (not limited to patent issues),\nconditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License.  If you cannot\ndistribute so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you\nmay not distribute the Library at all.  For example, if a patent\nlicense would not permit royalty-free redistribution of the Library by\nall those who receive copies directly or indirectly through you, then\nthe only way you could satisfy both it and this License would be to\nrefrain entirely from distribution of the Library.\n\nIf any portion of this section is held invalid or unenforceable under any\nparticular circumstance, the balance of the section is intended to apply,\nand the section as a whole is intended to apply in other circumstances.\n\nIt is not the purpose of this section to induce you to infringe any\npatents or other property right claims or to contest validity of any\nsuch claims; this section has the sole purpose of protecting the\nintegrity of the free software distribution system which is\nimplemented by public license practices.  Many people have made\ngenerous contributions to the wide range of software distributed\nthrough that system in reliance on consistent application of that\nsystem; it is up to the author/donor to decide if he or she is willing\nto distribute software through any other system and a licensee cannot\nimpose that choice.\n\nThis section is intended to make thoroughly clear what is believed to\nbe a consequence of the rest of this License.\n\n  12. If the distribution and/or use of the Library is restricted in\ncertain countries either by patents or by copyrighted interfaces, the\noriginal copyright holder who places the Library under this License may add\nan explicit geographical distribution limitation excluding those countries,\nso that distribution is permitted only in or among countries not thus\nexcluded.  In such case, this License incorporates the limitation as if\nwritten in the body of this License.\n\n  13. The Free Software Foundation may publish revised and/or new\nversions of the Lesser General Public License from time to time.\nSuch new versions will be similar in spirit to the present version,\nbut may differ in detail to address new problems or concerns.\n\nEach version is given a distinguishing version number.  If the Library\nspecifies a version number of this License which applies to it and\n\"any later version\", you have the option of following the terms and\nconditions either of that version or of any later version published by\nthe Free Software Foundation.  If the Library does not specify a\nlicense version number, you may choose any version ever published by\nthe Free Software Foundation.\n\n  14. If you wish to incorporate parts of the Library into other free\nprograms whose distribution conditions are incompatible with these,\nwrite to the author to ask for permission.  For software which is\ncopyrighted by the Free Software Foundation, write to the Free\nSoftware Foundation; we sometimes make exceptions for this.  Our\ndecision will be guided by the two goals of preserving the free status\nof all derivatives of our free software and of promoting the sharing\nand reuse of software generally.\n\n                            NO WARRANTY\n\n  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO\nWARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.\nEXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR\nOTHER PARTIES PROVIDE THE LIBRARY \"AS IS\" WITHOUT WARRANTY OF ANY\nKIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\nPURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE\nLIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME\nTHE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.\n\n  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN\nWRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY\nAND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU\nFOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR\nCONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE\nLIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING\nRENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A\nFAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF\nSUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH\nDAMAGES.\n\n                     END OF TERMS AND CONDITIONS\n\n           How to Apply These Terms to Your New Libraries\n\n  If you develop a new library, and you want it to be of the greatest\npossible use to the public, we recommend making it free software that\neveryone can redistribute and change.  You can do so by permitting\nredistribution under these terms (or, alternatively, under the terms of the\nordinary General Public License).\n\n  To apply these terms, attach the following notices to the library.  It is\nsafest to attach them to the start of each source file to most effectively\nconvey the exclusion of warranty; and each file should have at least the\n\"copyright\" line and a pointer to where the full notice is found.\n\n    <one line to give the library's name and a brief idea of what it does.>\n    Copyright (C) <year>  <name of author>\n\n    This library is free software; you can redistribute it and/or\n    modify it under the terms of the GNU Lesser General Public\n    License as published by the Free Software Foundation; either\n    version 2.1 of the License, or (at your option) any later version.\n\n    This library is distributed in the hope that it will be useful,\n    but WITHOUT ANY WARRANTY; without even the implied warranty of\n    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU\n    Lesser General Public License for more details.\n\n    You should have received a copy of the GNU Lesser General Public\n    License along with this library; if not, write to the Free Software\n    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA\n\nAlso add information on how to contact you by electronic and paper mail.\n\nYou should also get your employer (if you work as a programmer) or your\nschool, if any, to sign a \"copyright disclaimer\" for the library, if\nnecessary.  Here is a sample; alter the names:\n\n  Yoyodyne, Inc., hereby disclaims all copyright interest in the\n  library `Frob' (a library for tweaking knobs) written by James Random Hacker.\n\n  <signature of Ty Coon>, 1 April 1990\n  Ty Coon, President of Vice\n\nThat's all there is to it!\n================================================================================\n\nGStreamer  (Provided under following license)\n================================================================================\n\t\t  GNU LIBRARY GENERAL PUBLIC LICENSE\n\t\t       Version 2, June 1991\n\n Copyright (C) 1991 Free Software Foundation, Inc.\n                    51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA\n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n[This is the first released version of the library GPL.  It is\n numbered 2 because it goes with version 2 of the ordinary GPL.]\n\n\t\t\t    Preamble\n\n  The licenses for most software are designed to take away your\nfreedom to share and change it.  By contrast, the GNU General Public\nLicenses are intended to guarantee your freedom to share and change\nfree software--to make sure the software is free for all its users.\n\n  This license, the Library General Public License, applies to some\nspecially designated Free Software Foundation software, and to any\nother libraries whose authors decide to use it.  You can use it for\nyour libraries, too.\n\n  When we speak of free software, we are referring to freedom, not\nprice.  Our General Public Licenses are designed to make sure that you\nhave the freedom to distribute copies of free software (and charge for\nthis service if you wish), that you receive source code or can get it\nif you want it, that you can change the software or use pieces of it\nin new free programs; and that you know you can do these things.\n\n  To protect your rights, we need to make restrictions that forbid\nanyone to deny you these rights or to ask you to surrender the rights.\nThese restrictions translate to certain responsibilities for you if\nyou distribute copies of the library, or if you modify it.\n\n  For example, if you distribute copies of the library, whether gratis\nor for a fee, you must give the recipients all the rights that we gave\nyou.  You must make sure that they, too, receive or can get the source\ncode.  If you link a program with the library, you must provide\ncomplete object files to the recipients so that they can relink them\nwith the library, after making changes to the library and recompiling\nit.  And you must show them these terms so they know their rights.\n\n  Our method of protecting your rights has two steps: (1) copyright\nthe library, and (2) offer you this license which gives you legal\npermission to copy, distribute and/or modify the library.\n\n  Also, for each distributor's protection, we want to make certain\nthat everyone understands that there is no warranty for this free\nlibrary.  If the library is modified by someone else and passed on, we\nwant its recipients to know that what they have is not the original\nversion, so that any problems introduced by others will not reflect on\nthe original authors' reputations.\n\n  Finally, any free program is threatened constantly by software\npatents.  We wish to avoid the danger that companies distributing free\nsoftware will individually obtain patent licenses, thus in effect\ntransforming the program into proprietary software.  To prevent this,\nwe have made it clear that any patent must be licensed for everyone's\nfree use or not licensed at all.\n\n  Most GNU software, including some libraries, is covered by the ordinary\nGNU General Public License, which was designed for utility programs.  This\nlicense, the GNU Library General Public License, applies to certain\ndesignated libraries.  This license is quite different from the ordinary\none; be sure to read it in full, and don't assume that anything in it is\nthe same as in the ordinary license.\n\n  The reason we have a separate public license for some libraries is that\nthey blur the distinction we usually make between modifying or adding to a\nprogram and simply using it.  Linking a program with a library, without\nchanging the library, is in some sense simply using the library, and is\nanalogous to running a utility program or application program.  However, in\na textual and legal sense, the linked executable is a combined work, a\nderivative of the original library, and the ordinary General Public License\ntreats it as such.\n\n  Because of this blurred distinction, using the ordinary General\nPublic License for libraries did not effectively promote software\nsharing, because most developers did not use the libraries.  We\nconcluded that weaker conditions might promote sharing better.\n\n  However, unrestricted linking of non-free programs would deprive the\nusers of those programs of all benefit from the free status of the\nlibraries themselves.  This Library General Public License is intended to\npermit developers of non-free programs to use free libraries, while\npreserving your freedom as a user of such programs to change the free\nlibraries that are incorporated in them.  (We have not seen how to achieve\nthis as regards changes in header files, but we have achieved it as regards\nchanges in the actual functions of the Library.)  The hope is that this\nwill lead to faster development of free libraries.\n\n  The precise terms and conditions for copying, distribution and\nmodification follow.  Pay close attention to the difference between a\n\"work based on the library\" and a \"work that uses the library\".  The\nformer contains code derived from the library, while the latter only\nworks together with the library.\n\n  Note that it is possible for a library to be covered by the ordinary\nGeneral Public License rather than by this special one.\n\n\t\t  GNU LIBRARY GENERAL PUBLIC LICENSE\n   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION\n\n  0. This License Agreement applies to any software library which\ncontains a notice placed by the copyright holder or other authorized\nparty saying it may be distributed under the terms of this Library\nGeneral Public License (also called \"this License\").  Each licensee is\naddressed as \"you\".\n\n  A \"library\" means a collection of software functions and/or data\nprepared so as to be conveniently linked with application programs\n(which use some of those functions and data) to form executables.\n\n  The \"Library\", below, refers to any such software library or work\nwhich has been distributed under these terms.  A \"work based on the\nLibrary\" means either the Library or any derivative work under\ncopyright law: that is to say, a work containing the Library or a\nportion of it, either verbatim or with modifications and/or translated\nstraightforwardly into another language.  (Hereinafter, translation is\nincluded without limitation in the term \"modification\".)\n\n  \"Source code\" for a work means the preferred form of the work for\nmaking modifications to it.  For a library, complete source code means\nall the source code for all modules it contains, plus any associated\ninterface definition files, plus the scripts used to control compilation\nand installation of the library.\n\n  Activities other than copying, distribution and modification are not\ncovered by this License; they are outside its scope.  The act of\nrunning a program using the Library is not restricted, and output from\nsuch a program is covered only if its contents constitute a work based\non the Library (independent of the use of the Library in a tool for\nwriting it).  Whether that is true depends on what the Library does\nand what the program that uses the Library does.\n\n  1. You may copy and distribute verbatim copies of the Library's\ncomplete source code as you receive it, in any medium, provided that\nyou conspicuously and appropriately publish on each copy an\nappropriate copyright notice and disclaimer of warranty; keep intact\nall the notices that refer to this License and to the absence of any\nwarranty; and distribute a copy of this License along with the\nLibrary.\n\n  You may charge a fee for the physical act of transferring a copy,\nand you may at your option offer warranty protection in exchange for a\nfee.\n\n  2. You may modify your copy or copies of the Library or any portion\nof it, thus forming a work based on the Library, and copy and\ndistribute such modifications or work under the terms of Section 1\nabove, provided that you also meet all of these conditions:\n\n    a) The modified work must itself be a software library.\n\n    b) You must cause the files modified to carry prominent notices\n    stating that you changed the files and the date of any change.\n\n    c) You must cause the whole of the work to be licensed at no\n    charge to all third parties under the terms of this License.\n\n    d) If a facility in the modified Library refers to a function or a\n    table of data to be supplied by an application program that uses\n    the facility, other than as an argument passed when the facility\n    is invoked, then you must make a good faith effort to ensure that,\n    in the event an application does not supply such function or\n    table, the facility still operates, and performs whatever part of\n    its purpose remains meaningful.\n\n    (For example, a function in a library to compute square roots has\n    a purpose that is entirely well-defined independent of the\n    application.  Therefore, Subsection 2d requires that any\n    application-supplied function or table used by this function must\n    be optional: if the application does not supply it, the square\n    root function must still compute square roots.)\n\nThese requirements apply to the modified work as a whole.  If\nidentifiable sections of that work are not derived from the Library,\nand can be reasonably considered independent and separate works in\nthemselves, then this License, and its terms, do not apply to those\nsections when you distribute them as separate works.  But when you\ndistribute the same sections as part of a whole which is a work based\non the Library, the distribution of the whole must be on the terms of\nthis License, whose permissions for other licensees extend to the\nentire whole, and thus to each and every part regardless of who wrote\nit.\n\nThus, it is not the intent of this section to claim rights or contest\nyour rights to work written entirely by you; rather, the intent is to\nexercise the right to control the distribution of derivative or\ncollective works based on the Library.\n\nIn addition, mere aggregation of another work not based on the Library\nwith the Library (or with a work based on the Library) on a volume of\na storage or distribution medium does not bring the other work under\nthe scope of this License.\n\n  3. You may opt to apply the terms of the ordinary GNU General Public\nLicense instead of this License to a given copy of the Library.  To do\nthis, you must alter all the notices that refer to this License, so\nthat they refer to the ordinary GNU General Public License, version 2,\ninstead of to this License.  (If a newer version than version 2 of the\nordinary GNU General Public License has appeared, then you can specify\nthat version instead if you wish.)  Do not make any other change in\nthese notices.\n\n  Once this change is made in a given copy, it is irreversible for\nthat copy, so the ordinary GNU General Public License applies to all\nsubsequent copies and derivative works made from that copy.\n\n  This option is useful when you wish to copy part of the code of\nthe Library into a program that is not a library.\n\n  4. You may copy and distribute the Library (or a portion or\nderivative of it, under Section 2) in object code or executable form\nunder the terms of Sections 1 and 2 above provided that you accompany\nit with the complete corresponding machine-readable source code, which\nmust be distributed under the terms of Sections 1 and 2 above on a\nmedium customarily used for software interchange.\n\n  If distribution of object code is made by offering access to copy\nfrom a designated place, then offering equivalent access to copy the\nsource code from the same place satisfies the requirement to\ndistribute the source code, even though third parties are not\ncompelled to copy the source along with the object code.\n\n  5. A program that contains no derivative of any portion of the\nLibrary, but is designed to work with the Library by being compiled or\nlinked with it, is called a \"work that uses the Library\".  Such a\nwork, in isolation, is not a derivative work of the Library, and\ntherefore falls outside the scope of this License.\n\n  However, linking a \"work that uses the Library\" with the Library\ncreates an executable that is a derivative of the Library (because it\ncontains portions of the Library), rather than a \"work that uses the\nlibrary\".  The executable is therefore covered by this License.\nSection 6 states terms for distribution of such executables.\n\n  When a \"work that uses the Library\" uses material from a header file\nthat is part of the Library, the object code for the work may be a\nderivative work of the Library even though the source code is not.\nWhether this is true is especially significant if the work can be\nlinked without the Library, or if the work is itself a library.  The\nthreshold for this to be true is not precisely defined by law.\n\n  If such an object file uses only numerical parameters, data\nstructure layouts and accessors, and small macros and small inline\nfunctions (ten lines or less in length), then the use of the object\nfile is unrestricted, regardless of whether it is legally a derivative\nwork.  (Executables containing this object code plus portions of the\nLibrary will still fall under Section 6.)\n\n  Otherwise, if the work is a derivative of the Library, you may\ndistribute the object code for the work under the terms of Section 6.\nAny executables containing that work also fall under Section 6,\nwhether or not they are linked directly with the Library itself.\n\n  6. As an exception to the Sections above, you may also compile or\nlink a \"work that uses the Library\" with the Library to produce a\nwork containing portions of the Library, and distribute that work\nunder terms of your choice, provided that the terms permit\nmodification of the work for the customer's own use and reverse\nengineering for debugging such modifications.\n\n  You must give prominent notice with each copy of the work that the\nLibrary is used in it and that the Library and its use are covered by\nthis License.  You must supply a copy of this License.  If the work\nduring execution displays copyright notices, you must include the\ncopyright notice for the Library among them, as well as a reference\ndirecting the user to the copy of this License.  Also, you must do one\nof these things:\n\n    a) Accompany the work with the complete corresponding\n    machine-readable source code for the Library including whatever\n    changes were used in the work (which must be distributed under\n    Sections 1 and 2 above); and, if the work is an executable linked\n    with the Library, with the complete machine-readable \"work that\n    uses the Library\", as object code and/or source code, so that the\n    user can modify the Library and then relink to produce a modified\n    executable containing the modified Library.  (It is understood\n    that the user who changes the contents of definitions files in the\n    Library will not necessarily be able to recompile the application\n    to use the modified definitions.)\n\n    b) Accompany the work with a written offer, valid for at\n    least three years, to give the same user the materials\n    specified in Subsection 6a, above, for a charge no more\n    than the cost of performing this distribution.\n\n    c) If distribution of the work is made by offering access to copy\n    from a designated place, offer equivalent access to copy the above\n    specified materials from the same place.\n\n    d) Verify that the user has already received a copy of these\n    materials or that you have already sent this user a copy.\n\n  For an executable, the required form of the \"work that uses the\nLibrary\" must include any data and utility programs needed for\nreproducing the executable from it.  However, as a special exception,\nthe source code distributed need not include anything that is normally\ndistributed (in either source or binary form) with the major\ncomponents (compiler, kernel, and so on) of the operating system on\nwhich the executable runs, unless that component itself accompanies\nthe executable.\n\n  It may happen that this requirement contradicts the license\nrestrictions of other proprietary libraries that do not normally\naccompany the operating system.  Such a contradiction means you cannot\nuse both them and the Library together in an executable that you\ndistribute.\n\n  7. You may place library facilities that are a work based on the\nLibrary side-by-side in a single library together with other library\nfacilities not covered by this License, and distribute such a combined\nlibrary, provided that the separate distribution of the work based on\nthe Library and of the other library facilities is otherwise\npermitted, and provided that you do these two things:\n\n    a) Accompany the combined library with a copy of the same work\n    based on the Library, uncombined with any other library\n    facilities.  This must be distributed under the terms of the\n    Sections above.\n\n    b) Give prominent notice with the combined library of the fact\n    that part of it is a work based on the Library, and explaining\n    where to find the accompanying uncombined form of the same work.\n\n  8. You may not copy, modify, sublicense, link with, or distribute\nthe Library except as expressly provided under this License.  Any\nattempt otherwise to copy, modify, sublicense, link with, or\ndistribute the Library is void, and will automatically terminate your\nrights under this License.  However, parties who have received copies,\nor rights, from you under this License will not have their licenses\nterminated so long as such parties remain in full compliance.\n\n  9. You are not required to accept this License, since you have not\nsigned it.  However, nothing else grants you permission to modify or\ndistribute the Library or its derivative works.  These actions are\nprohibited by law if you do not accept this License.  Therefore, by\nmodifying or distributing the Library (or any work based on the\nLibrary), you indicate your acceptance of this License to do so, and\nall its terms and conditions for copying, distributing or modifying\nthe Library or works based on it.\n\n  10. Each time you redistribute the Library (or any work based on the\nLibrary), the recipient automatically receives a license from the\noriginal licensor to copy, distribute, link with or modify the Library\nsubject to these terms and conditions.  You may not impose any further\nrestrictions on the recipients' exercise of the rights granted herein.\nYou are not responsible for enforcing compliance by third parties to\nthis License.\n\n  11. If, as a consequence of a court judgment or allegation of patent\ninfringement or for any other reason (not limited to patent issues),\nconditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License.  If you cannot\ndistribute so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you\nmay not distribute the Library at all.  For example, if a patent\nlicense would not permit royalty-free redistribution of the Library by\nall those who receive copies directly or indirectly through you, then\nthe only way you could satisfy both it and this License would be to\nrefrain entirely from distribution of the Library.\n\nIf any portion of this section is held invalid or unenforceable under any\nparticular circumstance, the balance of the section is intended to apply,\nand the section as a whole is intended to apply in other circumstances.\n\nIt is not the purpose of this section to induce you to infringe any\npatents or other property right claims or to contest validity of any\nsuch claims; this section has the sole purpose of protecting the\nintegrity of the free software distribution system which is\nimplemented by public license practices.  Many people have made\ngenerous contributions to the wide range of software distributed\nthrough that system in reliance on consistent application of that\nsystem; it is up to the author/donor to decide if he or she is willing\nto distribute software through any other system and a licensee cannot\nimpose that choice.\n\nThis section is intended to make thoroughly clear what is believed to\nbe a consequence of the rest of this License.\n\n  12. If the distribution and/or use of the Library is restricted in\ncertain countries either by patents or by copyrighted interfaces, the\noriginal copyright holder who places the Library under this License may add\nan explicit geographical distribution limitation excluding those countries,\nso that distribution is permitted only in or among countries not thus\nexcluded.  In such case, this License incorporates the limitation as if\nwritten in the body of this License.\n\n  13. The Free Software Foundation may publish revised and/or new\nversions of the Library General Public License from time to time.\nSuch new versions will be similar in spirit to the present version,\nbut may differ in detail to address new problems or concerns.\n\nEach version is given a distinguishing version number.  If the Library\nspecifies a version number of this License which applies to it and\n\"any later version\", you have the option of following the terms and\nconditions either of that version or of any later version published by\nthe Free Software Foundation.  If the Library does not specify a\nlicense version number, you may choose any version ever published by\nthe Free Software Foundation.\n\n  14. If you wish to incorporate parts of the Library into other free\nprograms whose distribution conditions are incompatible with these,\nwrite to the author to ask for permission.  For software which is\ncopyrighted by the Free Software Foundation, write to the Free\nSoftware Foundation; we sometimes make exceptions for this.  Our\ndecision will be guided by the two goals of preserving the free status\nof all derivatives of our free software and of promoting the sharing\nand reuse of software generally.\n\n\t\t\t    NO WARRANTY\n\n  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO\nWARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.\nEXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR\nOTHER PARTIES PROVIDE THE LIBRARY \"AS IS\" WITHOUT WARRANTY OF ANY\nKIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\nPURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE\nLIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME\nTHE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.\n\n  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN\nWRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY\nAND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU\nFOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR\nCONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE\nLIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING\nRENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A\nFAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF\nSUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH\nDAMAGES.\n\n\t\t     END OF TERMS AND CONDITIONS\n\n     Appendix: How to Apply These Terms to Your New Libraries\n\n  If you develop a new library, and you want it to be of the greatest\npossible use to the public, we recommend making it free software that\neveryone can redistribute and change.  You can do so by permitting\nredistribution under these terms (or, alternatively, under the terms of the\nordinary General Public License).\n\n  To apply these terms, attach the following notices to the library.  It is\nsafest to attach them to the start of each source file to most effectively\nconvey the exclusion of warranty; and each file should have at least the\n\"copyright\" line and a pointer to where the full notice is found.\n\n    <one line to give the library's name and a brief idea of what it does.>\n    Copyright (C) <year>  <name of author>\n\n    This library is free software; you can redistribute it and/or\n    modify it under the terms of the GNU Library General Public\n    License as published by the Free Software Foundation; either\n    version 2 of the License, or (at your option) any later version.\n\n    This library is distributed in the hope that it will be useful,\n    but WITHOUT ANY WARRANTY; without even the implied warranty of\n    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU\n    Library General Public License for more details.\n\n    You should have received a copy of the GNU Library General Public\n    License along with this library; if not, write to the Free\n    Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.\n\nAlso add information on how to contact you by electronic and paper mail.\n\nYou should also get your employer (if you work as a programmer) or your\nschool, if any, to sign a \"copyright disclaimer\" for the library, if\nnecessary.  Here is a sample; alter the names:\n\n  Yoyodyne, Inc., hereby disclaims all copyright interest in the\n  library `Frob' (a library for tweaking knobs) written by James Random Hacker.\n\n  <signature of Ty Coon>, 1 April 1990\n  Ty Coon, President of Vice\n\nThat's all there is to it!\n================================================================================\n\ndocker 4.4.1  (Provided under following license)\n===============================================================================\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   Copyright 2016 Docker, Inc.\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n================================================================================\n\njinja 2.11.2  (Provided under following license)\n===============================================================================\n\nCopyright 2007 Pallets\n\nRedistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:\n\n    Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.\n    Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.\n    Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n================================================================================\n\nmarkupsafe 1.1.1  (Provided under following license)\n===============================================================================\n\nCopyright 2010 Pallets\n\nRedistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:\n\n    Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.\n    Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.\n    Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n================================================================================\n\nurllib3 1.26.2 (Provided under following license)\n===============================================================================\n\nMIT License\n\nCopyright (c) 2008-2020 Andrey Petrov and contributors (see CONTRIBUTORS.txt)\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n\n================================================================================\n\npyyaml 5.3.1 (Provided under following license)\n===============================================================================\n\nCopyright (c) 2017-2021 Ingy döt Net\nCopyright (c) 2006-2016 Kirill Simonov\n\nPermission is hereby granted, free of charge, to any person obtaining a copy of\nthis software and associated documentation files (the \"Software\"), to deal in\nthe Software without restriction, including without limitation the rights to\nuse, copy, modify, merge, publish, distribute, sublicense, and/or sell copies\nof the Software, and to permit persons to whom the Software is furnished to do\nso, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n\n================================================================================\n\ntyping 3.7.4.3  (Provided under following license)\n===============================================================================\n\nA. HISTORY OF THE SOFTWARE\n==========================\n\nPython was created in the early 1990s by Guido van Rossum at Stichting\nMathematisch Centrum (CWI, see http://www.cwi.nl) in the Netherlands\nas a successor of a language called ABC.  Guido remains Python's\nprincipal author, although it includes many contributions from others.\n\nIn 1995, Guido continued his work on Python at the Corporation for\nNational Research Initiatives (CNRI, see http://www.cnri.reston.va.us)\nin Reston, Virginia where he released several versions of the\nsoftware.\n\nIn May 2000, Guido and the Python core development team moved to\nBeOpen.com to form the BeOpen PythonLabs team.  In October of the same\nyear, the PythonLabs team moved to Digital Creations (now Zope\nCorporation, see http://www.zope.com).  In 2001, the Python Software\nFoundation (PSF, see http://www.python.org/psf/) was formed, a\nnon-profit organization created specifically to own Python-related\nIntellectual Property.  Zope Corporation is a sponsoring member of\nthe PSF.\n\nAll Python releases are Open Source (see http://www.opensource.org for\nthe Open Source Definition).  Historically, most, but not all, Python\nreleases have also been GPL-compatible; the table below summarizes\nthe various releases.\n\n    Release         Derived     Year        Owner       GPL-\n                    from                                compatible? (1)\n\n    0.9.0 thru 1.2              1991-1995   CWI         yes\n    1.3 thru 1.5.2  1.2         1995-1999   CNRI        yes\n    1.6             1.5.2       2000        CNRI        no\n    2.0             1.6         2000        BeOpen.com  no\n    1.6.1           1.6         2001        CNRI        yes (2)\n    2.1             2.0+1.6.1   2001        PSF         no\n    2.0.1           2.0+1.6.1   2001        PSF         yes\n    2.1.1           2.1+2.0.1   2001        PSF         yes\n    2.1.2           2.1.1       2002        PSF         yes\n    2.1.3           2.1.2       2002        PSF         yes\n    2.2 and above   2.1.1       2001-now    PSF         yes\n\nFootnotes:\n\n(1) GPL-compatible doesn't mean that we're distributing Python under\n    the GPL.  All Python licenses, unlike the GPL, let you distribute\n    a modified version without making your changes open source.  The\n    GPL-compatible licenses make it possible to combine Python with\n    other software that is released under the GPL; the others don't.\n\n(2) According to Richard Stallman, 1.6.1 is not GPL-compatible,\n    because its license has a choice of law clause.  According to\n    CNRI, however, Stallman's lawyer has told CNRI's lawyer that 1.6.1\n    is \"not incompatible\" with the GPL.\n\nThanks to the many outside volunteers who have worked under Guido's\ndirection to make these releases possible.\n\n\nB. TERMS AND CONDITIONS FOR ACCESSING OR OTHERWISE USING PYTHON\n===============================================================\n\nPYTHON SOFTWARE FOUNDATION LICENSE VERSION 2\n--------------------------------------------\n\n1. This LICENSE AGREEMENT is between the Python Software Foundation\n(\"PSF\"), and the Individual or Organization (\"Licensee\") accessing and\notherwise using this software (\"Python\") in source or binary form and\nits associated documentation.\n\n2. Subject to the terms and conditions of this License Agreement, PSF hereby\ngrants Licensee a nonexclusive, royalty-free, world-wide license to reproduce,\nanalyze, test, perform and/or display publicly, prepare derivative works,\ndistribute, and otherwise use Python alone or in any derivative version,\nprovided, however, that PSF's License Agreement and PSF's notice of copyright,\ni.e., \"Copyright (c) 2001, 2002, 2003, 2004, 2005, 2006, 2007, 2008, 2009, 2010,\n2011, 2012, 2013, 2014 Python Software Foundation; All Rights Reserved\" are\nretained in Python alone or in any derivative version prepared by Licensee.\n\n3. In the event Licensee prepares a derivative work that is based on\nor incorporates Python or any part thereof, and wants to make\nthe derivative work available to others as provided herein, then\nLicensee hereby agrees to include in any such work a brief summary of\nthe changes made to Python.\n\n4. PSF is making Python available to Licensee on an \"AS IS\"\nbasis.  PSF MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR\nIMPLIED.  BY WAY OF EXAMPLE, BUT NOT LIMITATION, PSF MAKES NO AND\nDISCLAIMS ANY REPRESENTATION OR WARRANTY OF MERCHANTABILITY OR FITNESS\nFOR ANY PARTICULAR PURPOSE OR THAT THE USE OF PYTHON WILL NOT\nINFRINGE ANY THIRD PARTY RIGHTS.\n\n5. PSF SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF PYTHON\nFOR ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR LOSS AS\nA RESULT OF MODIFYING, DISTRIBUTING, OR OTHERWISE USING PYTHON,\nOR ANY DERIVATIVE THEREOF, EVEN IF ADVISED OF THE POSSIBILITY THEREOF.\n\n6. This License Agreement will automatically terminate upon a material\nbreach of its terms and conditions.\n\n7. Nothing in this License Agreement shall be deemed to create any\nrelationship of agency, partnership, or joint venture between PSF and\nLicensee.  This License Agreement does not grant permission to use PSF\ntrademarks or trade name in a trademark sense to endorse or promote\nproducts or services of Licensee, or any third party.\n\n8. By copying, installing or otherwise using Python, Licensee\nagrees to be bound by the terms and conditions of this License\nAgreement.\n\n\nBEOPEN.COM LICENSE AGREEMENT FOR PYTHON 2.0\n-------------------------------------------\n\nBEOPEN PYTHON OPEN SOURCE LICENSE AGREEMENT VERSION 1\n\n1. This LICENSE AGREEMENT is between BeOpen.com (\"BeOpen\"), having an\noffice at 160 Saratoga Avenue, Santa Clara, CA 95051, and the\nIndividual or Organization (\"Licensee\") accessing and otherwise using\nthis software in source or binary form and its associated\ndocumentation (\"the Software\").\n\n2. Subject to the terms and conditions of this BeOpen Python License\nAgreement, BeOpen hereby grants Licensee a non-exclusive,\nroyalty-free, world-wide license to reproduce, analyze, test, perform\nand/or display publicly, prepare derivative works, distribute, and\notherwise use the Software alone or in any derivative version,\nprovided, however, that the BeOpen Python License is retained in the\nSoftware, alone or in any derivative version prepared by Licensee.\n\n3. BeOpen is making the Software available to Licensee on an \"AS IS\"\nbasis.  BEOPEN MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR\nIMPLIED.  BY WAY OF EXAMPLE, BUT NOT LIMITATION, BEOPEN MAKES NO AND\nDISCLAIMS ANY REPRESENTATION OR WARRANTY OF MERCHANTABILITY OR FITNESS\nFOR ANY PARTICULAR PURPOSE OR THAT THE USE OF THE SOFTWARE WILL NOT\nINFRINGE ANY THIRD PARTY RIGHTS.\n\n4. BEOPEN SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF THE\nSOFTWARE FOR ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR LOSS\nAS A RESULT OF USING, MODIFYING OR DISTRIBUTING THE SOFTWARE, OR ANY\nDERIVATIVE THEREOF, EVEN IF ADVISED OF THE POSSIBILITY THEREOF.\n\n5. This License Agreement will automatically terminate upon a material\nbreach of its terms and conditions.\n\n6. This License Agreement shall be governed by and interpreted in all\nrespects by the law of the State of California, excluding conflict of\nlaw provisions.  Nothing in this License Agreement shall be deemed to\ncreate any relationship of agency, partnership, or joint venture\nbetween BeOpen and Licensee.  This License Agreement does not grant\npermission to use BeOpen trademarks or trade names in a trademark\nsense to endorse or promote products or services of Licensee, or any\nthird party.  As an exception, the \"BeOpen Python\" logos available at\nhttp://www.pythonlabs.com/logos.html may be used according to the\npermissions granted on that web page.\n\n7. By copying, installing or otherwise using the software, Licensee\nagrees to be bound by the terms and conditions of this License\nAgreement.\n\n\nCNRI LICENSE AGREEMENT FOR PYTHON 1.6.1\n---------------------------------------\n\n1. This LICENSE AGREEMENT is between the Corporation for National\nResearch Initiatives, having an office at 1895 Preston White Drive,\nReston, VA 20191 (\"CNRI\"), and the Individual or Organization\n(\"Licensee\") accessing and otherwise using Python 1.6.1 software in\nsource or binary form and its associated documentation.\n\n2. Subject to the terms and conditions of this License Agreement, CNRI\nhereby grants Licensee a nonexclusive, royalty-free, world-wide\nlicense to reproduce, analyze, test, perform and/or display publicly,\nprepare derivative works, distribute, and otherwise use Python 1.6.1\nalone or in any derivative version, provided, however, that CNRI's\nLicense Agreement and CNRI's notice of copyright, i.e., \"Copyright (c)\n1995-2001 Corporation for National Research Initiatives; All Rights\nReserved\" are retained in Python 1.6.1 alone or in any derivative\nversion prepared by Licensee.  Alternately, in lieu of CNRI's License\nAgreement, Licensee may substitute the following text (omitting the\nquotes): \"Python 1.6.1 is made available subject to the terms and\nconditions in CNRI's License Agreement.  This Agreement together with\nPython 1.6.1 may be located on the Internet using the following\nunique, persistent identifier (known as a handle): 1895.22/1013.  This\nAgreement may also be obtained from a proxy server on the Internet\nusing the following URL: http://hdl.handle.net/1895.22/1013\".\n\n3. In the event Licensee prepares a derivative work that is based on\nor incorporates Python 1.6.1 or any part thereof, and wants to make\nthe derivative work available to others as provided herein, then\nLicensee hereby agrees to include in any such work a brief summary of\nthe changes made to Python 1.6.1.\n\n4. CNRI is making Python 1.6.1 available to Licensee on an \"AS IS\"\nbasis.  CNRI MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR\nIMPLIED.  BY WAY OF EXAMPLE, BUT NOT LIMITATION, CNRI MAKES NO AND\nDISCLAIMS ANY REPRESENTATION OR WARRANTY OF MERCHANTABILITY OR FITNESS\nFOR ANY PARTICULAR PURPOSE OR THAT THE USE OF PYTHON 1.6.1 WILL NOT\nINFRINGE ANY THIRD PARTY RIGHTS.\n\n5. CNRI SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF PYTHON\n1.6.1 FOR ANY INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR LOSS AS\nA RESULT OF MODIFYING, DISTRIBUTING, OR OTHERWISE USING PYTHON 1.6.1,\nOR ANY DERIVATIVE THEREOF, EVEN IF ADVISED OF THE POSSIBILITY THEREOF.\n\n6. This License Agreement will automatically terminate upon a material\nbreach of its terms and conditions.\n\n7. This License Agreement shall be governed by the federal\nintellectual property law of the United States, including without\nlimitation the federal copyright law, and, to the extent such\nU.S. federal law does not apply, by the law of the Commonwealth of\nVirginia, excluding Virginia's conflict of law provisions.\nNotwithstanding the foregoing, with regard to derivative works based\non Python 1.6.1 that incorporate non-separable material that was\npreviously distributed under the GNU General Public License (GPL), the\nlaw of the Commonwealth of Virginia shall govern this License\nAgreement only as to issues arising under or with respect to\nParagraphs 4, 5, and 7 of this License Agreement.  Nothing in this\nLicense Agreement shall be deemed to create any relationship of\nagency, partnership, or joint venture between CNRI and Licensee.  This\nLicense Agreement does not grant permission to use CNRI trademarks or\ntrade name in a trademark sense to endorse or promote products or\nservices of Licensee, or any third party.\n\n8. By clicking on the \"ACCEPT\" button where indicated, or by copying,\ninstalling or otherwise using Python 1.6.1, Licensee agrees to be\nbound by the terms and conditions of this License Agreement.\n\n        ACCEPT\n\n\nCWI LICENSE AGREEMENT FOR PYTHON 0.9.0 THROUGH 1.2\n--------------------------------------------------\n\nCopyright (c) 1991 - 1995, Stichting Mathematisch Centrum Amsterdam,\nThe Netherlands.  All rights reserved.\n\nPermission to use, copy, modify, and distribute this software and its\ndocumentation for any purpose and without fee is hereby granted,\nprovided that the above copyright notice appear in all copies and that\nboth that copyright notice and this permission notice appear in\nsupporting documentation, and that the name of Stichting Mathematisch\nCentrum or CWI not be used in advertising or publicity pertaining to\ndistribution of the software without specific, written prior\npermission.\n\nSTICHTING MATHEMATISCH CENTRUM DISCLAIMS ALL WARRANTIES WITH REGARD TO\nTHIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND\nFITNESS, IN NO EVENT SHALL STICHTING MATHEMATISCH CENTRUM BE LIABLE\nFOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES\nWHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN\nACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT\nOF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.\n\n================================================================================\n\nyaml-cpp 0.6.3 (Provided under following license)\n===============================================================================\n\nCopyright (c) 2008-2015 Jesse Beder.\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in\nall copies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN\nTHE SOFTWARE.\n================================================================================\n\ngflags 2.2.1 (Provided under following license)\n===============================================================================\n\nCopyright (c) 2006, Google Inc.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are\nmet:\n\n    * Redistributions of source code must retain the above copyright\nnotice, this list of conditions and the following disclaimer.\n    * Redistributions in binary form must reproduce the above\ncopyright notice, this list of conditions and the following disclaimer\nin the documentation and/or other materials provided with the\ndistribution.\n    * Neither the name of Google Inc. nor the names of its\ncontributors may be used to endorse or promote products derived from\nthis software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR\nA PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\nOWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\nSPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT\nLIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,\nDATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY\nTHEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n================================================================================\n\ngtest 1.8.0 (Provided under following license)\n===============================================================================\nCopyright 2008, Google Inc.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are\nmet:\n\n    * Redistributions of source code must retain the above copyright\nnotice, this list of conditions and the following disclaimer.\n    * Redistributions in binary form must reproduce the above\ncopyright notice, this list of conditions and the following disclaimer\nin the documentation and/or other materials provided with the\ndistribution.\n    * Neither the name of Google Inc. nor the names of its\ncontributors may be used to endorse or promote products derived from\nthis software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR\nA PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\nOWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\nSPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT\nLIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,\nDATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY\nTHEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n================================================================================\n\nrules_python 0.1.0 (Provided under following license)\n===============================================================================\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"{}\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright {yyyy} {name of copyright owner}\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n\n================================================================================\n\nDCMI specification (Provided under following license)\n===============================================================================\n\n\nCreative Commons Attribution 4.0 International Public License\n\nBy exercising the Licensed Rights (defined below), You accept and agree to be bound by the terms and conditions of this Creative Commons Attribution 4.0 International Public License (\"Public License\"). To the extent this Public License may be interpreted as a contract, You are granted the Licensed Rights in consideration of Your acceptance of these terms and conditions, and the Licensor grants You such rights in consideration of benefits the Licensor receives from making the Licensed Material available under these terms and conditions.\n\nSection 1 – Definitions.\n\n    Adapted Material means material subject to Copyright and Similar Rights that is derived from or based upon the Licensed Material and in which the Licensed Material is translated, altered, arranged, transformed, or otherwise modified in a manner requiring permission under the Copyright and Similar Rights held by the Licensor. For purposes of this Public License, where the Licensed Material is a musical work, performance, or sound recording, Adapted Material is always produced where the Licensed Material is synched in timed relation with a moving image.\n    Adapter's License means the license You apply to Your Copyright and Similar Rights in Your contributions to Adapted Material in accordance with the terms and conditions of this Public License.\n    Copyright and Similar Rights means copyright and/or similar rights closely related to copyright including, without limitation, performance, broadcast, sound recording, and Sui Generis Database Rights, without regard to how the rights are labeled or categorized. For purposes of this Public License, the rights specified in Section 2(b)(1)-(2) are not Copyright and Similar Rights.\n    Effective Technological Measures means those measures that, in the absence of proper authority, may not be circumvented under laws fulfilling obligations under Article 11 of the WIPO Copyright Treaty adopted on December 20, 1996, and/or similar international agreements.\n    Exceptions and Limitations means fair use, fair dealing, and/or any other exception or limitation to Copyright and Similar Rights that applies to Your use of the Licensed Material.\n    Licensed Material means the artistic or literary work, database, or other material to which the Licensor applied this Public License.\n    Licensed Rights means the rights granted to You subject to the terms and conditions of this Public License, which are limited to all Copyright and Similar Rights that apply to Your use of the Licensed Material and that the Licensor has authority to license.\n    Licensor means the individual(s) or entity(ies) granting rights under this Public License.\n    Share means to provide material to the public by any means or process that requires permission under the Licensed Rights, such as reproduction, public display, public performance, distribution, dissemination, communication, or importation, and to make material available to the public including in ways that members of the public may access the material from a place and at a time individually chosen by them.\n    Sui Generis Database Rights means rights other than copyright resulting from Directive 96/9/EC of the European Parliament and of the Council of 11 March 1996 on the legal protection of databases, as amended and/or succeeded, as well as other essentially equivalent rights anywhere in the world.\n    You means the individual or entity exercising the Licensed Rights under this Public License. Your has a corresponding meaning.\n\nSection 2 – Scope.\n\n    License grant.\n        Subject to the terms and conditions of this Public License, the Licensor hereby grants You a worldwide, royalty-free, non-sublicensable, non-exclusive, irrevocable license to exercise the Licensed Rights in the Licensed Material to:\n            reproduce and Share the Licensed Material, in whole or in part; and\n            produce, reproduce, and Share Adapted Material.\n        Exceptions and Limitations. For the avoidance of doubt, where Exceptions and Limitations apply to Your use, this Public License does not apply, and You do not need to comply with its terms and conditions.\n        Term. The term of this Public License is specified in Section 6(a).\n        Media and formats; technical modifications allowed. The Licensor authorizes You to exercise the Licensed Rights in all media and formats whether now known or hereafter created, and to make technical modifications necessary to do so. The Licensor waives and/or agrees not to assert any right or authority to forbid You from making technical modifications necessary to exercise the Licensed Rights, including technical modifications necessary to circumvent Effective Technological Measures. For purposes of this Public License, simply making modifications authorized by this Section 2(a)(4) never produces Adapted Material.\n        Downstream recipients.\n            Offer from the Licensor – Licensed Material. Every recipient of the Licensed Material automatically receives an offer from the Licensor to exercise the Licensed Rights under the terms and conditions of this Public License.\n            No downstream restrictions. You may not offer or impose any additional or different terms or conditions on, or apply any Effective Technological Measures to, the Licensed Material if doing so restricts exercise of the Licensed Rights by any recipient of the Licensed Material.\n        No endorsement. Nothing in this Public License constitutes or may be construed as permission to assert or imply that You are, or that Your use of the Licensed Material is, connected with, or sponsored, endorsed, or granted official status by, the Licensor or others designated to receive attribution as provided in Section 3(a)(1)(A)(i).\n\n    Other rights.\n        Moral rights, such as the right of integrity, are not licensed under this Public License, nor are publicity, privacy, and/or other similar personality rights; however, to the extent possible, the Licensor waives and/or agrees not to assert any such rights held by the Licensor to the limited extent necessary to allow You to exercise the Licensed Rights, but not otherwise.\n        Patent and trademark rights are not licensed under this Public License.\n        To the extent possible, the Licensor waives any right to collect royalties from You for the exercise of the Licensed Rights, whether directly or through a collecting society under any voluntary or waivable statutory or compulsory licensing scheme. In all other cases the Licensor expressly reserves any right to collect such royalties.\n\nSection 3 – License Conditions.\n\nYour exercise of the Licensed Rights is expressly made subject to the following conditions.\n\n    Attribution.\n\n        If You Share the Licensed Material (including in modified form), You must:\n            retain the following if it is supplied by the Licensor with the Licensed Material:\n                identification of the creator(s) of the Licensed Material and any others designated to receive attribution, in any reasonable manner requested by the Licensor (including by pseudonym if designated);\n                a copyright notice;\n                a notice that refers to this Public License;\n                a notice that refers to the disclaimer of warranties;\n                a URI or hyperlink to the Licensed Material to the extent reasonably practicable;\n            indicate if You modified the Licensed Material and retain an indication of any previous modifications; and\n            indicate the Licensed Material is licensed under this Public License, and include the text of, or the URI or hyperlink to, this Public License.\n        You may satisfy the conditions in Section 3(a)(1) in any reasonable manner based on the medium, means, and context in which You Share the Licensed Material. For example, it may be reasonable to satisfy the conditions by providing a URI or hyperlink to a resource that includes the required information.\n        If requested by the Licensor, You must remove any of the information required by Section 3(a)(1)(A) to the extent reasonably practicable.\n        If You Share Adapted Material You produce, the Adapter's License You apply must not prevent recipients of the Adapted Material from complying with this Public License.\n\nSection 4 – Sui Generis Database Rights.\n\nWhere the Licensed Rights include Sui Generis Database Rights that apply to Your use of the Licensed Material:\n\n    for the avoidance of doubt, Section 2(a)(1) grants You the right to extract, reuse, reproduce, and Share all or a substantial portion of the contents of the database;\n    if You include all or a substantial portion of the database contents in a database in which You have Sui Generis Database Rights, then the database in which You have Sui Generis Database Rights (but not its individual contents) is Adapted Material; and\n    You must comply with the conditions in Section 3(a) if You Share all or a substantial portion of the contents of the database.\n\nFor the avoidance of doubt, this Section 4 supplements and does not replace Your obligations under this Public License where the Licensed Rights include other Copyright and Similar Rights.\n\nSection 5 – Disclaimer of Warranties and Limitation of Liability.\n\n    Unless otherwise separately undertaken by the Licensor, to the extent possible, the Licensor offers the Licensed Material as-is and as-available, and makes no representations or warranties of any kind concerning the Licensed Material, whether express, implied, statutory, or other. This includes, without limitation, warranties of title, merchantability, fitness for a particular purpose, non-infringement, absence of latent or other defects, accuracy, or the presence or absence of errors, whether or not known or discoverable. Where disclaimers of warranties are not allowed in full or in part, this disclaimer may not apply to You.\n    To the extent possible, in no event will the Licensor be liable to You on any legal theory (including, without limitation, negligence) or otherwise for any direct, special, indirect, incidental, consequential, punitive, exemplary, or other losses, costs, expenses, or damages arising out of this Public License or use of the Licensed Material, even if the Licensor has been advised of the possibility of such losses, costs, expenses, or damages. Where a limitation of liability is not allowed in full or in part, this limitation may not apply to You.\n\n    The disclaimer of warranties and limitation of liability provided above shall be interpreted in a manner that, to the extent possible, most closely approximates an absolute disclaimer and waiver of all liability.\n\nSection 6 – Term and Termination.\n\n    This Public License applies for the term of the Copyright and Similar Rights licensed here. However, if You fail to comply with this Public License, then Your rights under this Public License terminate automatically.\n\n    Where Your right to use the Licensed Material has terminated under Section 6(a), it reinstates:\n        automatically as of the date the violation is cured, provided it is cured within 30 days of Your discovery of the violation; or\n        upon express reinstatement by the Licensor.\n    For the avoidance of doubt, this Section 6(b) does not affect any right the Licensor may have to seek remedies for Your violations of this Public License.\n    For the avoidance of doubt, the Licensor may also offer the Licensed Material under separate terms or conditions or stop distributing the Licensed Material at any time; however, doing so will not terminate this Public License.\n    Sections 1, 5, 6, 7, and 8 survive termination of this Public License.\n\nSection 7 – Other Terms and Conditions.\n\n    The Licensor shall not be bound by any additional or different terms or conditions communicated by You unless expressly agreed.\n    Any arrangements, understandings, or agreements regarding the Licensed Material not stated herein are separate from and independent of the terms and conditions of this Public License.\n\nSection 8 – Interpretation.\n\n    For the avoidance of doubt, this Public License does not, and shall not be interpreted to, reduce, limit, restrict, or impose conditions on any use of the Licensed Material that could lawfully be made without permission under this Public License.\n    To the extent possible, if any provision of this Public License is deemed unenforceable, it shall be automatically reformed to the minimum extent necessary to make it enforceable. If the provision cannot be reformed, it shall be severed from this Public License without affecting the enforceability of the remaining terms and conditions.\n    No term or condition of this Public License will be waived and no failure to comply consented to unless expressly agreed to by the Licensor.\n    Nothing in this Public License constitutes or may be interpreted as a limitation upon, or waiver of, any privileges and immunities that apply to the Licensor or You, including from the legal processes of any jurisdiction or authority.\n\n===============================================================================\n\nbreakpad (Provided under following license)\n===============================================================================\n\nCopyright (c) 2006, Google Inc.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are\nmet:\n\n    * Redistributions of source code must retain the above copyright\nnotice, this list of conditions and the following disclaimer.\n    * Redistributions in binary form must reproduce the above\ncopyright notice, this list of conditions and the following disclaimer\nin the documentation and/or other materials provided with the\ndistribution.\n    * Neither the name of Google Inc. nor the names of its\ncontributors may be used to endorse or promote products derived from\nthis software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR\nA PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\nOWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\nSPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT\nLIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,\nDATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY\nTHEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n--------------------------------------------------------------------\n\nCopyright 2001-2004 Unicode, Inc.\n\nDisclaimer\n\nThis source code is provided as is by Unicode, Inc. No claims are\nmade as to fitness for any particular purpose. No warranties of any\nkind are expressed or implied. The recipient agrees to determine\napplicability of information provided. If this file has been\npurchased on magnetic or optical media from Unicode, Inc., the\nsole remedy for any claim will be exchange of defective media\nwithin 90 days of receipt.\n\nLimitations on Rights to Redistribute This Code\n\nUnicode, Inc. hereby grants the right to freely use the information\nsupplied in this file in the creation of products supporting the\nUnicode Standard, and to make copies of this file in any form\nfor internal or external distribution as long as this notice\nremains attached.\n\n================================================================================\n\nlinux-syscall-support (lss) (Provided under following license)\n===============================================================================\n\nCopyright (c) 2005-2011, Google Inc.\nAll rights reserved.\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are\nmet:\n   Redistributions of source code must retain the above copyright\nnotice, this list of conditions and the following disclaimer.\n   Redistributions in binary form must reproduce the above\ncopyright notice, this list of conditions and the following disclaimer\nin the documentation and/or other materials provided with the\ndistribution.\n   Neither the name of Google Inc. nor the names of its\ncontributors may be used to endorse or promote products derived from\nthis software without specific prior written permission.\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR\nA PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\nOWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\nSPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT\nLIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,\nDATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY\nTHEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n================================================================================\n\nbazel skylib 1.0.2 (Provided under following license)\n================================================================================\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"[]\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright [yyyy] [name of copyright owner]\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n\n================================================================================\n\nnlohmann-json 3.10.4 (Provided under following license)\n================================================================================\nMIT License\n\nCopyright (c) 2013-2021 Niels Lohmann\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n================================================================================\n\nurwid 2.1.2 (Provided under following license)\n================================================================================\n GNU LESSER GENERAL PUBLIC LICENSE\n\t\t       Version 2.1, February 1999\n\n Copyright (C) 1991, 1999 Free Software Foundation, Inc.\n 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA\n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n[This is the first released version of the Lesser GPL.  It also counts\n as the successor of the GNU Library Public License, version 2, hence\n the version number 2.1.]\n\n\t\t\t    Preamble\n\n  The licenses for most software are designed to take away your\nfreedom to share and change it.  By contrast, the GNU General Public\nLicenses are intended to guarantee your freedom to share and change\nfree software--to make sure the software is free for all its users.\n\n  This license, the Lesser General Public License, applies to some\nspecially designated software packages--typically libraries--of the\nFree Software Foundation and other authors who decide to use it.  You\ncan use it too, but we suggest you first think carefully about whether\nthis license or the ordinary General Public License is the better\nstrategy to use in any particular case, based on the explanations below.\n\n  When we speak of free software, we are referring to freedom of use,\nnot price.  Our General Public Licenses are designed to make sure that\nyou have the freedom to distribute copies of free software (and charge\nfor this service if you wish); that you receive source code or can get\nit if you want it; that you can change the software and use pieces of\nit in new free programs; and that you are informed that you can do\nthese things.\n\n  To protect your rights, we need to make restrictions that forbid\ndistributors to deny you these rights or to ask you to surrender these\nrights.  These restrictions translate to certain responsibilities for\nyou if you distribute copies of the library or if you modify it.\n\n  For example, if you distribute copies of the library, whether gratis\nor for a fee, you must give the recipients all the rights that we gave\nyou.  You must make sure that they, too, receive or can get the source\ncode.  If you link other code with the library, you must provide\ncomplete object files to the recipients, so that they can relink them\nwith the library after making changes to the library and recompiling\nit.  And you must show them these terms so they know their rights.\n\n  We protect your rights with a two-step method: (1) we copyright the\nlibrary, and (2) we offer you this license, which gives you legal\npermission to copy, distribute and/or modify the library.\n\n  To protect each distributor, we want to make it very clear that\nthere is no warranty for the free library.  Also, if the library is\nmodified by someone else and passed on, the recipients should know\nthat what they have is not the original version, so that the original\nauthor's reputation will not be affected by problems that might be\nintroduced by others.\n\f\n  Finally, software patents pose a constant threat to the existence of\nany free program.  We wish to make sure that a company cannot\neffectively restrict the users of a free program by obtaining a\nrestrictive license from a patent holder.  Therefore, we insist that\nany patent license obtained for a version of the library must be\nconsistent with the full freedom of use specified in this license.\n\n  Most GNU software, including some libraries, is covered by the\nordinary GNU General Public License.  This license, the GNU Lesser\nGeneral Public License, applies to certain designated libraries, and\nis quite different from the ordinary General Public License.  We use\nthis license for certain libraries in order to permit linking those\nlibraries into non-free programs.\n\n  When a program is linked with a library, whether statically or using\na shared library, the combination of the two is legally speaking a\ncombined work, a derivative of the original library.  The ordinary\nGeneral Public License therefore permits such linking only if the\nentire combination fits its criteria of freedom.  The Lesser General\nPublic License permits more lax criteria for linking other code with\nthe library.\n\n  We call this license the \"Lesser\" General Public License because it\ndoes Less to protect the user's freedom than the ordinary General\nPublic License.  It also provides other free software developers Less\nof an advantage over competing non-free programs.  These disadvantages\nare the reason we use the ordinary General Public License for many\nlibraries.  However, the Lesser license provides advantages in certain\nspecial circumstances.\n\n  For example, on rare occasions, there may be a special need to\nencourage the widest possible use of a certain library, so that it becomes\na de-facto standard.  To achieve this, non-free programs must be\nallowed to use the library.  A more frequent case is that a free\nlibrary does the same job as widely used non-free libraries.  In this\ncase, there is little to gain by limiting the free library to free\nsoftware only, so we use the Lesser General Public License.\n\n  In other cases, permission to use a particular library in non-free\nprograms enables a greater number of people to use a large body of\nfree software.  For example, permission to use the GNU C Library in\nnon-free programs enables many more people to use the whole GNU\noperating system, as well as its variant, the GNU/Linux operating\nsystem.\n\n  Although the Lesser General Public License is Less protective of the\nusers' freedom, it does ensure that the user of a program that is\nlinked with the Library has the freedom and the wherewithal to run\nthat program using a modified version of the Library.\n\n  The precise terms and conditions for copying, distribution and\nmodification follow.  Pay close attention to the difference between a\n\"work based on the library\" and a \"work that uses the library\".  The\nformer contains code derived from the library, whereas the latter must\nbe combined with the library in order to run.\n\f\n\t\t  GNU LESSER GENERAL PUBLIC LICENSE\n   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION\n\n  0. This License Agreement applies to any software library or other\nprogram which contains a notice placed by the copyright holder or\nother authorized party saying it may be distributed under the terms of\nthis Lesser General Public License (also called \"this License\").\nEach licensee is addressed as \"you\".\n\n  A \"library\" means a collection of software functions and/or data\nprepared so as to be conveniently linked with application programs\n(which use some of those functions and data) to form executables.\n\n  The \"Library\", below, refers to any such software library or work\nwhich has been distributed under these terms.  A \"work based on the\nLibrary\" means either the Library or any derivative work under\ncopyright law: that is to say, a work containing the Library or a\nportion of it, either verbatim or with modifications and/or translated\nstraightforwardly into another language.  (Hereinafter, translation is\nincluded without limitation in the term \"modification\".)\n\n  \"Source code\" for a work means the preferred form of the work for\nmaking modifications to it.  For a library, complete source code means\nall the source code for all modules it contains, plus any associated\ninterface definition files, plus the scripts used to control compilation\nand installation of the library.\n\n  Activities other than copying, distribution and modification are not\ncovered by this License; they are outside its scope.  The act of\nrunning a program using the Library is not restricted, and output from\nsuch a program is covered only if its contents constitute a work based\non the Library (independent of the use of the Library in a tool for\nwriting it).  Whether that is true depends on what the Library does\nand what the program that uses the Library does.\n\n  1. You may copy and distribute verbatim copies of the Library's\ncomplete source code as you receive it, in any medium, provided that\nyou conspicuously and appropriately publish on each copy an\nappropriate copyright notice and disclaimer of warranty; keep intact\nall the notices that refer to this License and to the absence of any\nwarranty; and distribute a copy of this License along with the\nLibrary.\n\n  You may charge a fee for the physical act of transferring a copy,\nand you may at your option offer warranty protection in exchange for a\nfee.\n\f\n  2. You may modify your copy or copies of the Library or any portion\nof it, thus forming a work based on the Library, and copy and\ndistribute such modifications or work under the terms of Section 1\nabove, provided that you also meet all of these conditions:\n\n    a) The modified work must itself be a software library.\n\n    b) You must cause the files modified to carry prominent notices\n    stating that you changed the files and the date of any change.\n\n    c) You must cause the whole of the work to be licensed at no\n    charge to all third parties under the terms of this License.\n\n    d) If a facility in the modified Library refers to a function or a\n    table of data to be supplied by an application program that uses\n    the facility, other than as an argument passed when the facility\n    is invoked, then you must make a good faith effort to ensure that,\n    in the event an application does not supply such function or\n    table, the facility still operates, and performs whatever part of\n    its purpose remains meaningful.\n\n    (For example, a function in a library to compute square roots has\n    a purpose that is entirely well-defined independent of the\n    application.  Therefore, Subsection 2d requires that any\n    application-supplied function or table used by this function must\n    be optional: if the application does not supply it, the square\n    root function must still compute square roots.)\n\nThese requirements apply to the modified work as a whole.  If\nidentifiable sections of that work are not derived from the Library,\nand can be reasonably considered independent and separate works in\nthemselves, then this License, and its terms, do not apply to those\nsections when you distribute them as separate works.  But when you\ndistribute the same sections as part of a whole which is a work based\non the Library, the distribution of the whole must be on the terms of\nthis License, whose permissions for other licensees extend to the\nentire whole, and thus to each and every part regardless of who wrote\nit.\n\nThus, it is not the intent of this section to claim rights or contest\nyour rights to work written entirely by you; rather, the intent is to\nexercise the right to control the distribution of derivative or\ncollective works based on the Library.\n\nIn addition, mere aggregation of another work not based on the Library\nwith the Library (or with a work based on the Library) on a volume of\na storage or distribution medium does not bring the other work under\nthe scope of this License.\n\n  3. You may opt to apply the terms of the ordinary GNU General Public\nLicense instead of this License to a given copy of the Library.  To do\nthis, you must alter all the notices that refer to this License, so\nthat they refer to the ordinary GNU General Public License, version 2,\ninstead of to this License.  (If a newer version than version 2 of the\nordinary GNU General Public License has appeared, then you can specify\nthat version instead if you wish.)  Do not make any other change in\nthese notices.\n\f\n  Once this change is made in a given copy, it is irreversible for\nthat copy, so the ordinary GNU General Public License applies to all\nsubsequent copies and derivative works made from that copy.\n\n  This option is useful when you wish to copy part of the code of\nthe Library into a program that is not a library.\n\n  4. You may copy and distribute the Library (or a portion or\nderivative of it, under Section 2) in object code or executable form\nunder the terms of Sections 1 and 2 above provided that you accompany\nit with the complete corresponding machine-readable source code, which\nmust be distributed under the terms of Sections 1 and 2 above on a\nmedium customarily used for software interchange.\n\n  If distribution of object code is made by offering access to copy\nfrom a designated place, then offering equivalent access to copy the\nsource code from the same place satisfies the requirement to\ndistribute the source code, even though third parties are not\ncompelled to copy the source along with the object code.\n\n  5. A program that contains no derivative of any portion of the\nLibrary, but is designed to work with the Library by being compiled or\nlinked with it, is called a \"work that uses the Library\".  Such a\nwork, in isolation, is not a derivative work of the Library, and\ntherefore falls outside the scope of this License.\n\n  However, linking a \"work that uses the Library\" with the Library\ncreates an executable that is a derivative of the Library (because it\ncontains portions of the Library), rather than a \"work that uses the\nlibrary\".  The executable is therefore covered by this License.\nSection 6 states terms for distribution of such executables.\n\n  When a \"work that uses the Library\" uses material from a header file\nthat is part of the Library, the object code for the work may be a\nderivative work of the Library even though the source code is not.\nWhether this is true is especially significant if the work can be\nlinked without the Library, or if the work is itself a library.  The\nthreshold for this to be true is not precisely defined by law.\n\n  If such an object file uses only numerical parameters, data\nstructure layouts and accessors, and small macros and small inline\nfunctions (ten lines or less in length), then the use of the object\nfile is unrestricted, regardless of whether it is legally a derivative\nwork.  (Executables containing this object code plus portions of the\nLibrary will still fall under Section 6.)\n\n  Otherwise, if the work is a derivative of the Library, you may\ndistribute the object code for the work under the terms of Section 6.\nAny executables containing that work also fall under Section 6,\nwhether or not they are linked directly with the Library itself.\n\f\n  6. As an exception to the Sections above, you may also combine or\nlink a \"work that uses the Library\" with the Library to produce a\nwork containing portions of the Library, and distribute that work\nunder terms of your choice, provided that the terms permit\nmodification of the work for the customer's own use and reverse\nengineering for debugging such modifications.\n\n  You must give prominent notice with each copy of the work that the\nLibrary is used in it and that the Library and its use are covered by\nthis License.  You must supply a copy of this License.  If the work\nduring execution displays copyright notices, you must include the\ncopyright notice for the Library among them, as well as a reference\ndirecting the user to the copy of this License.  Also, you must do one\nof these things:\n\n    a) Accompany the work with the complete corresponding\n    machine-readable source code for the Library including whatever\n    changes were used in the work (which must be distributed under\n    Sections 1 and 2 above); and, if the work is an executable linked\n    with the Library, with the complete machine-readable \"work that\n    uses the Library\", as object code and/or source code, so that the\n    user can modify the Library and then relink to produce a modified\n    executable containing the modified Library.  (It is understood\n    that the user who changes the contents of definitions files in the\n    Library will not necessarily be able to recompile the application\n    to use the modified definitions.)\n\n    b) Use a suitable shared library mechanism for linking with the\n    Library.  A suitable mechanism is one that (1) uses at run time a\n    copy of the library already present on the user's computer system,\n    rather than copying library functions into the executable, and (2)\n    will operate properly with a modified version of the library, if\n    the user installs one, as long as the modified version is\n    interface-compatible with the version that the work was made with.\n\n    c) Accompany the work with a written offer, valid for at\n    least three years, to give the same user the materials\n    specified in Subsection 6a, above, for a charge no more\n    than the cost of performing this distribution.\n\n    d) If distribution of the work is made by offering access to copy\n    from a designated place, offer equivalent access to copy the above\n    specified materials from the same place.\n\n    e) Verify that the user has already received a copy of these\n    materials or that you have already sent this user a copy.\n\n  For an executable, the required form of the \"work that uses the\nLibrary\" must include any data and utility programs needed for\nreproducing the executable from it.  However, as a special exception,\nthe materials to be distributed need not include anything that is\nnormally distributed (in either source or binary form) with the major\ncomponents (compiler, kernel, and so on) of the operating system on\nwhich the executable runs, unless that component itself accompanies\nthe executable.\n\n  It may happen that this requirement contradicts the license\nrestrictions of other proprietary libraries that do not normally\naccompany the operating system.  Such a contradiction means you cannot\nuse both them and the Library together in an executable that you\ndistribute.\n\f\n  7. You may place library facilities that are a work based on the\nLibrary side-by-side in a single library together with other library\nfacilities not covered by this License, and distribute such a combined\nlibrary, provided that the separate distribution of the work based on\nthe Library and of the other library facilities is otherwise\npermitted, and provided that you do these two things:\n\n    a) Accompany the combined library with a copy of the same work\n    based on the Library, uncombined with any other library\n    facilities.  This must be distributed under the terms of the\n    Sections above.\n\n    b) Give prominent notice with the combined library of the fact\n    that part of it is a work based on the Library, and explaining\n    where to find the accompanying uncombined form of the same work.\n\n  8. You may not copy, modify, sublicense, link with, or distribute\nthe Library except as expressly provided under this License.  Any\nattempt otherwise to copy, modify, sublicense, link with, or\ndistribute the Library is void, and will automatically terminate your\nrights under this License.  However, parties who have received copies,\nor rights, from you under this License will not have their licenses\nterminated so long as such parties remain in full compliance.\n\n  9. You are not required to accept this License, since you have not\nsigned it.  However, nothing else grants you permission to modify or\ndistribute the Library or its derivative works.  These actions are\nprohibited by law if you do not accept this License.  Therefore, by\nmodifying or distributing the Library (or any work based on the\nLibrary), you indicate your acceptance of this License to do so, and\nall its terms and conditions for copying, distributing or modifying\nthe Library or works based on it.\n\n  10. Each time you redistribute the Library (or any work based on the\nLibrary), the recipient automatically receives a license from the\noriginal licensor to copy, distribute, link with or modify the Library\nsubject to these terms and conditions.  You may not impose any further\nrestrictions on the recipients' exercise of the rights granted herein.\nYou are not responsible for enforcing compliance by third parties with\nthis License.\n\f\n  11. If, as a consequence of a court judgment or allegation of patent\ninfringement or for any other reason (not limited to patent issues),\nconditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License.  If you cannot\ndistribute so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you\nmay not distribute the Library at all.  For example, if a patent\nlicense would not permit royalty-free redistribution of the Library by\nall those who receive copies directly or indirectly through you, then\nthe only way you could satisfy both it and this License would be to\nrefrain entirely from distribution of the Library.\n\nIf any portion of this section is held invalid or unenforceable under any\nparticular circumstance, the balance of the section is intended to apply,\nand the section as a whole is intended to apply in other circumstances.\n\nIt is not the purpose of this section to induce you to infringe any\npatents or other property right claims or to contest validity of any\nsuch claims; this section has the sole purpose of protecting the\nintegrity of the free software distribution system which is\nimplemented by public license practices.  Many people have made\ngenerous contributions to the wide range of software distributed\nthrough that system in reliance on consistent application of that\nsystem; it is up to the author/donor to decide if he or she is willing\nto distribute software through any other system and a licensee cannot\nimpose that choice.\n\nThis section is intended to make thoroughly clear what is believed to\nbe a consequence of the rest of this License.\n\n  12. If the distribution and/or use of the Library is restricted in\ncertain countries either by patents or by copyrighted interfaces, the\noriginal copyright holder who places the Library under this License may add\nan explicit geographical distribution limitation excluding those countries,\nso that distribution is permitted only in or among countries not thus\nexcluded.  In such case, this License incorporates the limitation as if\nwritten in the body of this License.\n\n  13. The Free Software Foundation may publish revised and/or new\nversions of the Lesser General Public License from time to time.\nSuch new versions will be similar in spirit to the present version,\nbut may differ in detail to address new problems or concerns.\n\nEach version is given a distinguishing version number.  If the Library\nspecifies a version number of this License which applies to it and\n\"any later version\", you have the option of following the terms and\nconditions either of that version or of any later version published by\nthe Free Software Foundation.  If the Library does not specify a\nlicense version number, you may choose any version ever published by\nthe Free Software Foundation.\n\f\n  14. If you wish to incorporate parts of the Library into other free\nprograms whose distribution conditions are incompatible with these,\nwrite to the author to ask for permission.  For software which is\ncopyrighted by the Free Software Foundation, write to the Free\nSoftware Foundation; we sometimes make exceptions for this.  Our\ndecision will be guided by the two goals of preserving the free status\nof all derivatives of our free software and of promoting the sharing\nand reuse of software generally.\n\n\t\t\t    NO WARRANTY\n\n  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO\nWARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.\nEXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR\nOTHER PARTIES PROVIDE THE LIBRARY \"AS IS\" WITHOUT WARRANTY OF ANY\nKIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\nPURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE\nLIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME\nTHE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.\n\n  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN\nWRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY\nAND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU\nFOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR\nCONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE\nLIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING\nRENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A\nFAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF\nSUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH\nDAMAGES.\n\n================================================================================\n\nrules_pkg 0.6.0 (Provided under following license)\n================================================================================\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"[]\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright [yyyy] [name of copyright owner]\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n\n================================================================================\n\ndlpack 0.8 (Provided under following license)\n================================================================================\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"{}\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright 2017 by Contributors\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n===============================================================================\n\nmagic_enum 0.9.3 (Provided under following license)\n===============================================================================\nMIT License\n\nCopyright (c) 2019 - 2023 Daniil Goncharov\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n\n================================================================================\n\npython-on-whales 0.65.0 (Provided under following license)\n================================================================================\nMIT License\n\nCopyright (c) 2020 Gabriel de Marmiesse de Lussan\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n===============================================================================\n\ncpprestsdk 2.10.18 (Provided under following license)\n===============================================================================\nC++ REST SDK\n\nThe MIT License (MIT)\n\nCopyright (c) Microsoft Corporation\n\nAll rights reserved.\n\nPermission is hereby granted, free of charge, to any person obtaining a copy of\nthis software and associated documentation files (the \"Software\"), to deal in\nthe Software without restriction, including without limitation the rights to\nuse, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of\nthe Software, and to permit persons to whom the Software is furnished to do so,\nsubject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n===============================================================================\n\ngrpc 1.48.0 (Provided under following license)\n===============================================================================\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"[]\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright [yyyy] [name of copyright owner]\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n\n-----------------------------------------------------------\n\nBSD 3-Clause License\n\nCopyright 2016, Google Inc.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice,\nthis list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\nthis list of conditions and the following disclaimer in the documentation\nand/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its\ncontributors may be used to endorse or promote products derived from this\nsoftware without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\nARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\nLIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\nCONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\nSUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\nINTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\nCONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\nARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF\nTHE POSSIBILITY OF SUCH DAMAGE.\n\n-----------------------------------------------------------\n\nMozilla Public License Version 2.0\n==================================\n\n1. Definitions\n--------------\n\n1.1. \"Contributor\"\n    means each individual or legal entity that creates, contributes to\n    the creation of, or owns Covered Software.\n\n1.2. \"Contributor Version\"\n    means the combination of the Contributions of others (if any) used\n    by a Contributor and that particular Contributor's Contribution.\n\n1.3. \"Contribution\"\n    means Covered Software of a particular Contributor.\n\n1.4. \"Covered Software\"\n    means Source Code Form to which the initial Contributor has attached\n    the notice in Exhibit A, the Executable Form of such Source Code\n    Form, and Modifications of such Source Code Form, in each case\n    including portions thereof.\n\n1.5. \"Incompatible With Secondary Licenses\"\n    means\n\n    (a) that the initial Contributor has attached the notice described\n        in Exhibit B to the Covered Software; or\n\n    (b) that the Covered Software was made available under the terms of\n        version 1.1 or earlier of the License, but not also under the\n        terms of a Secondary License.\n\n1.6. \"Executable Form\"\n    means any form of the work other than Source Code Form.\n\n1.7. \"Larger Work\"\n    means a work that combines Covered Software with other material, in\n    a separate file or files, that is not Covered Software.\n\n1.8. \"License\"\n    means this document.\n\n1.9. \"Licensable\"\n    means having the right to grant, to the maximum extent possible,\n    whether at the time of the initial grant or subsequently, any and\n    all of the rights conveyed by this License.\n\n1.10. \"Modifications\"\n    means any of the following:\n\n    (a) any file in Source Code Form that results from an addition to,\n        deletion from, or modification of the contents of Covered\n        Software; or\n\n    (b) any new file in Source Code Form that contains any Covered\n        Software.\n\n1.11. \"Patent Claims\" of a Contributor\n    means any patent claim(s), including without limitation, method,\n    process, and apparatus claims, in any patent Licensable by such\n    Contributor that would be infringed, but for the grant of the\n    License, by the making, using, selling, offering for sale, having\n    made, import, or transfer of either its Contributions or its\n    Contributor Version.\n\n1.12. \"Secondary License\"\n    means either the GNU General Public License, Version 2.0, the GNU\n    Lesser General Public License, Version 2.1, the GNU Affero General\n    Public License, Version 3.0, or any later versions of those\n    licenses.\n\n1.13. \"Source Code Form\"\n    means the form of the work preferred for making modifications.\n\n1.14. \"You\" (or \"Your\")\n    means an individual or a legal entity exercising rights under this\n    License. For legal entities, \"You\" includes any entity that\n    controls, is controlled by, or is under common control with You. For\n    purposes of this definition, \"control\" means (a) the power, direct\n    or indirect, to cause the direction or management of such entity,\n    whether by contract or otherwise, or (b) ownership of more than\n    fifty percent (50%) of the outstanding shares or beneficial\n    ownership of such entity.\n\n2. License Grants and Conditions\n--------------------------------\n\n2.1. Grants\n\nEach Contributor hereby grants You a world-wide, royalty-free,\nnon-exclusive license:\n\n(a) under intellectual property rights (other than patent or trademark)\n    Licensable by such Contributor to use, reproduce, make available,\n    modify, display, perform, distribute, and otherwise exploit its\n    Contributions, either on an unmodified basis, with Modifications, or\n    as part of a Larger Work; and\n\n(b) under Patent Claims of such Contributor to make, use, sell, offer\n    for sale, have made, import, and otherwise transfer either its\n    Contributions or its Contributor Version.\n\n2.2. Effective Date\n\nThe licenses granted in Section 2.1 with respect to any Contribution\nbecome effective for each Contribution on the date the Contributor first\ndistributes such Contribution.\n\n2.3. Limitations on Grant Scope\n\nThe licenses granted in this Section 2 are the only rights granted under\nthis License. No additional rights or licenses will be implied from the\ndistribution or licensing of Covered Software under this License.\nNotwithstanding Section 2.1(b) above, no patent license is granted by a\nContributor:\n\n(a) for any code that a Contributor has removed from Covered Software;\n    or\n\n(b) for infringements caused by: (i) Your and any other third party's\n    modifications of Covered Software, or (ii) the combination of its\n    Contributions with other software (except as part of its Contributor\n    Version); or\n\n(c) under Patent Claims infringed by Covered Software in the absence of\n    its Contributions.\n\nThis License does not grant any rights in the trademarks, service marks,\nor logos of any Contributor (except as may be necessary to comply with\nthe notice requirements in Section 3.4).\n\n2.4. Subsequent Licenses\n\nNo Contributor makes additional grants as a result of Your choice to\ndistribute the Covered Software under a subsequent version of this\nLicense (see Section 10.2) or under the terms of a Secondary License (if\npermitted under the terms of Section 3.3).\n\n2.5. Representation\n\nEach Contributor represents that the Contributor believes its\nContributions are its original creation(s) or it has sufficient rights\nto grant the rights to its Contributions conveyed by this License.\n\n2.6. Fair Use\n\nThis License is not intended to limit any rights You have under\napplicable copyright doctrines of fair use, fair dealing, or other\nequivalents.\n\n2.7. Conditions\n\nSections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted\nin Section 2.1.\n\n3. Responsibilities\n-------------------\n\n3.1. Distribution of Source Form\n\nAll distribution of Covered Software in Source Code Form, including any\nModifications that You create or to which You contribute, must be under\nthe terms of this License. You must inform recipients that the Source\nCode Form of the Covered Software is governed by the terms of this\nLicense, and how they can obtain a copy of this License. You may not\nattempt to alter or restrict the recipients' rights in the Source Code\nForm.\n\n3.2. Distribution of Executable Form\n\nIf You distribute Covered Software in Executable Form then:\n\n(a) such Covered Software must also be made available in Source Code\n    Form, as described in Section 3.1, and You must inform recipients of\n    the Executable Form how they can obtain a copy of such Source Code\n    Form by reasonable means in a timely manner, at a charge no more\n    than the cost of distribution to the recipient; and\n\n(b) You may distribute such Executable Form under the terms of this\n    License, or sublicense it under different terms, provided that the\n    license for the Executable Form does not attempt to limit or alter\n    the recipients' rights in the Source Code Form under this License.\n\n3.3. Distribution of a Larger Work\n\nYou may create and distribute a Larger Work under terms of Your choice,\nprovided that You also comply with the requirements of this License for\nthe Covered Software. If the Larger Work is a combination of Covered\nSoftware with a work governed by one or more Secondary Licenses, and the\nCovered Software is not Incompatible With Secondary Licenses, this\nLicense permits You to additionally distribute such Covered Software\nunder the terms of such Secondary License(s), so that the recipient of\nthe Larger Work may, at their option, further distribute the Covered\nSoftware under the terms of either this License or such Secondary\nLicense(s).\n\n3.4. Notices\n\nYou may not remove or alter the substance of any license notices\n(including copyright notices, patent notices, disclaimers of warranty,\nor limitations of liability) contained within the Source Code Form of\nthe Covered Software, except that You may alter any license notices to\nthe extent required to remedy known factual inaccuracies.\n\n3.5. Application of Additional Terms\n\nYou may choose to offer, and to charge a fee for, warranty, support,\nindemnity or liability obligations to one or more recipients of Covered\nSoftware. However, You may do so only on Your own behalf, and not on\nbehalf of any Contributor. You must make it absolutely clear that any\nsuch warranty, support, indemnity, or liability obligation is offered by\nYou alone, and You hereby agree to indemnify every Contributor for any\nliability incurred by such Contributor as a result of warranty, support,\nindemnity or liability terms You offer. You may include additional\ndisclaimers of warranty and limitations of liability specific to any\njurisdiction.\n\n4. Inability to Comply Due to Statute or Regulation\n---------------------------------------------------\n\nIf it is impossible for You to comply with any of the terms of this\nLicense with respect to some or all of the Covered Software due to\nstatute, judicial order, or regulation then You must: (a) comply with\nthe terms of this License to the maximum extent possible; and (b)\ndescribe the limitations and the code they affect. Such description must\nbe placed in a text file included with all distributions of the Covered\nSoftware under this License. Except to the extent prohibited by statute\nor regulation, such description must be sufficiently detailed for a\nrecipient of ordinary skill to be able to understand it.\n\n5. Termination\n--------------\n\n5.1. The rights granted under this License will terminate automatically\nif You fail to comply with any of its terms. However, if You become\ncompliant, then the rights granted under this License from a particular\nContributor are reinstated (a) provisionally, unless and until such\nContributor explicitly and finally terminates Your grants, and (b) on an\nongoing basis, if such Contributor fails to notify You of the\nnon-compliance by some reasonable means prior to 60 days after You have\ncome back into compliance. Moreover, Your grants from a particular\nContributor are reinstated on an ongoing basis if such Contributor\nnotifies You of the non-compliance by some reasonable means, this is the\nfirst time You have received notice of non-compliance with this License\nfrom such Contributor, and You become compliant prior to 30 days after\nYour receipt of the notice.\n\n5.2. If You initiate litigation against any entity by asserting a patent\ninfringement claim (excluding declaratory judgment actions,\ncounter-claims, and cross-claims) alleging that a Contributor Version\ndirectly or indirectly infringes any patent, then the rights granted to\nYou by any and all Contributors for the Covered Software under Section\n2.1 of this License shall terminate.\n\n5.3. In the event of termination under Sections 5.1 or 5.2 above, all\nend user license agreements (excluding distributors and resellers) which\nhave been validly granted by You or Your distributors under this License\nprior to termination shall survive termination.\n\n************************************************************************\n*                                                                      *\n*  6. Disclaimer of Warranty                                           *\n*  -------------------------                                           *\n*                                                                      *\n*  Covered Software is provided under this License on an \"as is\"       *\n*  basis, without warranty of any kind, either expressed, implied, or  *\n*  statutory, including, without limitation, warranties that the       *\n*  Covered Software is free of defects, merchantable, fit for a        *\n*  particular purpose or non-infringing. The entire risk as to the     *\n*  quality and performance of the Covered Software is with You.        *\n*  Should any Covered Software prove defective in any respect, You     *\n*  (not any Contributor) assume the cost of any necessary servicing,   *\n*  repair, or correction. This disclaimer of warranty constitutes an   *\n*  essential part of this License. No use of any Covered Software is   *\n*  authorized under this License except under this disclaimer.         *\n*                                                                      *\n************************************************************************\n\n************************************************************************\n*                                                                      *\n*  7. Limitation of Liability                                          *\n*  --------------------------                                          *\n*                                                                      *\n*  Under no circumstances and under no legal theory, whether tort      *\n*  (including negligence), contract, or otherwise, shall any           *\n*  Contributor, or anyone who distributes Covered Software as          *\n*  permitted above, be liable to You for any direct, indirect,         *\n*  special, incidental, or consequential damages of any character      *\n*  including, without limitation, damages for lost profits, loss of    *\n*  goodwill, work stoppage, computer failure or malfunction, or any    *\n*  and all other commercial damages or losses, even if such party      *\n*  shall have been informed of the possibility of such damages. This   *\n*  limitation of liability shall not apply to liability for death or   *\n*  personal injury resulting from such party's negligence to the       *\n*  extent applicable law prohibits such limitation. Some               *\n*  jurisdictions do not allow the exclusion or limitation of           *\n*  incidental or consequential damages, so this exclusion and          *\n*  limitation may not apply to You.                                    *\n*                                                                      *\n************************************************************************\n\n8. Litigation\n-------------\n\nAny litigation relating to this License may be brought only in the\ncourts of a jurisdiction where the defendant maintains its principal\nplace of business and such litigation shall be governed by laws of that\njurisdiction, without reference to its conflict-of-law provisions.\nNothing in this Section shall prevent a party's ability to bring\ncross-claims or counter-claims.\n\n9. Miscellaneous\n----------------\n\nThis License represents the complete agreement concerning the subject\nmatter hereof. If any provision of this License is held to be\nunenforceable, such provision shall be reformed only to the extent\nnecessary to make it enforceable. Any law or regulation which provides\nthat the language of a contract shall be construed against the drafter\nshall not be used to construe this License against a Contributor.\n\n10. Versions of the License\n---------------------------\n\n10.1. New Versions\n\nMozilla Foundation is the license steward. Except as provided in Section\n10.3, no one other than the license steward has the right to modify or\npublish new versions of this License. Each version will be given a\ndistinguishing version number.\n\n10.2. Effect of New Versions\n\nYou may distribute the Covered Software under the terms of the version\nof the License under which You originally received the Covered Software,\nor under the terms of any subsequent version published by the license\nsteward.\n\n10.3. Modified Versions\n\nIf you create software not governed by this License, and you want to\ncreate a new license for such software, you may create and use a\nmodified version of this License if you rename the license and remove\nany references to the name of the license steward (except to note that\nsuch modified license differs from this License).\n\n10.4. Distributing Source Code Form that is Incompatible With Secondary\nLicenses\n\nIf You choose to distribute Source Code Form that is Incompatible With\nSecondary Licenses under the terms of this version of the License, the\nnotice described in Exhibit B of this License must be attached.\n\nExhibit A - Source Code Form License Notice\n-------------------------------------------\n\n  This Source Code Form is subject to the terms of the Mozilla Public\n  License, v. 2.0. If a copy of the MPL was not distributed with this\n  file, You can obtain one at http://mozilla.org/MPL/2.0/.\n\nIf it is not possible or desirable to put the notice in a particular\nfile, then You may include the notice in a location (such as a LICENSE\nfile in a relevant directory) where a recipient would be likely to look\nfor such a notice.\n\nYou may add additional accurate notices of copyright ownership.\n\nExhibit B - \"Incompatible With Secondary Licenses\" Notice\n---------------------------------------------------------\n\n  This Source Code Form is \"Incompatible With Secondary Licenses\", as\n  defined by the Mozilla Public License, v. 2.0.\n===============================================================================\n\nboringssl-b9232f9e27e5668bc0414879dcdedb2a59ea75f2 (Provided under following license)\n===============================================================================\nBoringSSL is a fork of OpenSSL. As such, large parts of it fall under OpenSSL\nlicensing. Files that are completely new have a Google copyright and an ISC\nlicense. This license is reproduced at the bottom of this file.\n\nContributors to BoringSSL are required to follow the CLA rules for Chromium:\nhttps://cla.developers.google.com/clas\n\nFiles in third_party/ have their own licenses, as described therein. The MIT\nlicense, for third_party/fiat, which, unlike other third_party directories, is\ncompiled into non-test libraries, is included below.\n\nThe OpenSSL toolkit stays under a dual license, i.e. both the conditions of the\nOpenSSL License and the original SSLeay license apply to the toolkit. See below\nfor the actual license texts. Actually both licenses are BSD-style Open Source\nlicenses. In case of any license issues related to OpenSSL please contact\nopenssl-core@openssl.org.\n\nThe following are Google-internal bug numbers where explicit permission from\nsome authors is recorded for use of their work. (This is purely for our own\nrecord keeping.)\n  27287199\n  27287880\n  27287883\n\n  OpenSSL License\n  ---------------\n\n/* ====================================================================\n * Copyright (c) 1998-2011 The OpenSSL Project.  All rights reserved.\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * 1. Redistributions of source code must retain the above copyright\n *    notice, this list of conditions and the following disclaimer.\n *\n * 2. Redistributions in binary form must reproduce the above copyright\n *    notice, this list of conditions and the following disclaimer in\n *    the documentation and/or other materials provided with the\n *    distribution.\n *\n * 3. All advertising materials mentioning features or use of this\n *    software must display the following acknowledgment:\n *    \"This product includes software developed by the OpenSSL Project\n *    for use in the OpenSSL Toolkit. (http://www.openssl.org/)\";\n *\n * 4. The names \"OpenSSL Toolkit\" and \"OpenSSL Project\" must not be used to\n *    endorse or promote products derived from this software without\n *    prior written permission. For written permission, please contact\n *    openssl-core@openssl.org.\n *\n * 5. Products derived from this software may not be called \"OpenSSL\"\n *    nor may \"OpenSSL\" appear in their names without prior written\n *    permission of the OpenSSL Project.\n *\n * 6. Redistributions of any form whatsoever must retain the following\n *    acknowledgment:\n *    \"This product includes software developed by the OpenSSL Project\n *    for use in the OpenSSL Toolkit (http://www.openssl.org/)\";\n *\n * THIS SOFTWARE IS PROVIDED BY THE OpenSSL PROJECT ``AS IS'' AND ANY\n * EXPRESSED OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\n * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE OpenSSL PROJECT OR\n * ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT\n * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)\n * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,\n * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED\n * OF THE POSSIBILITY OF SUCH DAMAGE.\n * ====================================================================\n *\n * This product includes cryptographic software written by Eric Young\n * (eay@cryptsoft.com).  This product includes software written by Tim\n * Hudson (tjh@cryptsoft.com).\n *\n */\n\n Original SSLeay License\n -----------------------\n\n/* Copyright (C) 1995-1998 Eric Young (eay@cryptsoft.com)\n * All rights reserved.\n *\n * This package is an SSL implementation written\n * by Eric Young (eay@cryptsoft.com).\n * The implementation was written so as to conform with Netscapes SSL.\n *\n * This library is free for commercial and non-commercial use as long as\n * the following conditions are aheared to.  The following conditions\n * apply to all code found in this distribution, be it the RC4, RSA,\n * lhash, DES, etc., code; not just the SSL code.  The SSL documentation\n * included with this distribution is covered by the same copyright terms\n * except that the holder is Tim Hudson (tjh@cryptsoft.com).\n *\n * Copyright remains Eric Young's, and as such any Copyright notices in\n * the code are not to be removed.\n * If this package is used in a product, Eric Young should be given attribution\n * as the author of the parts of the library used.\n * This can be in the form of a textual message at program startup or\n * in documentation (online or textual) provided with the package.\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n * 1. Redistributions of source code must retain the copyright\n *    notice, this list of conditions and the following disclaimer.\n * 2. Redistributions in binary form must reproduce the above copyright\n *    notice, this list of conditions and the following disclaimer in the\n *    documentation and/or other materials provided with the distribution.\n * 3. All advertising materials mentioning features or use of this software\n *    must display the following acknowledgement:\n *    \"This product includes cryptographic software written by\n *     Eric Young (eay@cryptsoft.com)\"\n *    The word 'cryptographic' can be left out if the rouines from the library\n *    being used are not cryptographic related :-).\n * 4. If you include any Windows specific code (or a derivative thereof) from\n *    the apps directory (application code) you must include an acknowledgement:\n *    \"This product includes software written by Tim Hudson (tjh@cryptsoft.com)\"\n *\n * THIS SOFTWARE IS PROVIDED BY ERIC YOUNG ``AS IS'' AND\n * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE\n * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\n * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS\n * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)\n * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY\n * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF\n * SUCH DAMAGE.\n *\n * The licence and distribution terms for any publically available version or\n * derivative of this code cannot be changed.  i.e. this code cannot simply be\n * copied and put under another distribution licence\n * [including the GNU Public Licence.]\n */\n\n\nISC license used for completely new code in BoringSSL:\n\n/* Copyright (c) 2015, Google Inc.\n *\n * Permission to use, copy, modify, and/or distribute this software for any\n * purpose with or without fee is hereby granted, provided that the above\n * copyright notice and this permission notice appear in all copies.\n *\n * THE SOFTWARE IS PROVIDED \"AS IS\" AND THE AUTHOR DISCLAIMS ALL WARRANTIES\n * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF\n * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY\n * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES\n * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION\n * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN\n * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */\n\n\nThe code in third_party/fiat carries the MIT license:\n\nCopyright (c) 2015-2016 the fiat-crypto authors (see\nhttps://github.com/mit-plv/fiat-crypto/blob/master/AUTHORS).\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n\n\nLicenses for support code\n-------------------------\n\nParts of the TLS test suite are under the Go license. This code is not included\nin BoringSSL (i.e. libcrypto and libssl) when compiled, however, so\ndistributing code linked against BoringSSL does not trigger this license:\n\nCopyright (c) 2009 The Go Authors. All rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are\nmet:\n\n   * Redistributions of source code must retain the above copyright\nnotice, this list of conditions and the following disclaimer.\n   * Redistributions in binary form must reproduce the above\ncopyright notice, this list of conditions and the following disclaimer\nin the documentation and/or other materials provided with the\ndistribution.\n   * Neither the name of Google Inc. nor the names of its\ncontributors may be used to endorse or promote products derived from\nthis software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR\nA PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\nOWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\nSPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT\nLIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,\nDATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY\nTHEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n\nBoringSSL uses the Chromium test infrastructure to run a continuous build,\ntrybots etc. The scripts which manage this, and the script for generating build\nmetadata, are under the Chromium license. Distributing code linked against\nBoringSSL does not trigger this license.\n\nCopyright 2015 The Chromium Authors. All rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are\nmet:\n\n   * Redistributions of source code must retain the above copyright\nnotice, this list of conditions and the following disclaimer.\n   * Redistributions in binary form must reproduce the above\ncopyright notice, this list of conditions and the following disclaimer\nin the documentation and/or other materials provided with the\ndistribution.\n   * Neither the name of Google Inc. nor the names of its\ncontributors may be used to endorse or promote products derived from\nthis software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR\nA PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\nOWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\nSPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT\nLIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,\nDATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY\nTHEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n===============================================================================\n\nboost 1.79.0 (Provided under following license)\n===============================================================================\nBoost Software License - Version 1.0 - August 17th, 2003\n\nPermission is hereby granted, free of charge, to any person or organization\nobtaining a copy of the software and accompanying documentation covered by\nthis license (the \"Software\") to use, reproduce, display, distribute,\nexecute, and transmit the Software, and to prepare derivative works of the\nSoftware, and to permit third-parties to whom the Software is furnished to\ndo so, all subject to the following:\n\nThe copyright notices in the Software and this entire statement, including\nthe above license grant, this restriction and the following disclaimer,\nmust be included in all copies of the Software, in whole or in part, and\nall derivative works of the Software, unless such copies or derivative\nworks are solely in the form of machine-executable object code generated by\na source language processor.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT\nSHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE\nFOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,\nARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER\nDEALINGS IN THE SOFTWARE.\n===============================================================================\n\nprotobuf-22d0e265de7d2b3d2e9a00d071313502e7d4cccf (Provided under following license)\n===============================================================================\nCopyright 2008 Google Inc.  All rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are\nmet:\n\n    * Redistributions of source code must retain the above copyright\nnotice, this list of conditions and the following disclaimer.\n    * Redistributions in binary form must reproduce the above\ncopyright notice, this list of conditions and the following disclaimer\nin the documentation and/or other materials provided with the\ndistribution.\n    * Neither the name of Google Inc. nor the names of its\ncontributors may be used to endorse or promote products derived from\nthis software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR\nA PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\nOWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\nSPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT\nLIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,\nDATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY\nTHEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\nCode generated by the Protocol Buffer compiler is owned by the owner\nof the input file used when generating it.  This code is not\nstandalone and requires a support library to be linked with it.  This\nsupport library is itself covered by the above license.\n===============================================================================\n\npyarmor 7.7.4 (Provided under following license)\n===============================================================================\nPyarmor 8.0 End User License Agreement\n\nThe following agreement regarding Pyarmor is made between Jondy Zhao -\nreferred to as \"licensor\" - and anyone or organization who is installing,\naccessing or in any other way using Pyarmor - referred to as \"user\" or\n\"you\".\n\n1. Definitions\n--------------\n\n1.1. \"This Software\"\n\n    means Pyarmor 8.0, it doesn't include Pyarmor prior to 8.0. Pyarmor 8.0 is\n    rewritten, and provides new features. For compatibility, Pyarmor 8.0 also\n    includes most of the old features, this license doesn't apply to those old\n    features. It's only for new features.\n\n1.2. \"Product\"\n    means any application or software for sale.\n\n1.3. \"One Product\"\n    means a product name and everything that makes up this name. It includes\n    all the devices to develop, build, debug, test product. It also includes\n    product current version, history versions and all the future versions.\n\n    One product may has several variants, each variant name is composed of\n    product name plus feature name. As long as the proportion of the\n    variable part is far less than that of the common part, they're\n    considered as \"one product\".\n\n1.4. \"Customer\"\n    means anyone who uses user's product.\n\n1.5  \"Script\"\n    means Python script.\n\n1.6  \"User Script\"\n    means user owns this script.\n\n1.7  \"Other Script\"\n    means user doesn't own this script.\n\n1.8  \"Mix Str\"\n    means a feature of this software to obfuscate string constant in script\n\n1.9  \"BCC Mode\"\n    means an irreversible obfuscation method, a feature of this software\n\n1.10 \"RFT Mode\"\n    means an irreversible obfuscation method, a feature of this software\n\n1.11 \"Big Script\"\n    means script size exceeds a certain value\n\n1.12 \"Pyarmor License\"\n    means a file issued by licensor to unlock this software limitations\n\n1.13 \"Basic License\"\n    means a kind of Pyarmor License\n\n1.13 \"Pro License\"\n    means a kind of Pyarmor License\n\n1.14 \"Group License\"\n    means a kind of Pyarmor License\n\n1.15 \"License No.\"\n    means a string with format \"pyarmor-vax-xxxxxx\", \"x\" stands for a\n    digital, each Pyarmor License has an unique License No.\n\n1.16 \"Licensed Product\"\n    means product has been registered to one Pyarmor License.\n\n2. License Grants and Conditions\n--------------------------------\n\nInstalling and using this software signifies acceptance of these terms and\nconditions of the license. If you do not agree with the terms of this\nlicense, you must remove all of this software files from your storage\ndevices and cease to use this software.\n\n2.1. Trial Version Limitations\n\nThis software is published as shareware, free trial version never expires,\nbut there are some limitations.\n\n2.1.1 Can not obfuscate big script\n2.1.2 Can not use feature Mix Str\n2.1.3 Can not use feature RFT Mode, BCC Mode\n2.1.4 Can not be used in any commercial product if the total sale income of\n      this product is larger than 30x license fees\n2.1.5 Can not be used to provide obfuscation service in any form\n\n2.2. Grants\n\nUser purchases Pyarmor License to unlock these limitations except 2.1.5.\n\nLicensor issues 3 kind of Pyarmor License.\n\n2.2.1 Pyarmor Basic, unlock limitations 2.1.1, 2.1.2, 2.1.4\n2.2.2 Pyarmor Pro, unlock limitations 2.1.1, 2.1.2, 2.1.3, 2.1.4\n2.2.3 Pyarmor Group, unlock limitations 2.1.1, 2.1.2, 2.1.3, 2.1.4\n\nPyarmor Basic and Pyarmor Pro need internet connection to verify, it doesn't\nwork in the device without internet connection.\n\nPyarmor Group need not internet connection.\n\nEach Pyarmor License only need pay once, not periodically.\n\n2.3 Conditions\n\n2.3.1 Each Pyarmor License can only be used to register One Product. Each\n      Licensed Product has an unique License No. in global. If user has many\n      products, each product need purchase one Pyarmor License. Except 2.6.1\n\n2.3.2 Pyarmor License could be installed in many machines and devices which\n      used to develop, build, debug, test and support Licensed Product. But\n      there is limitation to be used at the same time. See 2.3.3\n\n2.3.3 Pyarmor Basic and Pro License can only be used in no more than 100\n      devices in 24 hours. Pyarmor License be used means use any feature of\n      Pyarmor in one machine. Running obfuscated scripts generated by\n      Pyarmor is not considered as Pyarmor License be used.\n\n2.3.4 Pyarmor Basic and Pro License need internet connection to verify.\n\n2.3.5 Pyarmor License could not be installed in customer's devices.\n\n2.3.6 Pyarmor License could not be used to obfuscate any other scripts.\n\n2.3.7 Pyarmor License could not be transferred to other product.\n\n2.4 Special Cases\n\n2.4.1 When product name is changed, but the product features are same,\n      Pyarmor License could be used without changing registration product\n      name.\n\n2.4.2 When organization user is renamed, or acquired by others, only product\n      name is not changed, Pyarmor License need change nothing, and still\n      could be used. But if product name is changed, even product functions\n      are same, Pyarmor License can't be used again.\n\n2.4.3 When product in developing and its name is not defined, the initial\n      registration could use \"TBD\" as the product name. In this case,\n      product name can be changed once. Before this product is sold, user\n      must change registration name to real product name.\n\n2.5 Modifying Software\n\nUser could modify Python scripts of this software to meet needs. But this\nmodified software could only be used in Licensed Product, it could not be\ndistributed to others.\n\n2.6 Fair Use\n\nThis License is not intended to limit any rights users have under applicable\ncopyright doctrines of fair use, fair dealing, or other equivalents.\n\n2.6.1 If user has many products, and has purchased one license for the first\n      product. The second product could use first product license only if sale\n      income of the second product less than 30x license fees. Once greater than\n      30x license fees, the second product need purchase its own license. It's\n      same to user's other products.\n\n3. Responsibilities and Commitments\n-----------------------------------\n\n3.1 This software does nothing except the features described in the software\n    documentation.\n\n3.2 Using Pyarmor Basic and Pro License, this software need internet\n    connection to request authorization. And only the related files of\n    Pyarmor License, serial number of hard disk, Ethernet address, IPv4/IPv6\n    address, hostname will be sent to remote server for verification.\n\n3.3 Except 3.2, this software does not record and collect any other device\n    information, and need not connect to internet.\n\n3.4 Regarding to obfuscated scripts generated by this software\n\n3.4.1 this software has no any control or limitation to obfuscated scripts,\n      the behaviors of obfuscated scripts are totally defined by user.\n\n3.4.2 License No. and product name will be embedded into obfuscated scripts,\n      user's regname, email and other information are not.\n\n4. Termination\n--------------\n\nThe rights granted under this License will terminate automatically if You\nfail to comply with any of its terms.\n\n4.1 You issue chargeback after purchased Pyarmor License has been registered\n    to one product. Even chargeback is rejected by bank or user cancels\n    chargeback, this Pyarmor License can't be used again.\n\n4.2 You let others got Pyarmor License intentionally or unintentionally\n\n4.3 You lost Pyarmor License by hacker\n\n4.4 In any cases include 4.2, 4.3, once licensor find too many machines use\n    one Pyarmor License exceeds permission, this Pyarmor License will be\n    ceased, and never could be used again.\n\n4.5 In any cases if licensor finds one Pyarmor License is used to other\n    product, even it's caused by 4.3, this Pyarmor License will be ceased. A\n    notify email will be sent to registration email of this Pyarmor License.\n\n5. Restrictions\n---------------\n\nYou shall not (and shall not allow any third party to):\n\n5.1 decompile, disassemble, or otherwise reverse engineer any binary files\n    of this software;\n\n5.2 distribute, sell, sublicense, rent, lease, or use this software (or any\n    portion thereof) for time sharing, hosting, service provider, or like\n    purposes;\n\n5.3 remove any product identification, proprietary, copyright, or other\n    notices contained in this software;\n\n5.4 copy, modify (except as expressly permitted in this Agreement) or\n    translate any part of this software, create a derivative work of any\n    part of this software, or incorporate this software into or with other\n    software, except to the extent expressly authorized in writing by\n    licensor;\n\n5.5 attempt to circumvent or disable the security key mechanism that\n    protects this software against unauthorized use (except and only to the\n    extent that applicable law prohibits or restricts such restrictions);\n\n6. Disclaimer of Warranty\n-------------------------\n\nThis Software is provided under this License on an \"as is\" basis, without\nwarranty of any kind, either expressed, implied, or statutory, including,\nwithout limitation, warranties that this software is free of defects, fit\nfor a particular purpose or non-infringing. The entire risk as to the\nquality and performance of this software is with users. Neither the licensor\nnor the agents of the licensor will be liable for data loss, damages, loss\nof profits or any other kind of loss while using or misusing this\nsoftware. This disclaimer of warranty constitutes an essential part of this\nLicense. No use of this software is authorized under this License except\nunder this disclaimer.\n\n7. Changes to this Agreement\n----------------------------\n\nLicensor reserves the right, at its sole discretion, to modify or replace\nthis Agreement at any time. If a revision is material licensor will provide\nat least 30 days' notice prior to any new terms taking effect. What\nconstitutes a material change will be determined at the sole discretion of\nthe licensor.\n\nBy continuing to access or use this software after any revisions become\neffective, You agree to be bound by the revised terms. If You do not agree\nto the new terms, You are no longer authorized to use this software.\n\n8. Litigation\n-------------\n\nAny litigation relating to this License may be brought only in the courts of\na jurisdiction where the defendant maintains its principal place of business\nand such litigation shall be governed by laws of that jurisdiction, without\nreference to its conflict-of-law provisions.  Nothing in this Section shall\nprevent a party's ability to bring cross-claims or counter-claims.\n===============================================================================\n\npyinstaller 5.13.2 (Provided under following license)\n===============================================================================\n================================\n The PyInstaller licensing terms\n================================\n\n\nCopyright (c) 2010-2023, PyInstaller Development Team\nCopyright (c) 2005-2009, Giovanni Bajo\nBased on previous work under copyright (c) 2002 McMillan Enterprises, Inc.\n\n\nPyInstaller is licensed under the terms of the GNU General Public License\nas published by the Free Software Foundation; either version 2 of the License,\nor (at your option) any later version.\n\n\nBootloader Exception\n--------------------\n\nIn addition to the permissions in the GNU General Public License, the\nauthors give you unlimited permission to link or embed compiled bootloader\nand related files into combinations with other programs, and to distribute\nthose combinations without any restriction coming from the use of those\nfiles. (The General Public License restrictions do apply in other respects;\nfor example, they cover modification of the files, and distribution when\nnot linked into a combined executable.)\n\n\nBootloader and Related Files\n----------------------------\n\nBootloader and related files are files which are embedded within the\nfinal executable. This includes files in directories:\n\n./bootloader/\n./PyInstaller/loader\n\n\nRun-time Hooks\n----------------------------\n\nRun-time Hooks are a different kind of files embedded within the final\nexecutable. To ease moving them into a separate repository, or into the\nrespective project, these files are now licensed under the Apache License,\nVersion 2.0.\n\nRun-time Hooks are in the directory\n./PyInstaller/hooks/rthooks\n\n\nThe PyInstaller.isolated submodule\n----------------------------------\n\nBy request, the PyInstaller.isolated submodule and its corresponding tests are\nadditionally licensed with the MIT license so that it may be reused outside of\nPyInstaller under GPL 2.0 or MIT terms and conditions -- whichever is the most\nsuitable to the recipient downstream project. Affected files/directories are:\n\n./PyInstaller/isolated/\n./tests/unit/test_isolation.py\n\n\nAbout the PyInstaller Development Team\n--------------------------------------\n\nThe PyInstaller Development Team is the set of contributors\nto the PyInstaller project. A full list with details is kept\nin the documentation directory, in the file\n``doc/CREDITS.rst``.\n\nThe core team that coordinates development on GitHub can be found here:\nhttps://github.com/pyinstaller/pyinstaller.  As of 2021, it consists of:\n\n* Hartmut Goebel\n* Jasper Harrison\n* Bryan Jones\n* Brenainn Woodsend\n* Rok Mandeljc\n\nOur Copyright Policy\n--------------------\n\nPyInstaller uses a shared copyright model. Each contributor maintains copyright\nover their contributions to PyInstaller. But, it is important to note that these\ncontributions are typically only changes to the repositories. Thus,\nthe PyInstaller source code, in its entirety is not the copyright of any single\nperson or institution.  Instead, it is the collective copyright of the entire\nPyInstaller Development Team.  If individual contributors want to maintain\na record of what changes/contributions they have specific copyright on, they\nshould indicate their copyright in the commit message of the change, when they\ncommit the change to the PyInstaller repository.\n\nWith this in mind, the following banner should be used in any source code file\nto indicate the copyright and license terms:\n\n\n#-----------------------------------------------------------------------------\n# Copyright (c) 2005-2023, PyInstaller Development Team.\n#\n# Distributed under the terms of the GNU General Public License (version 2\n# or later) with exception for distributing the bootloader.\n#\n# The full license is in the file COPYING.txt, distributed with this software.\n#\n# SPDX-License-Identifier: (GPL-2.0-or-later WITH Bootloader-exception)\n#-----------------------------------------------------------------------------\n\n\nFor run-time hooks, the following banner should be used:\n\n#-----------------------------------------------------------------------------\n# Copyright (c) 2005-2023, PyInstaller Development Team.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n#\n# The full license is in the file COPYING.txt, distributed with this software.\n#\n# SPDX-License-Identifier: Apache-2.0\n#-----------------------------------------------------------------------------\n\n\n================================\nGNU General Public License\n================================\n\nhttps://gnu.org/licenses/gpl-2.0.html\n\n\n\t\t    GNU GENERAL PUBLIC LICENSE\n\t\t       Version 2, June 1991\n\n Copyright (C) 1989, 1991 Free Software Foundation, Inc.\n 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA\n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n\t\t\t    Preamble\n\n  The licenses for most software are designed to take away your\nfreedom to share and change it.  By contrast, the GNU General Public\nLicense is intended to guarantee your freedom to share and change free\nsoftware--to make sure the software is free for all its users.  This\nGeneral Public License applies to most of the Free Software\nFoundation's software and to any other program whose authors commit to\nusing it.  (Some other Free Software Foundation software is covered by\nthe GNU Library General Public License instead.)  You can apply it to\nyour programs, too.\n\n  When we speak of free software, we are referring to freedom, not\nprice.  Our General Public Licenses are designed to make sure that you\nhave the freedom to distribute copies of free software (and charge for\nthis service if you wish), that you receive source code or can get it\nif you want it, that you can change the software or use pieces of it\nin new free programs; and that you know you can do these things.\n\n  To protect your rights, we need to make restrictions that forbid\nanyone to deny you these rights or to ask you to surrender the rights.\nThese restrictions translate to certain responsibilities for you if you\ndistribute copies of the software, or if you modify it.\n\n  For example, if you distribute copies of such a program, whether\ngratis or for a fee, you must give the recipients all the rights that\nyou have.  You must make sure that they, too, receive or can get the\nsource code.  And you must show them these terms so they know their\nrights.\n\n  We protect your rights with two steps: (1) copyright the software, and\n(2) offer you this license which gives you legal permission to copy,\ndistribute and/or modify the software.\n\n  Also, for each author's protection and ours, we want to make certain\nthat everyone understands that there is no warranty for this free\nsoftware.  If the software is modified by someone else and passed on, we\nwant its recipients to know that what they have is not the original, so\nthat any problems introduced by others will not reflect on the original\nauthors' reputations.\n\n  Finally, any free program is threatened constantly by software\npatents.  We wish to avoid the danger that redistributors of a free\nprogram will individually obtain patent licenses, in effect making the\nprogram proprietary.  To prevent this, we have made it clear that any\npatent must be licensed for everyone's free use or not licensed at all.\n\n  The precise terms and conditions for copying, distribution and\nmodification follow.\n\n\t\t    GNU GENERAL PUBLIC LICENSE\n   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION\n\n  0. This License applies to any program or other work which contains\na notice placed by the copyright holder saying it may be distributed\nunder the terms of this General Public License.  The \"Program\", below,\nrefers to any such program or work, and a \"work based on the Program\"\nmeans either the Program or any derivative work under copyright law:\nthat is to say, a work containing the Program or a portion of it,\neither verbatim or with modifications and/or translated into another\nlanguage.  (Hereinafter, translation is included without limitation in\nthe term \"modification\".)  Each licensee is addressed as \"you\".\n\nActivities other than copying, distribution and modification are not\ncovered by this License; they are outside its scope.  The act of\nrunning the Program is not restricted, and the output from the Program\nis covered only if its contents constitute a work based on the\nProgram (independent of having been made by running the Program).\nWhether that is true depends on what the Program does.\n\n  1. You may copy and distribute verbatim copies of the Program's\nsource code as you receive it, in any medium, provided that you\nconspicuously and appropriately publish on each copy an appropriate\ncopyright notice and disclaimer of warranty; keep intact all the\nnotices that refer to this License and to the absence of any warranty;\nand give any other recipients of the Program a copy of this License\nalong with the Program.\n\nYou may charge a fee for the physical act of transferring a copy, and\nyou may at your option offer warranty protection in exchange for a fee.\n\n  2. You may modify your copy or copies of the Program or any portion\nof it, thus forming a work based on the Program, and copy and\ndistribute such modifications or work under the terms of Section 1\nabove, provided that you also meet all of these conditions:\n\n    a) You must cause the modified files to carry prominent notices\n    stating that you changed the files and the date of any change.\n\n    b) You must cause any work that you distribute or publish, that in\n    whole or in part contains or is derived from the Program or any\n    part thereof, to be licensed as a whole at no charge to all third\n    parties under the terms of this License.\n\n    c) If the modified program normally reads commands interactively\n    when run, you must cause it, when started running for such\n    interactive use in the most ordinary way, to print or display an\n    announcement including an appropriate copyright notice and a\n    notice that there is no warranty (or else, saying that you provide\n    a warranty) and that users may redistribute the program under\n    these conditions, and telling the user how to view a copy of this\n    License.  (Exception: if the Program itself is interactive but\n    does not normally print such an announcement, your work based on\n    the Program is not required to print an announcement.)\n\nThese requirements apply to the modified work as a whole.  If\nidentifiable sections of that work are not derived from the Program,\nand can be reasonably considered independent and separate works in\nthemselves, then this License, and its terms, do not apply to those\nsections when you distribute them as separate works.  But when you\ndistribute the same sections as part of a whole which is a work based\non the Program, the distribution of the whole must be on the terms of\nthis License, whose permissions for other licensees extend to the\nentire whole, and thus to each and every part regardless of who wrote it.\n\nThus, it is not the intent of this section to claim rights or contest\nyour rights to work written entirely by you; rather, the intent is to\nexercise the right to control the distribution of derivative or\ncollective works based on the Program.\n\nIn addition, mere aggregation of another work not based on the Program\nwith the Program (or with a work based on the Program) on a volume of\na storage or distribution medium does not bring the other work under\nthe scope of this License.\n\n  3. You may copy and distribute the Program (or a work based on it,\nunder Section 2) in object code or executable form under the terms of\nSections 1 and 2 above provided that you also do one of the following:\n\n    a) Accompany it with the complete corresponding machine-readable\n    source code, which must be distributed under the terms of Sections\n    1 and 2 above on a medium customarily used for software interchange; or,\n\n    b) Accompany it with a written offer, valid for at least three\n    years, to give any third party, for a charge no more than your\n    cost of physically performing source distribution, a complete\n    machine-readable copy of the corresponding source code, to be\n    distributed under the terms of Sections 1 and 2 above on a medium\n    customarily used for software interchange; or,\n\n    c) Accompany it with the information you received as to the offer\n    to distribute corresponding source code.  (This alternative is\n    allowed only for noncommercial distribution and only if you\n    received the program in object code or executable form with such\n    an offer, in accord with Subsection b above.)\n\nThe source code for a work means the preferred form of the work for\nmaking modifications to it.  For an executable work, complete source\ncode means all the source code for all modules it contains, plus any\nassociated interface definition files, plus the scripts used to\ncontrol compilation and installation of the executable.  However, as a\nspecial exception, the source code distributed need not include\nanything that is normally distributed (in either source or binary\nform) with the major components (compiler, kernel, and so on) of the\noperating system on which the executable runs, unless that component\nitself accompanies the executable.\n\nIf distribution of executable or object code is made by offering\naccess to copy from a designated place, then offering equivalent\naccess to copy the source code from the same place counts as\ndistribution of the source code, even though third parties are not\ncompelled to copy the source along with the object code.\n\n  4. You may not copy, modify, sublicense, or distribute the Program\nexcept as expressly provided under this License.  Any attempt\notherwise to copy, modify, sublicense or distribute the Program is\nvoid, and will automatically terminate your rights under this License.\nHowever, parties who have received copies, or rights, from you under\nthis License will not have their licenses terminated so long as such\nparties remain in full compliance.\n\n  5. You are not required to accept this License, since you have not\nsigned it.  However, nothing else grants you permission to modify or\ndistribute the Program or its derivative works.  These actions are\nprohibited by law if you do not accept this License.  Therefore, by\nmodifying or distributing the Program (or any work based on the\nProgram), you indicate your acceptance of this License to do so, and\nall its terms and conditions for copying, distributing or modifying\nthe Program or works based on it.\n\n  6. Each time you redistribute the Program (or any work based on the\nProgram), the recipient automatically receives a license from the\noriginal licensor to copy, distribute or modify the Program subject to\nthese terms and conditions.  You may not impose any further\nrestrictions on the recipients' exercise of the rights granted herein.\nYou are not responsible for enforcing compliance by third parties to\nthis License.\n\n  7. If, as a consequence of a court judgment or allegation of patent\ninfringement or for any other reason (not limited to patent issues),\nconditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License.  If you cannot\ndistribute so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you\nmay not distribute the Program at all.  For example, if a patent\nlicense would not permit royalty-free redistribution of the Program by\nall those who receive copies directly or indirectly through you, then\nthe only way you could satisfy both it and this License would be to\nrefrain entirely from distribution of the Program.\n\nIf any portion of this section is held invalid or unenforceable under\nany particular circumstance, the balance of the section is intended to\napply and the section as a whole is intended to apply in other\ncircumstances.\n\nIt is not the purpose of this section to induce you to infringe any\npatents or other property right claims or to contest validity of any\nsuch claims; this section has the sole purpose of protecting the\nintegrity of the free software distribution system, which is\nimplemented by public license practices.  Many people have made\ngenerous contributions to the wide range of software distributed\nthrough that system in reliance on consistent application of that\nsystem; it is up to the author/donor to decide if he or she is willing\nto distribute software through any other system and a licensee cannot\nimpose that choice.\n\nThis section is intended to make thoroughly clear what is believed to\nbe a consequence of the rest of this License.\n\n  8. If the distribution and/or use of the Program is restricted in\ncertain countries either by patents or by copyrighted interfaces, the\noriginal copyright holder who places the Program under this License\nmay add an explicit geographical distribution limitation excluding\nthose countries, so that distribution is permitted only in or among\ncountries not thus excluded.  In such case, this License incorporates\nthe limitation as if written in the body of this License.\n\n  9. The Free Software Foundation may publish revised and/or new versions\nof the General Public License from time to time.  Such new versions will\nbe similar in spirit to the present version, but may differ in detail to\naddress new problems or concerns.\n\nEach version is given a distinguishing version number.  If the Program\nspecifies a version number of this License which applies to it and \"any\nlater version\", you have the option of following the terms and conditions\neither of that version or of any later version published by the Free\nSoftware Foundation.  If the Program does not specify a version number of\nthis License, you may choose any version ever published by the Free Software\nFoundation.\n\n  10. If you wish to incorporate parts of the Program into other free\nprograms whose distribution conditions are different, write to the author\nto ask for permission.  For software which is copyrighted by the Free\nSoftware Foundation, write to the Free Software Foundation; we sometimes\nmake exceptions for this.  Our decision will be guided by the two goals\nof preserving the free status of all derivatives of our free software and\nof promoting the sharing and reuse of software generally.\n\n\t\t\t    NO WARRANTY\n\n  11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY\nFOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN\nOTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES\nPROVIDE THE PROGRAM \"AS IS\" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED\nOR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF\nMERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS\nTO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE\nPROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,\nREPAIR OR CORRECTION.\n\n  12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING\nWILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR\nREDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,\nINCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING\nOUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED\nTO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY\nYOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER\nPROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGES.\n\n\t\t     END OF TERMS AND CONDITIONS\n\n================================\nApache License 2.0\n================================\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"[]\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright [yyyy] [name of copyright owner]\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n\n===========\nMIT License\n===========\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n===============================================================================\n\nrules_boost_630cf5dbad418ee8cfa637b1e33125b11807721d (Provided under following license)\n===============================================================================\nApache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"{}\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright {yyyy} {name of copyright owner}\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n   ===============================================================================\n===============================================================================\n\nlibnuma 2.0.14 (Provided under following license)\n===============================================================================\n\nThis package was debianized by Ian Wienand <ianw@gelato.unsw.edu.au>\non Wed, 15 Dec 2004.\n\nlibnuma sources are available from\nhttps://github.com/numactl/numactl\n\nUpstream Authors:\nAndi Kleen <ak@suse.de>\n\nCopyright (C) 2004 Andi Kleen\n\nnumactl and the demo programs are under the GNU General Public License, v.2\nlibnuma is under the GNU Lesser General Public License, v2.1.\nThe manpages are under the same license as the Linux manpages (see the files)\n\nThis program is free software; you can redistribute it and/or modify\nit under the terms of the GNU General Public License as published\nby the Free Software Foundation; either version 2 of the License,\nor (at your option) any later version.  The full text of the License\nmay be found in /usr/share/common-licenses/GPL.\n\nThe redistribution of portions of this program are controlled by\nthe GNU Lesser General Public License as published by the Free\nSoftware Foundation.  The full text of the License may be found in\n/usr/share/common-licenses/LGPL.\n\nThis program is distributed in the hope that it will be useful,\nbut WITHOUT ANY WARRANTY; without even the implied warranty of\nMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU\nGeneral Public License for more details.\n\nParts of the numademo program are derived from the STREAM benchmark,\navailable from\n\nhttp://www.cs.virginia.edu/stream/\n===============================================================================\n\nGitPython (Provided under following license)\n===============================================================================\n\nCopyright (C) 2008, 2009 Michael Trier and contributors\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions\nare met:\n\n* Redistributions of source code must retain the above copyright\nnotice, this list of conditions and the following disclaimer.\n\n* Redistributions in binary form must reproduce the above copyright\nnotice, this list of conditions and the following disclaimer in the\ndocumentation and/or other materials provided with the distribution.\n\n* Neither the name of the GitPython project nor the names of\nits contributors may be used to endorse or promote products derived\nfrom this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR\nA PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\nOWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\nSPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\nTO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\nPROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\nLIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\nNEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\nSOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n===============================================================================\n\nRAPIDS Memory Manager (RMM) (Provided under following license)\n===============================================================================\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"[]\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright [yyyy] [name of copyright owner]\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n\n===============================================================================\n\nspdlog (Provided under following license)\n===============================================================================\n\nThe MIT License (MIT)\n\nCopyright (c) 2016 Gabi Melman.\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in\nall copies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN\nTHE SOFTWARE.\n\n-- NOTE: Third party dependency used by this software --\nThis software depends on the fmt lib (MIT License),\nand users must comply to its license: https://raw.githubusercontent.com/fmtlib/fmt/master/LICENSE\n\n===============================================================================\n\nfmt (Provided under following license)\n===============================================================================\n\nCopyright (c) 2012 - present, Victor Zverovich and {fmt} contributors\n\nPermission is hereby granted, free of charge, to any person obtaining\na copy of this software and associated documentation files (the\n\"Software\"), to deal in the Software without restriction, including\nwithout limitation the rights to use, copy, modify, merge, publish,\ndistribute, sublicense, and/or sell copies of the Software, and to\npermit persons to whom the Software is furnished to do so, subject to\nthe following conditions:\n\nThe above copyright notice and this permission notice shall be\nincluded in all copies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND,\nEXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF\nMERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND\nNONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE\nLIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION\nOF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION\nWITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.\n\n--- Optional exception to the license ---\n\nAs an exception, if, as a result of your compiling your source code, portions\nof this Software are embedded into a machine-executable object form of such\nsource code, you may redistribute such embedded portions in such object form\nwithout including the above copyright and permission notices.\n\n===============================================================================\n\nyq (Provided under following license)\n===============================================================================\nApache License\nVersion 2.0, January 2004\nhttp://www.apache.org/licenses/\n\nTERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n1. Definitions.\n\n\"License\" shall mean the terms and conditions for use, reproduction, and\ndistribution as defined by Sections 1 through 9 of this document.\n\n\"Licensor\" shall mean the copyright owner or entity authorized by the copyright\nowner that is granting the License.\n\n\"Legal Entity\" shall mean the union of the acting entity and all other entities\nthat control, are controlled by, or are under common control with that entity.\nFor the purposes of this definition, \"control\" means (i) the power, direct or\nindirect, to cause the direction or management of such entity, whether by\ncontract or otherwise, or (ii) ownership of fifty percent (50%) or more of the\noutstanding shares, or (iii) beneficial ownership of such entity.\n\n\"You\" (or \"Your\") shall mean an individual or Legal Entity exercising\npermissions granted by this License.\n\n\"Source\" form shall mean the preferred form for making modifications, including\nbut not limited to software source code, documentation source, and configuration\nfiles.\n\n\"Object\" form shall mean any form resulting from mechanical transformation or\ntranslation of a Source form, including but not limited to compiled object code,\ngenerated documentation, and conversions to other media types.\n\n\"Work\" shall mean the work of authorship, whether in Source or Object form, made\navailable under the License, as indicated by a copyright notice that is included\nin or attached to the work (an example is provided in the Appendix below).\n\n\"Derivative Works\" shall mean any work, whether in Source or Object form, that\nis based on (or derived from) the Work and for which the editorial revisions,\nannotations, elaborations, or other modifications represent, as a whole, an\noriginal work of authorship. For the purposes of this License, Derivative Works\nshall not include works that remain separable from, or merely link (or bind by\nname) to the interfaces of, the Work and Derivative Works thereof.\n\n\"Contribution\" shall mean any work of authorship, including the original version\nof the Work and any modifications or additions to that Work or Derivative Works\nthereof, that is intentionally submitted to Licensor for inclusion in the Work\nby the copyright owner or by an individual or Legal Entity authorized to submit\non behalf of the copyright owner. For the purposes of this definition,\n\"submitted\" means any form of electronic, verbal, or written communication sent\nto the Licensor or its representatives, including but not limited to\ncommunication on electronic mailing lists, source code control systems, and\nissue tracking systems that are managed by, or on behalf of, the Licensor for\nthe purpose of discussing and improving the Work, but excluding communication\nthat is conspicuously marked or otherwise designated in writing by the copyright\nowner as \"Not a Contribution.\"\n\n\"Contributor\" shall mean Licensor and any individual or Legal Entity on behalf\nof whom a Contribution has been received by Licensor and subsequently\nincorporated within the Work.\n\n2. Grant of Copyright License.\n\nSubject to the terms and conditions of this License, each Contributor hereby\ngrants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free,\nirrevocable copyright license to reproduce, prepare Derivative Works of,\npublicly display, publicly perform, sublicense, and distribute the Work and such\nDerivative Works in Source or Object form.\n\n3. Grant of Patent License.\n\nSubject to the terms and conditions of this License, each Contributor hereby\ngrants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free,\nirrevocable (except as stated in this section) patent license to make, have\nmade, use, offer to sell, sell, import, and otherwise transfer the Work, where\nsuch license applies only to those patent claims licensable by such Contributor\nthat are necessarily infringed by their Contribution(s) alone or by combination\nof their Contribution(s) with the Work to which such Contribution(s) was\nsubmitted. If You institute patent litigation against any entity (including a\ncross-claim or counterclaim in a lawsuit) alleging that the Work or a\nContribution incorporated within the Work constitutes direct or contributory\npatent infringement, then any patent licenses granted to You under this License\nfor that Work shall terminate as of the date such litigation is filed.\n\n4. Redistribution.\n\nYou may reproduce and distribute copies of the Work or Derivative Works thereof\nin any medium, with or without modifications, and in Source or Object form,\nprovided that You meet the following conditions:\n\nYou must give any other recipients of the Work or Derivative Works a copy of\nthis License; and\nYou must cause any modified files to carry prominent notices stating that You\nchanged the files; and\nYou must retain, in the Source form of any Derivative Works that You distribute,\nall copyright, patent, trademark, and attribution notices from the Source form\nof the Work, excluding those notices that do not pertain to any part of the\nDerivative Works; and\nIf the Work includes a \"NOTICE\" text file as part of its distribution, then any\nDerivative Works that You distribute must include a readable copy of the\nattribution notices contained within such NOTICE file, excluding those notices\nthat do not pertain to any part of the Derivative Works, in at least one of the\nfollowing places: within a NOTICE text file distributed as part of the\nDerivative Works; within the Source form or documentation, if provided along\nwith the Derivative Works; or, within a display generated by the Derivative\nWorks, if and wherever such third-party notices normally appear. The contents of\nthe NOTICE file are for informational purposes only and do not modify the\nLicense. You may add Your own attribution notices within Derivative Works that\nYou distribute, alongside or as an addendum to the NOTICE text from the Work,\nprovided that such additional attribution notices cannot be construed as\nmodifying the License.\nYou may add Your own copyright statement to Your modifications and may provide\nadditional or different license terms and conditions for use, reproduction, or\ndistribution of Your modifications, or for any such Derivative Works as a whole,\nprovided Your use, reproduction, and distribution of the Work otherwise complies\nwith the conditions stated in this License.\n\n5. Submission of Contributions.\n\nUnless You explicitly state otherwise, any Contribution intentionally submitted\nfor inclusion in the Work by You to the Licensor shall be under the terms and\nconditions of this License, without any additional terms or conditions.\nNotwithstanding the above, nothing herein shall supersede or modify the terms of\nany separate license agreement you may have executed with Licensor regarding\nsuch Contributions.\n\n6. Trademarks.\n\nThis License does not grant permission to use the trade names, trademarks,\nservice marks, or product names of the Licensor, except as required for\nreasonable and customary use in describing the origin of the Work and\nreproducing the content of the NOTICE file.\n\n7. Disclaimer of Warranty.\n\nUnless required by applicable law or agreed to in writing, Licensor provides the\nWork (and each Contributor provides its Contributions) on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied,\nincluding, without limitation, any warranties or conditions of TITLE,\nNON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are\nsolely responsible for determining the appropriateness of using or\nredistributing the Work and assume any risks associated with Your exercise of\npermissions under this License.\n\n8. Limitation of Liability.\n\nIn no event and under no legal theory, whether in tort (including negligence),\ncontract, or otherwise, unless required by applicable law (such as deliberate\nand grossly negligent acts) or agreed to in writing, shall any Contributor be\nliable to You for damages, including any direct, indirect, special, incidental,\nor consequential damages of any character arising as a result of this License or\nout of the use or inability to use the Work (including but not limited to\ndamages for loss of goodwill, work stoppage, computer failure or malfunction, or\nany and all other commercial damages or losses), even if such Contributor has\nbeen advised of the possibility of such damages.\n\n9. Accepting Warranty or Additional Liability.\n\nWhile redistributing the Work or Derivative Works thereof, You may choose to\noffer, and charge a fee for, acceptance of support, warranty, indemnity, or\nother liability obligations and/or rights consistent with this License. However,\nin accepting such obligations, You may act only on Your own behalf and on Your\nsole responsibility, not on behalf of any other Contributor, and only if You\nagree to indemnify, defend, and hold each Contributor harmless for any liability\nincurred by, or claims asserted against, such Contributor by reason of your\naccepting any such warranty or additional liability.\n\nEND OF TERMS AND CONDITIONS\n\nAPPENDIX: How to apply the Apache License to your work\n\nTo apply the Apache License to your work, attach the following boilerplate\nnotice, with the fields enclosed by brackets \"[]\" replaced with your own\nidentifying information. (Don't include the brackets!) The text should be\nenclosed in the appropriate comment syntax for the file format. We also\nrecommend that a file or class name and description of purpose be included on\nthe same \"printed page\" as the copyright notice for easier identification within\nthird-party archives.\n\n   Copyright [yyyy] [name of copyright owner]\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n     http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n===============================================================================\n\nbazelbuild/bazel-gazelle 0.24.0 (Provided under following license)\n===============================================================================\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"[]\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright [yyyy] [name of copyright owner]\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n\n===============================================================================\n\nbazelbuild/rules_go 0.24.2 (Provided under following license)\n===============================================================================\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"[]\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright [yyyy] [name of copyright owner]\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n\n===============================================================================\n\nbazelbuild/rules_docker 0.22.0 (Provided under following license)\n===============================================================================\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"{}\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright {yyyy} {name of copyright owner}\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License.\n\n===============================================================================\n"
  },
  {
    "path": "isaac_ros_gxf/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022-2024, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_gxf</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS GXF</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n  <author>Swapnesh Wani</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>magic_enum</depend>\n  <depend>libucx0</depend>\n  <depend condition=\"$ISAAC_ROS_PLATFORM == amd64\">nvsci</depend>\n  <depend condition=\"$ISAAC_ROS_PLATFORM == amd64\">libgnat-23</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf/src/isaac_ros_gxf.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n// Placeholder to allow ament_cmake_lint to not error on no source files.\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_argus/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_argus LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\nif(EXISTS \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\")\n  install(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\n  set_property(TARGET ${PROJECT_NAME} PROPERTY\n    INTERFACE_LINK_LIBRARIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n  )\nendif()\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_argus/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_argus</name>\n  <version>4.4.0</version>\n  <description>GXF extension containing argus components.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n\n  <exec_depend>isaac_ros_gxf</exec_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_atlas/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_atlas LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_atlas/gxf/extensions/atlas/composite_schema_server.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <mutex>\n\n#include \"common/unique_index_map.hpp\"\n#include \"gems/composite/schema.hpp\"\n#include \"gxf/core/component.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// CompositeSchemaServer is a central repository for storing composite schemas across multiple\n// components. When a Schema is loaded to the server, the server will then provide an 64 bit key\n// that is guaranteed to be unique for the lifetime of the CompositeSchemaServer. This allows\n// components to pass composite messages along with the key, rather than needing to encode the whole\n// Schema in the message.\nclass CompositeSchemaServer : public gxf::Component {\n public:\n  gxf_result_t registerInterface(gxf::Registrar* registrar) override;\n  gxf_result_t initialize() override;\n\n  // Stores a schema into the server and returns a unique ID tied to the schema.\n  gxf::Expected<uint64_t> add(const composite::Schema& schema);\n\n  // Retrieves a schema from the server using the provided unique ID.\n  gxf::Expected<composite::Schema> get(uint64_t uid) const;\n\n  // Returns true if the unique index map is at maximum capacity.\n  bool full() const { return (schemas_.size() == schemas_.capacity()); }\n\n private:\n  gxf::Parameter<uint64_t> maximum_capacity_;\n\n  // Map of uint64_t to schemas that provides unique keys for every insert.\n  UniqueIndexMap<composite::Schema> schemas_;\n\n  // Protect from concurrent read/write access to the schemas_.\n  mutable std::mutex mutex_;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_atlas/gxf/extensions/atlas/pose_tree_frame.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <optional>\n#include <string>\n\n#include \"extensions/gxf_helpers/parameter_parser_isaac.hpp\"\n#include \"extensions/gxf_helpers/parameter_wrapper_isaac.hpp\"\n#include \"extensions/gxf_helpers/string_provider.hpp\"\n#include \"gems/core/math/pose3.hpp\"\n#include \"gems/core/optional.hpp\"\n#include \"gems/pose_tree/pose_tree.hpp\"\n#include \"gxf/core/component.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Creates a frame in the PoseTree with given name. Optionally sets its pose. Provides methods to\n// access its name and uid.\nclass PoseTreeFrame : public gxf::Component {\n public:\n  gxf_result_t registerInterface(gxf::Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n\n  // Returns UID of the frame\n  PoseTree::frame_t frame_uid() const { return frame_uid_; }\n  // Returns name of the frame\n  const char* frame_name() const { return frame_name_string_.c_str(); }\n  // Returns name of the parent frame\n  const char* parent_frame_name() const;\n  // Retursn whether the frame can be moved using interactive markers.\n  bool interactive_marker() const { return interactive_marker_; }\n\n private:\n  // Sets up the frame in PoseTree with given parameters, if it has not already. This function can\n  // be called by the initialize() of this component or by the initialize() of other PoseTreeFrame\n  // components, but it executes once per initialization.\n  gxf_result_t setup();\n\n  gxf::Parameter<std::string> frame_name_;\n  gxf::Parameter<int32_t> number_edges_;\n  gxf::Parameter<gxf::Handle<PoseTreeFrame>> parent_frame_;\n  gxf::Parameter<::nvidia::isaac::Pose3d> initial_pose_;\n  gxf::Parameter<bool> invert_pose_;\n  gxf::Parameter<int32_t> maximum_edge_history_;\n  gxf::Parameter<gxf::Handle<PoseTree>> pose_tree_;\n  gxf::Parameter<gxf::Handle<StringProvider>> namespace_;\n  gxf::Parameter<bool> interactive_marker_;\n\n  // The value returned by setup() function. Cleared at deinitialize().\n  std::optional<gxf_result_t> setup_result_ = std::nullopt;\n  // Cached values\n  PoseTree::frame_t frame_uid_;\n  std::string frame_name_string_;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_atlas/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_atlas</name>\n  <version>4.4.0</version>\n  <description>GXF extension containing environment management components, such as pose tree, map providers, and the composite schema server.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n  <depend>gxf_isaac_gems</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_camera_utils/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_camera_utils LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Dependencies\nfind_package(CUDAToolkit REQUIRED)\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(yaml-cpp)\n\n# Camera utils extension\nament_auto_add_library(${PROJECT_NAME} SHARED\n  gxf/extensions/camera_utils/camera_utils.cpp\n  gxf/extensions/camera_utils/stereo_camera_synchronizer.cpp\n  gxf/extensions/camera_utils/stereo_camera_synchronizer.hpp\n  gxf/extensions/camera_utils/camera_info_synchronizer.cpp\n  gxf/extensions/camera_utils/camera_info_synchronizer.hpp\n)\ntarget_include_directories(${PROJECT_NAME} PUBLIC \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE\n)\n\n# Install the binary file\ninstall(TARGETS ${PROJECT_NAME} DESTINATION share/${PROJECT_NAME}/gxf/lib)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_camera_utils/gxf/extensions/camera_utils/camera_info_synchronizer.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include \"camera_info_synchronizer.hpp\"\n\n#include <algorithm>\n#include <limits>\n#include <vector>\n\n#include \"gxf/core/expected_macro.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\nnamespace {\n// Global constants that detail sync behavior\nsize_t kSyncOldestOnFastestQueue = 0;\nsize_t kSyncNewestOnSlowestQueue = 1;\n}  // namespace\n\ngxf_result_t\nCameraInfoSynchronization::registerInterface(gxf::Registrar* registrar) {\n  gxf::Expected<void> result;\n  result &= registrar->parameter(camera_message_rx_, \"camera_message_rx\",\n                                 \"Camera Message receiver\",\n                                 \"Camera message receiver handle.\");\n  result &= registrar->parameter(\n      camera_info_message_rx_, \"camera_info_message_rx\",\n      \"Camera Info message receiver\", \"Camera info message receiver handle.\");\n  result &= registrar->parameter(camera_message_tx_, \"camera_message_tx\",\n                                 \"Camera Message transmitter\",\n                                 \"Camera message transmitter handle.\");\n  result &= registrar->parameter(camera_info_message_tx_, \"camera_info_message_tx\",\n                           \"Camera Info message transmitter\",\n                           \"Camera info message transmitter handle.\");\n  result &= registrar->parameter(use_latest_camera_info_, \"use_latest_camera_info\",\n                           \"Use latest camera info\",\n                           \"Use latest camera info message, make time stamp \"\n                           \"same as image and forward\",\n                           false);\n  result &= registrar->parameter(drop_old_messages_, \"drop_old_messages\",\n                           \"Drop oldest messages inside tick function\",\n                           \"Drop oldest messages inside tick function\", false);\n  result &= registrar->parameter(\n      sync_policy_, \"sync_policy\", \"Synchronization policy\",\n      \"Synchronization policy, 0: choose oldest message from fastest moving queue\"\n      \"1: choose newest message from slowest moving queue. 0 is good for processing most of the\"\n      \"data at the cost of being potentially slower than realtime. 1 is good for finding sync\"\n      \"points close to real time but at the cost of dropping more data.\",\n      static_cast<uint64_t>(0));\n\n  return gxf::ToResultCode(result);\n}\n\ngxf_result_t CameraInfoSynchronization::start() {\n  camera_acq_times_.reserve(camera_message_rx_->capacity());\n  camera_info_acq_times_.reserve(camera_info_message_rx_->capacity());\n  return GXF_SUCCESS;\n}\n\ngxf_result_t CameraInfoSynchronization::dropOldestMessagesIfQueueIsFull() {\n  // Clear all queues\n  RETURN_IF_ERROR(camera_message_rx_->sync());\n  if (camera_message_rx_->size() == camera_message_rx_->capacity()) {\n    // If you see this log, please disable the drop_old_messages parameter and make sure your\n    // GXF node is the first entry point into the Nitros graph. The Nitros graph will force add\n    // a policy 0 on all incoming receivers and will drop messages if the queue is full.\n    // This log is to inform the user that they might be unintentionally dropping messages.\n    GXF_LOG_WARNING(\"Dropping messages from camera message receiver\");\n    RETURN_IF_ERROR(camera_message_rx_->receive());\n  }\n  RETURN_IF_ERROR(camera_info_message_rx_->sync());\n  if (camera_info_message_rx_->size() == camera_info_message_rx_->capacity()) {\n    // If you see this log, please disable the drop_old_messages parameter and make sure your\n    // GXF node is the first entry point into the Nitros graph. The Nitros graph will force add\n    // a policy 0 on all incoming receivers and will drop messages if the queue is full.\n    // This log is to inform the user that they might be unintentionally dropping messages.\n    GXF_LOG_WARNING(\"Dropping messages from camera info message receiver\");\n    RETURN_IF_ERROR(camera_info_message_rx_->receive());\n  }\n  return GXF_SUCCESS;\n}\n\ngxf_result_t CameraInfoSynchronization::useLatestCameraInfo() {\n  bool all_messages_available = true;\n\n  RETURN_IF_ERROR(camera_message_rx_->sync());\n  if (camera_message_rx_->size() == 0) {\n    GXF_LOG_DEBUG(\n        \"Camera message receiver has no messages for synchronization!\");\n    all_messages_available = false;\n  }\n\n  RETURN_IF_ERROR(camera_info_message_rx_->sync());\n  if (camera_info_message_rx_->size() == 0) {\n    GXF_LOG_DEBUG(\n        \"Camera info message receiver has no messages for synchronization!\");\n    all_messages_available = false;\n  }\n\n  if (!all_messages_available && drop_old_messages_.get()) {\n    // We do this to keep the graph ticking and not leading to blocks.\n    // To enable no dropping of messages, users are requested to use individual\n    // MessageAvailableSchedulingTerms for each rx. We do not need to do this if used as the first\n    // input in a Nitros node because Nitros adds a policy: 0 to the input rx of the graph\n    // automatically.\n    return dropOldestMessagesIfQueueIsFull();\n  } else if (!all_messages_available) {\n    // Do not do anything if not all messages are available\n    return GXF_SUCCESS;\n  }\n\n  gxf::Entity camera_message = UNWRAP_OR_RETURN(camera_message_rx_->receive());\n  gxf::Entity camera_info_message = UNWRAP_OR_RETURN(camera_info_message_rx_->receive());\n\n  // Make camera info timestamp same as camera message\n  auto timestamp_components_cam = camera_message.findAllHeap<gxf::Timestamp>();\n  auto acq_time = timestamp_components_cam->front().value()->acqtime;\n\n  // This will publish the camera and camera info messages with the same\n  // timestamp. This might be required upstream by some codelets\n  camera_message_tx_->publish(camera_message, acq_time);\n  camera_info_message_tx_->publish(camera_info_message, acq_time);\n\n  return GXF_SUCCESS;\n}\n\ngxf_result_t syncMessages(const std::vector<int64_t>& acq_times,\n                          const gxf::Handle<gxf::Receiver>& rx,\n                          const gxf::Handle<gxf::Transmitter>& tx,\n                          const int64_t candidate_sync_point) {\n  for (size_t index = acq_times.size(); index > 0; index--) {\n    if (acq_times[index-1] == candidate_sync_point) {\n      gxf::Entity message = UNWRAP_OR_RETURN(rx->receive());\n      RETURN_IF_ERROR(tx->publish(message));\n      return GXF_SUCCESS;\n    } else if (acq_times[index-1] < candidate_sync_point) {\n      // Old message, drop it\n      RETURN_IF_ERROR(rx->receive());\n    } else if (acq_times[index-1] > candidate_sync_point) {\n      // This should not happen\n      GXF_LOG_ERROR(\"Message timestamp is not monotonically increasing\");\n      return GXF_FAILURE;\n    }\n  }\n  GXF_LOG_ERROR(\"Did not publish anything, should have published\");\n  return GXF_FAILURE;\n}\n\ngxf_result_t CameraInfoSynchronization::tick() {\n  // Clear all queues for processing in this tick call\n  camera_acq_times_.clear();\n  camera_info_acq_times_.clear();\n\n  if (use_latest_camera_info_.get()) {\n    return useLatestCameraInfo();\n  }\n  // Check if all input queues have messages\n  bool all_messages_available = true;\n  RETURN_IF_ERROR(camera_message_rx_->sync());\n  RETURN_IF_ERROR(camera_info_message_rx_->sync());\n  if (camera_message_rx_->size() == 0 || camera_info_message_rx_->size() == 0) {\n    // Not all the inputs have messages, unexpected.\n    GXF_LOG_DEBUG(\"Not all the inputs have messages for synchronization!\");\n    all_messages_available = false;\n  }\n\n  if (!all_messages_available && drop_old_messages_.get()) {\n    // Acts as a policy inside the tick function\n    return dropOldestMessagesIfQueueIsFull();\n  } else if (!all_messages_available) {\n    // Do not do anything if not all messages are available\n    return GXF_SUCCESS;\n  }\n\n  // Read the timestamps of available messages from camera input\n  for (size_t index = camera_message_rx_->size(); index > 0; index--) {\n    auto msg_result = camera_message_rx_->peek(index - 1);\n    if (msg_result) {\n      auto timestamp_components = msg_result->findAllHeap<gxf::Timestamp>();\n      if (!timestamp_components) {\n        return gxf::ToResultCode(timestamp_components);\n      }\n      if (0 == timestamp_components->size()) {\n        GXF_LOG_ERROR(\"No timestamp found from the camera message\");\n        return GXF_ENTITY_COMPONENT_NOT_FOUND;\n      }\n      camera_acq_times_.push_back(\n          timestamp_components->front().value()->acqtime);\n    }\n  }\n\n  // Read the timestamps of available messages from camera info input\n  for (size_t index = camera_info_message_rx_->size(); index > 0; index--) {\n    auto msg_result = camera_info_message_rx_->peek(index - 1);\n    if (msg_result) {\n      auto timestamp_components = msg_result->findAllHeap<gxf::Timestamp>();\n      if (!timestamp_components) {\n        return gxf::ToResultCode(timestamp_components);\n      }\n      if (0 == timestamp_components->size()) {\n        GXF_LOG_ERROR(\"No timestamp found from the camera info message\");\n        return GXF_ENTITY_COMPONENT_NOT_FOUND;\n      }\n      camera_info_acq_times_.push_back(\n          timestamp_components->front().value()->acqtime);\n    }\n  }\n\n  if (camera_acq_times_.size() == 0 || camera_info_acq_times_.size() == 0) {\n    GXF_LOG_ERROR(\"This array should not be empty at this stage\");\n    return GXF_FAILURE;\n  }\n\n  int64_t candidate_sync_point = std::numeric_limits<int64_t>::max();\n  if (sync_policy_.get() == kSyncNewestOnSlowestQueue) {\n    /* Find  newest timestamp from slowest moving queue, this makes sure with\n      the messages we have we will find a sync point closest to real time, and\n      not the potential non optimal time chosen by oldest message in fastest\n      moving queue\n      Each timestamps.front() is the newest message from that queue.We pick the minimum of\n      these newest timestamps\n    */\n    if (!camera_acq_times_.empty() && camera_acq_times_.front() < candidate_sync_point) {\n      candidate_sync_point = camera_acq_times_.front();\n    }\n    if (!camera_info_acq_times_.empty() && camera_info_acq_times_.front() < candidate_sync_point) {\n      candidate_sync_point = camera_info_acq_times_.front();\n    }\n  } else if (sync_policy_.get() == kSyncOldestOnFastestQueue) {\n    // find oldest timestamp we're going to pick from the fastest moving queue\n    candidate_sync_point = 0;\n    if (camera_acq_times_.back() > candidate_sync_point) {\n      candidate_sync_point = camera_acq_times_.back();\n    }\n    if (camera_info_acq_times_.back() > candidate_sync_point) {\n      candidate_sync_point = camera_info_acq_times_.back();\n    }\n  }\n\n  bool found_in_camera_queue = false;\n  bool found_in_camera_info_queue = false;\n  for (auto it = camera_acq_times_.begin(); it != camera_acq_times_.end(); it++) {\n    if (*it == candidate_sync_point) {\n      found_in_camera_queue = true;\n    }\n  }\n  for (auto it = camera_info_acq_times_.begin(); it != camera_info_acq_times_.end(); it++) {\n    if (*it == candidate_sync_point) {\n      found_in_camera_info_queue = true;\n    }\n  }\n\n  if (!found_in_camera_queue || !found_in_camera_info_queue) {\n    GXF_LOG_DEBUG(\"Did not find candidate sync point in one of the input queues\");\n    return GXF_SUCCESS;\n  }\n\n  GXF_LOG_DEBUG(\"Candidate timestamp for syncing (from camera queue): %zd\",\n                candidate_sync_point);\n  RETURN_IF_ERROR(syncMessages(camera_info_acq_times_, camera_info_message_rx_,\n                               camera_info_message_tx_, candidate_sync_point));\n  RETURN_IF_ERROR(syncMessages(camera_acq_times_, camera_message_rx_,\n                               camera_message_tx_, candidate_sync_point));\n  return GXF_SUCCESS;\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_camera_utils/gxf/extensions/camera_utils/camera_info_synchronizer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"gxf/core/gxf.h\"\n\n#include \"common/assert.hpp\"\n#include \"common/type_name.hpp\"\n#include \"gxf/core/handle.hpp\"\n#include \"gxf/core/parameter_parser_std.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// A codelet to sync camera messages with camera info messages. It will match the timestamp of a\n// camera info message with the timestamp of the corresponding camera message. If the parameter\n// for use_latest_camera_info is set to true, it will use the latest camera info message\n// It will make sure to update the camera info message with the same timestamp as that of the\n// camera message\nclass CameraInfoSynchronization : public gxf::Codelet {\n public:\n  gxf_result_t registerInterface(gxf::Registrar* registrar) override;\n  gxf_result_t start() override;\n  gxf_result_t tick() override;\n  // This function will drop oldest messages from either queues to keep the graph ticking.\n  // It is needed when a user decides to use this codelet with a MultiMessageAvailableScheduling\n  // term.\n  gxf_result_t dropOldestMessagesIfQueueIsFull();\n  // Gets latest camera info is in the queues and matches it with the image. It will update\n  // timestamp of the camera info message to be the same as that of the camera message.\n  gxf_result_t useLatestCameraInfo();\n\n private:\n  gxf::Parameter<gxf::Handle<gxf::Receiver>> camera_message_rx_;\n  gxf::Parameter<gxf::Handle<gxf::Receiver>> camera_info_message_rx_;\n  gxf::Parameter<gxf::Handle<gxf::Transmitter>> camera_message_tx_;\n  gxf::Parameter<gxf::Handle<gxf::Transmitter>> camera_info_message_tx_;\n  gxf::Parameter<bool> use_latest_camera_info_;\n  gxf::Parameter<bool> drop_old_messages_;\n  gxf::Parameter<uint64_t> sync_policy_;\n  // These parameters are used to store the timestamps of the camera and camera info messages\n  // These are preallocated in the start function to not have memory access during runtime\n  std::vector<int64_t> camera_acq_times_;\n  std::vector<int64_t> camera_info_acq_times_;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_camera_utils/gxf/extensions/camera_utils/camera_utils.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include \"extensions/camera_utils/camera_info_synchronizer.hpp\"\n#include \"extensions/camera_utils/stereo_camera_synchronizer.hpp\"\n#include \"gxf/std/extension_factory_helper.hpp\"\n\nGXF_EXT_FACTORY_BEGIN()\n\nGXF_EXT_FACTORY_SET_INFO(0xa722f33021690730, 0x8efa17986d37641d, \"NvIsaacCameraUtilsExtension\",\n                         \"Extension containing miscellaneous camera utility components\",\n                         \"Isaac SDK\", \"2.0.0\", \"LICENSE\");\n\nGXF_EXT_FACTORY_ADD(0x8760c3e2306c1ff6, 0xb9f0fb97cef0110f,\n                    nvidia::isaac::StereoCameraSynchronizer, nvidia::gxf::Codelet,\n                    \"Copies timestamp from left input msg to right input msg\");\n\nGXF_EXT_FACTORY_ADD(0x8760c3e2306c2ff6, 0xb9f0fb97cef0210f,\n                    nvidia::isaac::CameraInfoSynchronization, nvidia::gxf::Codelet,\n                    \"Synchronizes camera info and camera message, uses cached camera info if \"\n                    \"needed\");\n\nGXF_EXT_FACTORY_END()\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_camera_utils/gxf/extensions/camera_utils/stereo_camera_synchronizer.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"stereo_camera_synchronizer.hpp\"\n\n#include <limits>\n\n#include \"gems/gxf_helpers/expected_macro_gxf.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\ngxf_result_t StereoCameraSynchronizer::registerInterface(gxf::Registrar* registrar) {\n  gxf::Expected<void> result;\n  result &= registrar->parameter(\n      rx_right_camera_, \"rx_right_camera\", \"RX Right Camera Message\",\n      \"Input for Right CameraMessage entities\");\n  result &= registrar->parameter(\n      rx_left_camera_, \"rx_left_camera\", \"RX Left Camera Message\",\n      \"Input for Left CameraMessage entities\");\n  result &= registrar->parameter(\n      tx_right_camera_, \"tx_right_camera\", \"TX Right Camera Message\",\n      \"Input for Right CameraMessage entities\");\n  result &= registrar->parameter(\n      tx_left_camera_, \"tx_left_camera\", \"TX Left Camera Message\",\n      \"Input for Left CameraMessage entities\");\n  result &= registrar->parameter(\n      max_timestamp_diff_, \"max_timestamp_diff\", \"Max difference timestamps nanoseconds\",\n      \"Max difference between timestamps in nanoseconds\",\n      static_cast<int64_t>(200000));  // 200 micro seconds\n  return gxf::ToResultCode(result);\n}\n\ngxf_result_t StereoCameraSynchronizer::start() {\n  // Set all previous timestamps to minimum value of int64_t\n  // This ensures that the first message received does NOT trigger the\n  // \"not monotonically increasing\" error\n  prev_left_unnamed_timestamp_ = std::numeric_limits<int64_t>::min();\n  prev_right_unnamed_timestamp_ = std::numeric_limits<int64_t>::min();\n  prev_left_named_timestamp_ = std::numeric_limits<int64_t>::min();\n  prev_right_named_timestamp_ = std::numeric_limits<int64_t>::min();\n  return GXF_SUCCESS;\n}\n\ngxf_result_t StereoCameraSynchronizer::tick() {\n  auto left_camera_entity = UNWRAP_OR_RETURN(rx_left_camera_->receive());\n  auto right_camera_entity = UNWRAP_OR_RETURN(rx_right_camera_->receive());\n  auto left_unnamed_timestamp = UNWRAP_OR_RETURN(left_camera_entity.get<gxf::Timestamp>());\n  auto right_unnamed_timestamp = UNWRAP_OR_RETURN(right_camera_entity.get<gxf::Timestamp>());\n  auto left_named_timestamp = UNWRAP_OR_RETURN(left_camera_entity.get<gxf::Timestamp>(\"timestamp\"));\n  auto right_named_timestamp = UNWRAP_OR_RETURN(\n    right_camera_entity.get<gxf::Timestamp>(\"timestamp\"));\n\n  const int64_t unnamed_diff = std::abs(left_unnamed_timestamp->acqtime -\n    right_unnamed_timestamp->acqtime);\n  if (unnamed_diff > max_timestamp_diff_) {\n    GXF_LOG_ERROR(\"L/R frames are not synchronized, unnamed timestamp diff = %ld ns\", unnamed_diff);\n    return GXF_FAILURE;\n  }\n\n  const int64_t named_diff = std::abs(left_named_timestamp->acqtime -\n    right_named_timestamp->acqtime);\n  if (named_diff > max_timestamp_diff_) {\n    GXF_LOG_ERROR(\"L/R frames are not synchronized, named timestamp diff = %ld ns\", named_diff);\n    return GXF_FAILURE;\n  }\n  // Set right input timestamp to left input timestamp\n  right_unnamed_timestamp->acqtime = left_unnamed_timestamp->acqtime;\n  right_named_timestamp->acqtime = left_named_timestamp->acqtime;\n\n\n  // Check if previous timestamp is lower than current timestamp\n  if (prev_left_unnamed_timestamp_ > left_unnamed_timestamp->acqtime) {\n    GXF_LOG_ERROR(\"Left unnamed timestamp is not monotonically increasing\");\n    return GXF_FAILURE;\n  }\n  if (prev_right_unnamed_timestamp_ > right_unnamed_timestamp->acqtime) {\n    GXF_LOG_ERROR(\"Right unnamed timestamp is not monotonically increasing\");\n    return GXF_FAILURE;\n  }\n  if (prev_left_named_timestamp_ > left_named_timestamp->acqtime) {\n    GXF_LOG_ERROR(\"Left named timestamp is not monotonically increasing\");\n    return GXF_FAILURE;\n  }\n  if (prev_right_named_timestamp_ > right_named_timestamp->acqtime) {\n    GXF_LOG_ERROR(\"Right named timestamp is not monotonically increasing\");\n    return GXF_FAILURE;\n  }\n\n  prev_left_unnamed_timestamp_ = left_unnamed_timestamp->acqtime;\n  prev_right_unnamed_timestamp_ = right_unnamed_timestamp->acqtime;\n  prev_left_named_timestamp_ = left_named_timestamp->acqtime;\n  prev_right_named_timestamp_ = right_named_timestamp->acqtime;\n\n  RETURN_IF_ERROR(tx_left_camera_->publish(left_camera_entity));\n  RETURN_IF_ERROR(tx_right_camera_->publish(right_camera_entity));\n\n  return GXF_SUCCESS;\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_camera_utils/gxf/extensions/camera_utils/stereo_camera_synchronizer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// This codelet checks if stereo frames timestamps are within a tolerance and\n// will edit the right input timestamps to match the left input timestamp\n// Some downstream applications expect matched stereo frames to have exactly the same timestamp\n// even though in reality matched frames can differ by a few microseconds.\nclass StereoCameraSynchronizer : public gxf::Codelet {\n  gxf_result_t registerInterface(gxf::Registrar* registrar) override;\n  gxf_result_t start() override;\n  gxf_result_t tick() override;\n  gxf_result_t stop() override { return GXF_SUCCESS; }\n\n private:\n  gxf::Parameter<gxf::Handle<gxf::Receiver>> rx_right_camera_;\n  gxf::Parameter<gxf::Handle<gxf::Receiver>> rx_left_camera_;\n  gxf::Parameter<gxf::Handle<gxf::Transmitter>> tx_right_camera_;\n  gxf::Parameter<gxf::Handle<gxf::Transmitter>> tx_left_camera_;\n  gxf::Parameter<int64_t> max_timestamp_diff_;\n  int64_t prev_left_unnamed_timestamp_, prev_right_unnamed_timestamp_;\n  int64_t prev_left_named_timestamp_, prev_right_named_timestamp_;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_camera_utils/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_camera_utils</name>\n  <version>4.4.0</version>\n  <description>NvIsaacCameraUtilsExtension</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n  <depend>gxf_isaac_messages</depend>\n  <depend>gxf_isaac_tensorops</depend>\n  <depend>isaac-ros-cli</depend>\n  <depend>tensorrt</depend>\n\n  <build_depend>gxf_isaac_gems</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_cuda/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_cuda LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_LINK_LIBRARIES \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n    install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n            DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n    set_target_properties(${PROJECT_NAME} PROPERTIES\n            INTERFACE_INCLUDE_DIRECTORIES \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_cuda/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_cuda</name>\n  <version>4.4.0</version>\n  <description>GXF extension for Cuda related components in GXF Core.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_flatscan_localization/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_flatscan_localization LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_flatscan_localization/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_flatscan_localization</name>\n  <version>4.4.0</version>\n  <description>Extension containing flatscan localization related components.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_gems LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_property(TARGET ${PROJECT_NAME} PROPERTY\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"${EIGEN3_INCLUDE_DIR}\"\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\"\n  )\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/algorithm/string_utils.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <algorithm>\n#include <cctype>\n#include <random>\n#include <sstream>\n#include <string>\n#include <vector>\n\nnamespace nvidia {\nnamespace isaac {\n\n// Checks wether a string starts with a certain string\ninline bool StartsWith(const std::string& str, const std::string& prefix) {\n  return str.size() >= prefix.size()\n      && str.compare(0, prefix.size(), prefix) == 0;\n}\n\n// Checks wether a string ends with a certain string\ninline bool EndsWith(const std::string& str, const std::string& suffix) {\n  return str.size() >= suffix.size()\n      && str.compare(str.size() - suffix.size(), suffix.size(), suffix) == 0;\n}\n\n// Creates a random alpha-numeric string\ntemplate <typename URBG>\nstd::string RandomAlphaNumeric(size_t length, URBG&& rng) {\n  // All alpha numeric characters\n  constexpr char kAlphaNumeric[] =\n      \"ABCDEFGHIJKLMNOPQRSTUVWXYZ\"\n      \"abcdefghijklmnopqrstuvwxyz\"\n      \"0123456789\";\n  std::uniform_int_distribution<size_t> random_index(0, sizeof(kAlphaNumeric) - 1);\n  std::string result(length, 0);\n  std::generate_n(result.begin(), length, [&]() { return kAlphaNumeric[random_index(rng)]; });\n  return result;\n}\n\n// Splits a string by deliminator character\ninline std::vector<std::string> SplitString(const std::string& str, const char delim) {\n  std::vector<std::string> result;\n  std::stringstream ss(str);\n  std::string item;\n  while (!std::getline(ss, item, delim).fail()) {\n    result.push_back(item);\n  }\n  return result;\n}\n\n// Converts a string to lower-case characters\ninline std::string ToLowerCase(std::string data) {\n  std::transform(data.begin(), data.end(), data.begin(), ::tolower);\n  return data;\n}\n\n// Trims the spaces at the beginning and end of string\ninline std::string TrimString(std::string s) {\n  s.erase(s.begin(), find_if(s.begin(), s.end(), [](int ch) { return !std::isspace(ch); }));\n  s.erase(find_if(s.rbegin(), s.rend(), [](int ch) { return !std::isspace(ch); }).base(), s.end());\n  return s;\n}\n\n// Returns a new string containing the last count characters of a string\ninline std::string TakeLast(const std::string& str, size_t count) {\n  if (str.length() <= count) {\n    return str;\n  } else {\n    if (count < 2) {\n      return str.substr(str.length() - count, count);\n    } else {\n      return \"..\" + str.substr(str.length() - count + 2, count - 2);\n    }\n  }\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/common/composite_schema_uid.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cstdint>\n\nnamespace nvidia {\nnamespace isaac {\n\n// Composite schema identification for the data in a message.\nstruct CompositeSchemaUid {\n  // Unique identifier of the composite schema which is used to represent the data\n  uint64_t uid;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/common/pose_frame_uid.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cstdint>\n\nnamespace nvidia {\nnamespace isaac {\n\n// Spatial information for the data in a message.\nstruct PoseFrameUid {\n  // Unique identifier of the pose tree frame to which this data is attached.\n  uint64_t uid;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/composite/composite_from_tensor.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/composite/composite_view.hpp\"\n#include \"gems/composite/schema_tensor_archive.hpp\"\n#include \"gems/core/tensor/tensor.hpp\"\n#include \"gxf/core/expected.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\nnamespace from_tensor_details {\n\n// Check to see if we actually derive from a composite view\nstd::false_type composite_view_match(const void*);\ntemplate <typename K, int32_t N>\nstd::true_type composite_view_match(const CompositeContainerConstPointer<K, N>*);\ntemplate <typename K, int32_t N>\nstd::true_type composite_view_match(const CompositeContainerPointer<K, N>*);\n\n// Check to see if we actually derive from a composite view\nstd::false_type composite_data_match(const void*);\ntemplate <typename K, int32_t N>\nstd::true_type composite_data_match(const CompositeContainerArray<K, N>*);\ntemplate <typename K, int32_t N>\nstd::true_type composite_data_match(const CompositeContainerEigen<K, N>*);\n\n// Define helper types.\ntemplate <typename T>\nusing is_composite_view = decltype(composite_view_match(std::declval<T*>()));\ntemplate <typename T>\nusing is_composite_data = decltype(composite_data_match(std::declval<T*>()));\n\n}  // namespace from_tensor_details\n\n// Constructs CompositeConstView from TensorConstView\ntemplate <typename T>\ngxf::Expected<T> CompositeFromTensor(\n    ::nvidia::isaac::CpuTensorConstView<typename T::scalar_t, 1> tensor) {\n  static_assert(from_tensor_details::is_composite_view<T>::value, \"Type needs to be a composite\");\n  if (tensor.dimensions()[0] != T::kDimension) {\n    return gxf::Unexpected{GXF_INVALID_DATA_FORMAT};\n  }\n  T derived;\n  derived.pointer = tensor.element_wise_begin();\n  return derived;\n}\n\n// Constructs CompositeView or CompositeConstView from TensorView\ntemplate <typename T>\ngxf::Expected<T> CompositeFromTensor(\n    ::nvidia::isaac::CpuTensorView<typename T::scalar_t, 1> tensor) {\n  static_assert(from_tensor_details::is_composite_view<T>::value, \"Type needs to be a composite\");\n  if (tensor.dimensions()[0] != T::kDimension) {\n    return gxf::Unexpected{GXF_INVALID_DATA_FORMAT};\n  }\n  T derived;\n  derived.pointer = tensor.element_wise_begin();\n  return derived;\n}\n\n// Constructs CompositeView or CompositeConstView from Tensor\ntemplate <typename T>\ngxf::Expected<T> CompositeFromTensor(::nvidia::isaac::CpuTensor<typename T::scalar_t, 1>& tensor) {\n  return CompositeFromTensor<T>(tensor.view());\n}\n\n// Constructs CompositeArray or CompositeEigen from TensorConstView and schemas\ntemplate <typename T>\ngxf::Expected<T> CompositeFromTensor(\n    ::nvidia::isaac::CpuTensorConstView<typename T::scalar_t, 1> tensor,\n    const composite::Schema& tensor_schema,\n    const composite::Schema& composite_schema) {\n  static_assert(from_tensor_details::is_composite_data<T>::value, \"Type must be a composite data \"\n                \"container, since we cannot guarantee parameter order\");\n  const auto maybe_index_map = CompositeIndexMap::Create(composite_schema, tensor_schema);\n  if (!maybe_index_map) {\n    return gxf::Unexpected{GXF_INVALID_DATA_FORMAT};\n  }\n\n  T derived;\n  FromSchemaTensor(tensor, *maybe_index_map, derived);\n  return derived;\n}\n\n// Constructs CompositeArray or CompositeEigen from Tensor and schemas\ntemplate <typename T>\ngxf::Expected<T> CompositeFromTensor(::nvidia::isaac::CpuTensor<typename T::scalar_t, 1>& tensor,\n    const composite::Schema& tensor_schema,\n    const composite::Schema& composite_schema) {\n  return CompositeFromTensor<T>(tensor.const_view(), tensor_schema, composite_schema);\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/composite/composite_view.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <array>\n#include <cstdint>\n#include <type_traits>\n\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Forward declare for operator=\ntemplate <typename K, int32_t N>\nstruct CompositeContainerConstPointer;\ntemplate <typename K, int32_t N>\nstruct CompositeContainerPointer;\ntemplate <typename K, int32_t N>\nstruct CompositeContainerArray;\ntemplate <typename K, int32_t N>\nstruct CompositeContainerEigen;\n\n// A base class for an immutable composite view which uses a const pointer to access data.\ntemplate <typename K, int32_t N>\nstruct CompositeContainerConstPointer {\n  CompositeContainerConstPointer() : pointer(nullptr) {}\n  CompositeContainerConstPointer(const K* p) : pointer(p) {}\n\n  static_assert(std::is_arithmetic<K>::value, \"Composite only supports arithmetic types\");\n\n  using scalar_t = K;\n  using Scalar = K;\n  static constexpr int32_t kDimension = N;\n\n  constexpr int32_t size() const { return N; }\n\n  K operator[](int32_t i) const { return pointer[i]; }\n\n  const K* pointer;\n};\n\n// Accesses an element in a composite\ntemplate <int32_t I, typename K, int32_t N>\nK get(const CompositeContainerConstPointer<K, N>& state) {\n  static_assert(0 <= I && I < N, \"Index out of bounds\");\n  return state.pointer[I];\n}\n\n// A base class for a mutable composite view which uses a pointer to access data.\ntemplate <typename K, int32_t N>\nstruct CompositeContainerPointer {\n  CompositeContainerPointer() : pointer(nullptr) {}\n  CompositeContainerPointer(K* p) : pointer(p) {}\n\n  static_assert(std::is_arithmetic<K>::value, \"Composite only supports arithmetic types\");\n\n  using scalar_t = K;\n  using Scalar = K;\n  static constexpr int32_t kDimension = N;\n\n  constexpr int32_t size() const { return N; }\n\n  K operator[](int32_t i) const { return pointer[i]; }\n  K& operator[](int32_t i) { return pointer[i]; }\n\n  CompositeContainerPointer<K, N>& operator=(const CompositeContainerConstPointer<K, N>& other) {\n    for (int32_t i = 0; i < size(); ++i) {\n      pointer[i] = other[i];\n    }\n    return *this;\n  }\n  CompositeContainerPointer<K, N>& operator=(const CompositeContainerArray<K, N>& other) {\n    for (int32_t i = 0; i < size(); ++i) {\n      pointer[i] = other[i];\n    }\n    return *this;\n  }\n  CompositeContainerPointer<K, N>& operator=(const CompositeContainerEigen<K, N>& other) {\n    for (int32_t i = 0; i < size(); ++i) {\n      pointer[i] = other[i];\n    }\n    return *this;\n  }\n\n  K* pointer;\n};\n\n// Accesses an element in a composite\ntemplate <int32_t I, typename K, int32_t N>\nK get(const CompositeContainerPointer<K, N>& state) {\n  static_assert(0 <= I && I < N, \"Index out of bounds\");\n  return state.pointer[I];\n}\n\n// Accesses an element in a composite\ntemplate <int32_t I, typename K, int32_t N>\nK& get(CompositeContainerPointer<K, N>& state) {\n  static_assert(0 <= I && I < N, \"Index out of bounds\");\n  return state.pointer[I];\n}\n\n// A base class for a composite which uses std::array to store data.\ntemplate <typename K, int32_t N>\nstruct CompositeContainerArray {\n  static_assert(std::is_arithmetic<K>::value, \"Composite only supports arithmetic types\");\n\n  using scalar_t = K;\n  using Scalar = K;\n  static constexpr int32_t kDimension = N;\n\n  constexpr int32_t size() const { return N; }\n\n  K operator[](int32_t i) const { return data[i]; }\n  K& operator[](int32_t i) { return data[i]; }\n\n  CompositeContainerArray<K, N>& operator=(const CompositeContainerConstPointer<K, N>& other) {\n    for (int32_t i = 0; i < size(); ++i) {\n      data[i] = other[i];\n    }\n    return *this;\n  }\n  CompositeContainerArray<K, N>& operator=(const CompositeContainerPointer<K, N>& other) {\n    for (int32_t i = 0; i < size(); ++i) {\n      data[i] = other[i];\n    }\n    return *this;\n  }\n\n  std::array<K, N> data;\n};\n\n// A base class for a composite which uses ::nvidia::isaac::Vector to store data.\ntemplate <typename K, int32_t N>\nstruct CompositeContainerEigen {\n  static_assert(std::is_arithmetic<K>::value, \"Composite only supports arithmetic types\");\n\n  using scalar_t = K;\n  using Scalar = K;\n  static constexpr int32_t kDimension = N;\n\n  constexpr int32_t size() const { return N; }\n\n  K operator[](int32_t i) const { return elements[i]; }\n  K& operator[](int32_t i) { return elements[i]; }\n\n  CompositeContainerEigen<K, N>& operator=(const CompositeContainerConstPointer<K, N>& other) {\n    for (int32_t i = 0; i < size(); ++i) {\n      elements[i] = other[i];\n    }\n    return *this;\n  }\n  CompositeContainerEigen<K, N>& operator=(const CompositeContainerPointer<K, N>& other) {\n    for (int32_t i = 0; i < size(); ++i) {\n      elements[i] = other[i];\n    }\n    return *this;\n  }\n\n  ::nvidia::isaac::Vector<K, N> elements;\n};\n\n// Accesses an element in a composite\ntemplate <int32_t I, typename K, int32_t N>\nK get(const CompositeContainerArray<K, N>& state) {\n  return std::get<I>(state.data);\n}\n\n// Accesses an element in a composite\ntemplate <int32_t I, typename K, int32_t N>\nK& get(CompositeContainerArray<K, N>& state) {\n  return std::get<I>(state.data);\n}\n\n// Accesses an element in a composite\ntemplate <int32_t I, typename K, int32_t N>\nK get(const CompositeContainerEigen<K, N>& state) {\n  return state.elements[I];\n}\n\n// Accesses an element in a composite\ntemplate <int32_t I, typename K, int32_t N>\nK& get(CompositeContainerEigen<K, N>& state) {\n  return state.elements[I];\n}\n\n// An immutable composite view\ntemplate <typename K, typename Indices, template <typename, typename> typename ImmutableInterface>\nusing CompositeConstView = ImmutableInterface<K, CompositeContainerConstPointer<K, Indices::kSize>>;\n\n// A mutable composite view\ntemplate <typename K, typename Indices, template <typename, typename> typename MutableInterface>\nusing CompositeView = MutableInterface<K, CompositeContainerPointer<K, Indices::kSize>>;\n\n// A composite which owns its data\ntemplate <typename K, typename Indices, template <typename, typename> typename MutableInterface>\nusing Composite = MutableInterface<K, CompositeContainerArray<K, Indices::kSize>>;\n\n// A composite which owns its data as an Eigen Vector\ntemplate <typename K, typename Indices, template <typename, typename> typename MutableInterface>\nusing CompositeEigen = MutableInterface<K, CompositeContainerEigen<K, Indices::kSize>>;\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/composite/measure.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\nnamespace nvidia {\nnamespace isaac {\nnamespace composite {\n\n// Possible measure for an entity in Composite Message\nenum class Measure {\n  kNone,                 // Unknown\n  kTime,                 // Second\n  kMass,                 // Kilogram\n  kPosition,             // Meter\n  kSpeed,                // Meter per second\n  kAcceleration,         // Meter per squared second\n  kRotation,             // Rotation of 2d or 3d\n  kAngularSpeed,         // Radian per second\n  kAngularAcceleration,  // Radian per squared second\n  kNormal,               // A normal\n  kColor,                // A color\n  kCurvature,            // Radian per meter\n  kCurvatureDerivative,  // Radian per squared meter\n};\n\n}  // namespace composite\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/composite/quantity.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n#include <utility>\n\n#include \"gems/composite/measure.hpp\"\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace composite {\n\n// A quantity describes meta data for values stored in a composite.\nstruct Quantity {\n  // Helper function to create a scalar quantity\n  static Quantity Scalar(std::string entity, Measure measure) {\n    return Quantity{std::move(entity), measure, ::nvidia::isaac::VectorXi::Constant(1, 1)};\n  }\n\n  // Helper function to create a vector quantity\n  static Quantity Vector(std::string entity, Measure measure, int dimension) {\n    return Quantity{std::move(entity), measure, ::nvidia::isaac::VectorXi::Constant(1, dimension)};\n  }\n\n  // Equality operator\n  bool operator==(const Quantity& other) const {\n    return measure == other.measure && entity == other.entity && dimensions == other.dimensions;\n  }\n\n  // Total number of elements\n  int getElementCount() const { return dimensions.prod(); }\n\n  // The name of the entity for which the values was measured. Multiple quantities can be\n  // measured per entity.\n  std::string entity;\n  // The measure, e.g. unit or type, of the value.\n  Measure measure;\n  // The dimensions of the value. Values can be multi-dimensional but most are scalars or vectors.\n  ::nvidia::isaac::VectorXi dimensions;\n};\n\n// Exact comparison of quantities\nstruct QuantityEquality {\n  bool operator()(const Quantity& lhs, const Quantity& rhs) const { return lhs == rhs; }\n};\n\n// Hash function for quantities using combination of hashes on elements\nstruct QuantityHash {\n  std::size_t operator()(const Quantity& value) const {\n    return std::hash<std::string>{}.operator()(value.entity) ^\n           std::hash<int32_t>{}.operator()(static_cast<int32_t>(value.measure)) ^\n           std::hash<int32_t>{}.operator()(static_cast<int32_t>(value.dimensions.size()));\n  }\n};\n\n}  // namespace composite\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/composite/schema.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <optional>\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#include \"gems/composite/quantity.hpp\"\n#include \"gems/core/math/types.hpp\"\n#include \"gems/core/optional.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace composite {\n\n// A schema contains information about the quantities which are stored in the composite\nclass Schema {\n public:\n  // Constructs schema from given quanties\n  static std::optional<Schema> Create(std::vector<Quantity>&& quantities, std::string hash = \"\");\n  // Constructs schema from scalar quantities of identical measure\n  static std::optional<Schema> Create(\n      const std::vector<std::string>& entities, const Measure measure);\n\n  // Similar to `Create` but will Assert if schema is invalid\n  Schema(std::vector<Quantity>&& quantities, std::string hash = \"\");\n  // Similar to `Create` but will Assert if schema is invalid\n  Schema(const std::vector<std::string>& entities, const Measure measure);\n\n  // Empty schema\n  Schema() = default;\n\n  // Equality operator\n  bool operator==(const Schema& other) const {\n    return element_count_ == other.element_count_ && hash_ == other.hash_ &&\n           quantities_ == other.quantities_;\n  }\n\n  // The hash of the schame\n  const std::string& getHash() const { return hash_; }\n\n  // Returns a list of all quantities\n  const std::vector<Quantity>& getQuantities() const { return quantities_; }\n\n  // Total number of values required to store the schema\n  int getElementCount() const { return element_count_; }\n\n  // Gets the starting index of a quantity in the list ofvalues. Returns nullopt if quantity is\n  // not in the schema.\n  std::optional<int> findQuantityValueIndex(const Quantity& quantity) const;\n\n private:\n  // Updates `element_count_`, `values_offset_` and `index_` variables based on quantities.\n  bool createIndex();\n\n  // The hash of the currently used schema\n  std::string hash_;\n\n  // All quantities which are part of this schema\n  std::vector<Quantity> quantities_;\n\n  // Total number of elements in the schema\n  int element_count_ = 0;\n\n  // Indies of first quantity element in the values vector\n  std::unordered_map<Quantity, int, QuantityHash, QuantityEquality> index_;\n};\n\n}  // namespace composite\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/composite/schema_tensor_archive.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <array>\n#include <cstdint>\n#include <vector>\n\n#include \"common/expected.hpp\"\n#include \"gems/composite/schema.hpp\"\n#include \"gems/core/tensor/tensor.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// An index map which can be used to map indices from one composite to another.\nclass CompositeIndexMap {\n public:\n  // Error codes used by this data type\n  enum struct Error {\n    kQuantityNotFound,  // A quantity from the target schema was not found in the source schema.\n  };\n\n  // Computes an index map which can be used to associate elements in the target schema with\n  // elements in the source schema. Will fail in case the mapping is not possible.\nstatic Expected<CompositeIndexMap, Error> Create(const composite::Schema& source,\n                                                 const composite::Schema& target);\n\n  // Map a target index to the corresponding source index (see ComputeCompositeIndexMap)\n  int32_t operator()(int32_t target) const { return indices_[target]; }\n\n private:\n  // Private constructor to force initialization through static builder for RAII.\n  CompositeIndexMap(int size) : indices_(size) {}\n\n  // Stores the source index for each target index.\n  // FIXME Use a stack allocated, bounded data type once available.\n  std::vector<int32_t> indices_;\n};\n\n// Helper type to encode a composite into a tensor\ntemplate <typename K>\nstruct TensorArchiveEncoder {\n  // Writes a value into the tensor at the given index.\n  void operator()(int32_t index, K value) {\n    composite_tensor(index) = value;\n  }\n\n  // The encoder writes data to this tensor.\n  ::nvidia::isaac::CpuTensorView1<K> composite_tensor;\n};\n\n// Helper type to decode a composite from a tensor with the help of an index map\ntemplate <typename K>\nstruct TensorArchiveDecoder {\n  // Reads a value from the tensor using an index from the *target* schema. The index map is used\n  // to map target schema indices to indices used to access the tensor.\n  void operator()(int32_t index, K& value) {\n    value = composite_tensor(composite_index_map(index));\n  }\n\n  // The decoder reads data from this tensor.\n  ::nvidia::isaac::CpuTensorConstView1<K> composite_tensor;\n  // An index map which is used to map indices given to `operator()` to the correct location in\n  // the tensor.\n  CompositeIndexMap composite_index_map;\n};\n\n// Reads a composite state from a tensor\ntemplate <typename State>\nvoid FromSchemaTensor(\n    ::nvidia::isaac::CpuTensorConstView1<typename State::scalar_t> composite_tensor,\n    const CompositeIndexMap& composite_index_map, State& state) {\n  TensorArchiveDecoder<typename State::scalar_t> decoder{composite_tensor, composite_index_map};\n  for (int32_t i = 0; i < state.size(); i++) {\n    decoder(i, state[i]);\n  }\n}\n\n// Writes a composite state to a tensor\ntemplate <typename State>\nvoid ToSchemaTensor(const State& state,\n                    ::nvidia::isaac::CpuTensorView1<typename State::scalar_t> composite_tensor) {\n  TensorArchiveEncoder<typename State::scalar_t> encoder{composite_tensor};\n  for (int32_t i = 0; i < state.size(); i++) {\n    encoder(i, state[i]);\n  }\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/coms/socket.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <arpa/inet.h>\n#include <ifaddrs.h>\n#include <linux/if.h>\n#include <netinet/in.h>\n#include <netinet/tcp.h>\n#include <sys/ioctl.h>\n#include <sys/poll.h>\n#include <sys/socket.h>\n#include <unistd.h>\n\n#include <cstring>\n#include <functional>\n#include <string>\n\n#include \"gems/core/logger.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\nclass Socket {\n public:\n  // Creates a UDP receiving socket\n  // Remote address corresponds to the sending socket. Port will be used for remote and local port\n  static Socket* CreateRxUDPSocket(const std::string& remote_address, uint16_t port) {\n    Socket* s = new Socket(remote_address, port, SOCK_DGRAM);\n    memset(&s->local_address_, 0, sizeof(s->local_address_));\n    s->local_address_.sin_port   = htons(port);\n    s->local_address_.sin_family = AF_INET;\n    s->local_address_.sin_addr.s_addr = htonl(INADDR_ANY);\n    return s;\n  }\n\n  // Creates a TCP receiving socket\n  static Socket* CreateRxTCPSocket(const std::string& source_address, uint16_t source_port) {\n    return new Socket(source_address, source_port, SOCK_STREAM);\n  }\n\n  // Try to start a socket or fails and return -1\n  int32_t startSocket() {\n    int32_t res = 0;\n\n    // TODO Assert\n    if (type_ == SOCK_STREAM) {\n      res = connect(sockfd_, (struct sockaddr *)&emitter_address_,\n              sizeof(emitter_address_));\n      LOG_WARNING(\"Never tested\");\n      // Set reading options\n      read_flags_ = MSG_PEEK | MSG_WAITALL;\n    } else if (type_ == SOCK_DGRAM) {\n      uint32_t reuse = 1;\n      // Set Reuse Option\n      struct timeval tv;\n      tv.tv_sec = 1;\n      tv.tv_usec = 0;\n      res |= setsockopt(sockfd_, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));\n      res |= setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(uint32_t));\n      if (res < 0) {\n        LOG_ERROR(\"Cannot set Socket options\");\n      }\n      if (bind(sockfd_, (struct sockaddr *)&local_address_, sizeof(local_address_)) == -1) {\n        LOG_ERROR(\"Cannot bind!\");\n        return -1;\n      }\n      read_flags_ = 0;\n    }\n    is_running_ = true;\n    return res;\n  }\n\n  // Reads a packet\n  size_t readPacket(char* data, size_t size) {\n    if (!sockfd_) {\n      LOG_ERROR(\"Socket is invalid, reopen the connection!\");\n      return -1;\n    }\n    socklen_t slen = sizeof(emitter_address_);\n    int32_t recvlen = recvfrom(sockfd_, data, size, 0,\n                  (struct sockaddr *)&emitter_address_, &slen);\n    return recvlen;\n  }\n\n  // Reads a packet from\n  size_t writePacket(const void* data, size_t size) {\n    if (!sockfd_) {\n      LOG_ERROR(\"Socket is invalid, reopen the connection!\");\n      return -1;\n    }\n    socklen_t slen = sizeof(emitter_address_);\n    int32_t recvlen = sendto(sockfd_, data, size, 0,\n                            (struct sockaddr *)&emitter_address_, slen);\n    return recvlen;\n  }\n\n  // Closes socket\n  void closeSocket() {\n    if (sockfd_) {\n      close(sockfd_);\n    }\n    is_running_ = false;\n  }\n\n  bool isRunning() {\n    return is_running_;\n  }\n\n  // Identify the (externally exposed) IPv4 address corresponding to this communication socket.\n  uint32_t getIpv4() const {\n    uint32_t local_ipv4_address(0);\n\n    // For each connected IPv4 network interface address, if the emitter address meets masking\n    // requirement, we found our local IPv4. Don't bother checking if the socket is not running.\n    struct ifaddrs* network_interface_addresses = nullptr;\n    if (getifaddrs(&network_interface_addresses) != -1) {\n      struct ifaddrs* network_interface_addresses_iterator = network_interface_addresses;\n      while (network_interface_addresses_iterator != nullptr) {\n        // Validate the socket family is indeed IPv4.\n        if ((network_interface_addresses_iterator->ifa_addr != nullptr) &&\n            (network_interface_addresses_iterator->ifa_addr->sa_family == AF_INET)) {\n          // Validate the socket is actually connected.\n          struct sockaddr_in* local_socket_address =\n              (struct sockaddr_in*)network_interface_addresses_iterator->ifa_addr;\n          if (local_socket_address->sin_addr.s_addr != htonl(INADDR_ANY)) {\n            // Validate the network masking requirements meets our emitter address.\n            struct sockaddr_in* ifu_netmask =\n                (struct sockaddr_in*)network_interface_addresses_iterator->ifa_netmask;\n            if ((local_socket_address->sin_addr.s_addr & ifu_netmask->sin_addr.s_addr) ==\n                (emitter_address_.sin_addr.s_addr & ifu_netmask->sin_addr.s_addr)) {\n              local_ipv4_address = local_socket_address->sin_addr.s_addr;\n              break;\n            }\n          }\n        }\n        network_interface_addresses_iterator = network_interface_addresses_iterator->ifa_next;\n      }\n\n      if (network_interface_addresses) {\n        freeifaddrs(network_interface_addresses);\n      }\n    }\n\n    return local_ipv4_address;\n  }\n\n  int32_t getFileDescriptor() const {\n    return sockfd_;\n  }\n\n private:\n  int32_t sockfd_;\n  struct sockaddr_in local_address_;\n  struct sockaddr_in emitter_address_;\n\n  int32_t read_flags_;\n  int32_t type_;\n  bool is_running_;\n\n  Socket(const std::string& remote_address, uint16_t remote_port, uint16_t type,\n       const std::string& interface = \"\") {\n    // Prepares for incoming data\n    memset(&emitter_address_, 0, sizeof(emitter_address_));\n    emitter_address_.sin_port   = htons(remote_port);\n    emitter_address_.sin_family = AF_INET;\n    emitter_address_.sin_addr.s_addr = inet_addr(remote_address.c_str());\n    type_ = type;\n    is_running_ = false;\n\n    // Creates the socket\n    sockfd_ = socket(AF_INET, type, 0);\n\n    if (!interface.empty()) {\n      struct ifreq if_bind;\n      strncpy(if_bind.ifr_name, interface.c_str(), IFNAMSIZ);\n      const int error = setsockopt(sockfd_, SOL_SOCKET, SO_BINDTODEVICE,\n                                   reinterpret_cast<char*>(&if_bind), sizeof(if_bind));\n      if (error < 0) {\n        LOG_ERROR(\"Failed to specify interface %s\", interface.c_str());\n      }\n    }\n  }\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/control_types/differential_drive.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cstdint>\n\n#include \"gems/composite/composite_view.hpp\"\n#include \"gems/composite/schema.hpp\"\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Describes the full 2D pose and kinematic state of a Differential base\nstruct DifferentialBaseEgoMotionIndices {\n  enum {\n    // Cartesian position X (m)\n    kPositionX,\n    // Cartesian position Y (m)\n    kPositionY,\n    // In-plane rotation angle (radian)\n    kHeading,\n    // Linear (longitudinal) speed (m/s)\n    kLinearSpeed,\n    // Rotation speed (radian/s)\n    kAngularSpeed,\n    // Linear (longitudinal) acceleration (m/s^2)\n    kLinearAcceleration,\n    // Angular (rotaion) acceleration (radians/s^2)\n    kAngularAcceleration,\n    kSize\n  };\n};\n\ninline composite::Schema DifferentialBaseEgoMotionCompositeSchema() {\n  return composite::Schema({\n    composite::Quantity::Vector(\"position\", composite::Measure::kPosition, 2),\n    composite::Quantity::Scalar(\"heading\", composite::Measure::kRotation),\n    composite::Quantity::Scalar(\"linear_speed\", composite::Measure::kSpeed),\n    composite::Quantity::Scalar(\"angular_speed\", composite::Measure::kAngularSpeed),\n    composite::Quantity::Scalar(\"linear_acceleration\", composite::Measure::kAcceleration),\n    composite::Quantity::Scalar(\"angular_acceleration\", composite::Measure::kAngularAcceleration),\n  });\n}\n\ntemplate <typename K, typename Base>\nstruct DifferentialBaseEgoMotionImmutableInterface : public Base {\n  using Base::operator=;\n  using Vector2ConstMap = Eigen::Map<\n      const ::nvidia::isaac::Vector2<K>, 0,\n    Eigen::InnerStride<DifferentialBaseEgoMotionIndices::kPositionY\n                       - DifferentialBaseEgoMotionIndices::kPositionX>>;\n  K position_x() const { return get<DifferentialBaseEgoMotionIndices::kPositionX>(*this); }\n  K position_y() const { return get<DifferentialBaseEgoMotionIndices::kPositionY>(*this); }\n  K heading() const { return get<DifferentialBaseEgoMotionIndices::kHeading>(*this); }\n  K linear_speed() const { return get<DifferentialBaseEgoMotionIndices::kLinearSpeed>(*this); }\n  K angular_speed() const { return get<DifferentialBaseEgoMotionIndices::kAngularSpeed>(*this); }\n  K linear_acceleration() const {\n      return get<DifferentialBaseEgoMotionIndices::kLinearAcceleration>(*this); }\n  K angular_acceleration() const {\n      return get<DifferentialBaseEgoMotionIndices::kAngularAcceleration>(*this); }\n\n  Vector2ConstMap position() const {\n    return Vector2ConstMap(this->pointer + DifferentialBaseEgoMotionIndices::kPositionX);\n  }\n};\n\ntemplate <typename K, typename Base>\nstruct DifferentialBaseEgoMotionMutableInterface :\n    public DifferentialBaseEgoMotionImmutableInterface<K, Base> {\n  using DifferentialBaseEgoMotionImmutableInterface<K, Base>::operator=;\n  using Vector2Map = Eigen::Map<::nvidia::isaac::Vector2<K>, 0,\n                                Eigen::InnerStride<DifferentialBaseEgoMotionIndices::kPositionY\n                                                   - DifferentialBaseEgoMotionIndices::kPositionX>>;\n  K position_x() const { return get<DifferentialBaseEgoMotionIndices::kPositionX>(*this); }\n  K position_y() const { return get<DifferentialBaseEgoMotionIndices::kPositionY>(*this); }\n  K heading() const { return get<DifferentialBaseEgoMotionIndices::kHeading>(*this); }\n  K angular_speed() const { return get<DifferentialBaseEgoMotionIndices::kAngularSpeed>(*this); }\n  K linear_speed() const { return get<DifferentialBaseEgoMotionIndices::kLinearSpeed>(*this); }\n  K angular_acceleration() const {\n      return get<DifferentialBaseEgoMotionIndices::kAngularAcceleration>(*this); }\n  K linear_acceleration() const {\n      return get<DifferentialBaseEgoMotionIndices::kLinearAcceleration>(*this); }\n\n  K& position_x() { return get<DifferentialBaseEgoMotionIndices::kPositionX>(*this); }\n  K& position_y() { return get<DifferentialBaseEgoMotionIndices::kPositionY>(*this); }\n  K& heading() { return get<DifferentialBaseEgoMotionIndices::kHeading>(*this); }\n  K& angular_speed() { return get<DifferentialBaseEgoMotionIndices::kAngularSpeed>(*this); }\n  K& linear_speed() { return get<DifferentialBaseEgoMotionIndices::kLinearSpeed>(*this); }\n  K& angular_acceleration() {\n      return get<DifferentialBaseEgoMotionIndices::kAngularAcceleration>(*this); }\n  K& linear_acceleration() {\n      return get<DifferentialBaseEgoMotionIndices::kLinearAcceleration>(*this); }\n\n  Vector2Map position() { return Vector2Map(&position_x()); }\n};\n\ntemplate <typename K>\nusing DifferentialBaseEgoMotionConstView = CompositeConstView<K, DifferentialBaseEgoMotionIndices,\n                                                      DifferentialBaseEgoMotionImmutableInterface>;\n\ntemplate <typename K>\nusing DifferentialBaseEgoMotionView = CompositeView<K, DifferentialBaseEgoMotionIndices,\n                                            DifferentialBaseEgoMotionMutableInterface>;\n\ntemplate <typename K>\nusing DifferentialBaseEgoMotion = Composite<K, DifferentialBaseEgoMotionIndices,\n        DifferentialBaseEgoMotionMutableInterface>;\n\ntemplate <typename K>\nusing DifferentialBaseEgoMotionEigen =\n  CompositeEigen<K, DifferentialBaseEgoMotionIndices, DifferentialBaseEgoMotionMutableInterface>;\n\n// Describes the command type for a Differential base, ie. output of a controller and intput to the\n// hardware driver\nstruct DifferentialBaseCommandIndices {\n  enum {\n    // Linear (longitudinal) speed (m/s)\n    kLinearSpeed,\n    // Rotation speed (radian/s)\n    kAngularSpeed,\n    kSize\n  };\n};\n\ninline composite::Schema DifferentialBaseCommandCompositeSchema() {\n  return composite::Schema({\n    composite::Quantity::Scalar(\"linear_speed\", composite::Measure::kSpeed),\n    composite::Quantity::Scalar(\"angular_speed\", composite::Measure::kAngularSpeed),\n  });\n}\n\ntemplate <typename K, typename Base>\nstruct DifferentialBaseCommandImmutableInterface : public Base {\n  using Base::operator=;\n\n  K linear_speed() const { return get<DifferentialBaseCommandIndices::kLinearSpeed>(*this); }\n  K angular_speed() const { return get<DifferentialBaseCommandIndices::kAngularSpeed>(*this); }\n};\n\ntemplate <typename K, typename Base>\nstruct DifferentialBaseCommandMutableInterface :\n    public DifferentialBaseCommandImmutableInterface<K, Base> {\n  using DifferentialBaseCommandImmutableInterface<K, Base>::operator=;\n\n  K linear_speed() const { return get<DifferentialBaseCommandIndices::kLinearSpeed>(*this); }\n  K angular_speed() const { return get<DifferentialBaseCommandIndices::kAngularSpeed>(*this); }\n\n  K& linear_speed() { return get<DifferentialBaseCommandIndices::kLinearSpeed>(*this); }\n  K& angular_speed() { return get<DifferentialBaseCommandIndices::kAngularSpeed>(*this); }\n};\n\ntemplate <typename K>\nusing DifferentialBaseCommandConstView =\n   CompositeConstView<K, DifferentialBaseCommandIndices, DifferentialBaseCommandImmutableInterface>;\n\ntemplate <typename K>\nusing DifferentialBaseCommandView =\n    CompositeView<K, DifferentialBaseCommandIndices, DifferentialBaseCommandMutableInterface>;\n\ntemplate <typename K>\nusing DifferentialBaseCommand = Composite<K, DifferentialBaseCommandIndices,\n                                      DifferentialBaseCommandMutableInterface>;\n\n// Describes the kinematic state for an Differential base, for example input to base odometry\nstruct DifferentialBaseStateIndices {\n  enum {\n    // Linear (longitudinal) speed (m/s)\n    kLinearSpeed,\n    // Rotation speed (radian/s)\n    kAngularSpeed,\n    // Linear (longitudinal) acceleration (m/s^2)\n    kLinearAcceleration,\n    // Angular (rotational) acceleration (radians/s^2)\n    kAngularAcceleration,\n    kSize\n  };\n};\n\ninline composite::Schema DifferentialBaseStateCompositeSchema() {\n  return composite::Schema({\n    composite::Quantity::Scalar(\"linear_speed\", composite::Measure::kSpeed),\n    composite::Quantity::Scalar(\"angular_speed\", composite::Measure::kRotation),\n    composite::Quantity::Scalar(\"linear_acceleration\", composite::Measure::kAcceleration),\n    composite::Quantity::Scalar(\"angular_acceleration\", composite::Measure::kRotation)\n  });\n}\n\ntemplate <typename K, typename Base>\nstruct DifferentialBaseStateImmutableInterface : public Base {\n  using Base::operator=;\n\n  K linear_speed() const { return get<DifferentialBaseStateIndices::kLinearSpeed>(*this); }\n  K angular_speed() const { return get<DifferentialBaseStateIndices::kAngularSpeed>(*this); }\n  K linear_acceleration() const {\n    return get<DifferentialBaseStateIndices::kLinearAcceleration>(*this); }\n  K angular_acceleration() const {\n    return get<DifferentialBaseStateIndices::kAngularAcceleration>(*this);\n  }\n};\n\ntemplate <typename K, typename Base>\nstruct DifferentialBaseStateMutableInterface\n    : public DifferentialBaseStateImmutableInterface<K, Base> {\n  using DifferentialBaseStateImmutableInterface<K, Base>::operator=;\n\n  K linear_speed() const { return get<DifferentialBaseStateIndices::kLinearSpeed>(*this); }\n  K angular_speed() const { return get<DifferentialBaseStateIndices::kAngularSpeed>(*this); }\n  K linear_acceleration() const {\n    return get<DifferentialBaseStateIndices::kLinearAcceleration>(*this); }\n  K angular_acceleration() const {\n    return get<DifferentialBaseStateIndices::kAngularAcceleration>(*this); }\n\n  K& linear_speed() { return get<DifferentialBaseStateIndices::kLinearSpeed>(*this); }\n  K& angular_speed() {\n    return get<DifferentialBaseStateIndices::kAngularSpeed>(*this); }\n  K& linear_acceleration() {\n    return get<DifferentialBaseStateIndices::kLinearAcceleration>(*this); }\n  K& angular_acceleration() {\n    return get<DifferentialBaseStateIndices::kAngularAcceleration>(*this);\n  }\n};\n\ntemplate <typename K>\nusing DifferentialBaseStateConstView =\n    CompositeConstView<K, DifferentialBaseStateIndices,\n                       DifferentialBaseStateImmutableInterface>;\n\ntemplate <typename K>\nusing DifferentialBaseStateView =\n    CompositeView<K, DifferentialBaseStateIndices, DifferentialBaseStateMutableInterface>;\n\ntemplate <typename K>\nusing DifferentialBaseState =\n    Composite<K, DifferentialBaseStateIndices, DifferentialBaseStateMutableInterface>;\n\ntemplate <typename K>\nusing DifferentialBaseStateEigen =\n    CompositeEigen<K, DifferentialBaseStateIndices, DifferentialBaseStateMutableInterface>;\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/allocator/allocator_base.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2019-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cstddef>\n\n#include \"gems/core/byte.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Base class for allocators which allocate and deallocate blocks of memory.\nclass AllocatorBase {\n public:\n  // Type for a raw pointer to a block of memory\n  using pointer_t = byte*;\n\n  virtual ~AllocatorBase() = default;\n\n  // Allocates memory for at least N instances of the given type\n  template <typename T>\n  T* allocate(size_t count) {\n    return reinterpret_cast<T*>(allocateBytes(sizeof(T) * count));\n  }\n\n  // Deallocates a block of memory previously allocated with `allocate`. Need to be passed the same\n  // instance count used during the call to `allocate`.\n  template <typename T>\n  void deallocate(T* pointer, size_t count) {\n    deallocateBytes(reinterpret_cast<pointer_t>(pointer), sizeof(T) * count);\n  }\n\n  // Allocates memory for at least the given number of bytes\n  virtual pointer_t allocateBytes(size_t size) = 0;\n\n  // Deallocates a block of memory previously allocated with `allocate`. Need to be passed the same\n  // byte count used during the call to `allocate`.\n  virtual void deallocateBytes(pointer_t pointer, size_t size) = 0;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/allocator/allocators.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2019-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cstddef>\n\n#include \"gems/core/allocator/allocator_base.hpp\"\n#include \"gems/core/byte.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Provides a globally available allocator to allocate CPU memory.\nstruct CpuAllocator {\n  static byte* Allocate(size_t size);\n  static void Deallocate(byte* pointer, size_t size);\n};\n\n// Gets the currently used allocator for CPU memory allocation.\nAllocatorBase* GetCpuAllocator();\n\n// Provides a globally available allocator to allocate CUDA memory.\nstruct CudaAllocator {\n  static byte* Allocate(size_t size);\n  static void Deallocate(byte* pointer, size_t size);\n};\n\n// Gets the currently used allocator for CUDA memory allocation.\nAllocatorBase* GetCudaAllocator();\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/assert.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cstdlib>\n#include <string>\n\n#include \"gems/core/logger.hpp\"\n\n// Prints a panic message and aborts the program\n#define PANIC(...)                                                                  \\\n  {                                                                                 \\\n    ::nvidia::isaac::logger::Log(                                                   \\\n        __FILE__, __LINE__, ::nvidia::isaac::logger::Severity::PANIC, __VA_ARGS__); \\\n    std::abort();                                                                   \\\n  }\n\n// Checks if an expression evaluates to true. If not prints a panic message and aborts the program.\n#define ASSERT(expr, ...)                                                           \\\n  if (!(expr)) {                                                                    \\\n    ::nvidia::isaac::logger::Log(                                                   \\\n        __FILE__, __LINE__, ::nvidia::isaac::logger::Severity::PANIC, __VA_ARGS__); \\\n    std::abort();                                                                   \\\n  }\n\n// Asserts that A == B. If not prints a panic message and aborts the program.\n#define ISAAC_ASSERT_EQ(exp_a, exp_b)                                        \\\n  {                                                                          \\\n    const auto _va = exp_a;                                                  \\\n    const auto _vb = exp_b;                                                  \\\n    ASSERT(                                                                  \\\n        _va == _vb, \"Assert failed: %s == %s.\", std::to_string(_va).c_str(), \\\n        std::to_string(_vb).c_str());                                        \\\n  }\n\n// Asserts that A != B. If not prints a panic message and aborts the program.\n#define ISAAC_ASSERT_NE(exp_a, exp_b)                                        \\\n  {                                                                          \\\n    const auto _va = exp_a;                                                  \\\n    const auto _vb = exp_b;                                                  \\\n    ASSERT(                                                                  \\\n        _va != _vb, \"Assert failed: %s != %s.\", std::to_string(_va).c_str(), \\\n        std::to_string(_vb).c_str());                                        \\\n  }\n\n// Asserts that A > B. If not prints a panic message and aborts the program.\n#define ISAAC_ASSERT_GT(exp_a, exp_b)                                      \\\n  {                                                                        \\\n    const auto _va = exp_a;                                                \\\n    const auto _vb = exp_b;                                                \\\n    ASSERT(                                                                \\\n        _va > _vb, \"Assert failed: %s > %s.\", std::to_string(_va).c_str(), \\\n        std::to_string(_vb).c_str());                                      \\\n  }\n\n// Asserts that A >= B. If not prints a panic message and aborts the program.\n#define ISAAC_ASSERT_GE(exp_a, exp_b)                                        \\\n  {                                                                          \\\n    const auto _va = exp_a;                                                  \\\n    const auto _vb = exp_b;                                                  \\\n    ASSERT(                                                                  \\\n        _va >= _vb, \"Assert failed: %s >= %s.\", std::to_string(_va).c_str(), \\\n        std::to_string(_vb).c_str());                                        \\\n  }\n\n// Asserts that A > B. If not prints a panic message and aborts the program.\n#define ISAAC_ASSERT_LT(exp_a, exp_b)                                      \\\n  {                                                                        \\\n    const auto _va = exp_a;                                                \\\n    const auto _vb = exp_b;                                                \\\n    ASSERT(                                                                \\\n        _va < _vb, \"Assert failed: %s > %s.\", std::to_string(_va).c_str(), \\\n        std::to_string(_vb).c_str());                                      \\\n  }\n\n// Asserts that A <= B. If not prints a panic message and aborts the program.\n#define ISAAC_ASSERT_LE(exp_a, exp_b)                                        \\\n  {                                                                          \\\n    const auto _va = exp_a;                                                  \\\n    const auto _vb = exp_b;                                                  \\\n    ASSERT(                                                                  \\\n        _va <= _vb, \"Assert failed: %s <= %s.\", std::to_string(_va).c_str(), \\\n        std::to_string(_vb).c_str());                                        \\\n  }\n\n// Asserts that abs(A - B) < threshold. If not prints a panic message and aborts the program.\n#define ISAAC_ASSERT_NEAR(exp_a, exp_b, exp_threshold)                        \\\n  {                                                                           \\\n    const auto _vdiff = exp_a - exp_b;                                        \\\n    const auto _vthreshold = exp_threshold;                                   \\\n    ASSERT(                                                                   \\\n        std::abs(_vdiff) <= _vthreshold,                                      \\\n        \"Assert failed: difference between %s and %s as |%s| <= %s.\",         \\\n        std::to_string(exp_a).c_str(), std::to_string(exp_b).c_str(),         \\\n        std::to_string(_vdiff).c_str(), std::to_string(_vthreshold).c_str()); \\\n  }\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/buffers/algorithm.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <type_traits>\n\n#include \"gems/core/assert.hpp\"\n#include \"gems/core/buffers/traits.hpp\"\n#include \"gems/core/byte.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Copies raw memory from one buffer to another buffer\nvoid CopyArrayRaw(\n    const void* source, BufferStorageMode source_storage, void* target,\n    BufferStorageMode target_storage, size_t size);\n\n// Copies data between two buffers layout as matrices\n// For both source and target buffer the pointer and the row stride between rows is given.\n// `row_size` indicates the number of bytes which are actually filled with data. Bytes which are\n// not filled with used data might not be copied.\nvoid CopyMatrixRaw(\n    const void* source, size_t source_stride, BufferStorageMode source_storage, void* target,\n    size_t target_stride, BufferStorageMode target_storage, size_t rows, size_t row_size);\n\n// Copies memory from one buffer to another buffer\ntemplate <typename Source, typename Target>\nvoid CopyArray(const Source& source, Target& target) {\n  // Asserts that the buffers have the same size\n  ASSERT(source.size() == target.size(), \"size mismatch: %zu vs %zu\", source.size(), target.size());\n  // Copy the bytes\n  auto source_view = View(source).template reinterpret<const byte>();\n  auto target_view = View(target).template reinterpret<byte>();\n  CopyArrayRaw(\n      reinterpret_cast<const void*>(source_view.begin()), BufferTraits<Source>::kStorageMode,\n      reinterpret_cast<void*>(target_view.begin()), BufferTraits<Target>::kStorageMode,\n      source_view.size());\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/buffers/buffer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <memory>\n#include <type_traits>\n#include <utility>\n\n#include \"gems/core/allocator/allocators.hpp\"\n#include \"gems/core/buffers/traits.hpp\"\n#include \"gems/core/byte.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// -------------------------------------------------------------------------------------------------\n\nnamespace detail {\n\n// A simple pointer which can be tagged. This is used for example to differentiate between a pointer\n// to host memory and a pointer to device memory.\ntemplate <typename K, typename Tag = byte>\nclass TaggedPointer {\n public:\n  using value_t = std::remove_cv_t<K>;\n  using const_pointer_t = TaggedPointer<const value_t, Tag>;\n  using pointer_t = TaggedPointer<value_t, Tag>;\n  using tag_t = Tag;\n  // Standard constructor\n  TaggedPointer(K* data = nullptr) : data_(data) {}\n\n  // Default copy\n  TaggedPointer(const TaggedPointer& other) = default;\n  TaggedPointer& operator=(const TaggedPointer& other) = default;\n\n  // Move will set source to nullptr\n  TaggedPointer(TaggedPointer&& other) { *this = std::move(other); }\n  TaggedPointer& operator=(TaggedPointer&& other) {\n    data_ = other.data_;\n    other.data_ = nullptr;\n    return *this;\n  }\n\n  // Sets the actual pointer\n  TaggedPointer& operator=(K* other) {\n    data_ = other;\n    return *this;\n  }\n  // Gets the actual pointer\n  K* get() const { return data_; }\n  operator K*() const { return data_; }\n\n private:\n  K* data_;\n};\n\n}  // namespace detail\n\n// -------------------------------------------------------------------------------------------------\n\nnamespace detail {\n\n// Base class for Buffer and BufferView which provides storage for the memory pointer\n// and corresponding dimensions.\ntemplate <typename Pointer>\nclass BufferBase {\n public:\n  using pointer_t = Pointer;\n\n  BufferBase() : pointer_(nullptr), size_(0) {}\n\n  BufferBase(Pointer pointer, size_t size) : pointer_(std::move(pointer)), size_(size) {}\n\n  // pointer to the first row\n  const Pointer& pointer() const { return pointer_; }\n  // The total size of the buffer in bytes\n  size_t size() const { return size_; }\n  // A pointer to the first byte of the buffer.\n  auto begin() const { return pointer_.get(); }\n  // A pointer behind the last byte of the buffer.\n  auto end() const { return begin() + size(); }\n\n protected:\n  Pointer pointer_;\n  size_t size_;\n};\n\n}  // namespace detail\n\n// -------------------------------------------------------------------------------------------------\n\n// A buffer which owns its memory\ntemplate <typename Pointer, typename Allocator>\nclass Buffer : public detail::BufferBase<Pointer> {\n public:\n  using mutable_view_t = detail::BufferBase<typename Pointer::pointer_t>;\n  using const_view_t = detail::BufferBase<typename Pointer::const_pointer_t>;\n\n  Buffer() : handle_(nullptr, Deleter{0}) {}\n\n  // Allocates memory for `size` bytes.\n  Buffer(size_t size)\n      : detail::BufferBase<Pointer>(nullptr, size),\n        handle_(Allocator::Allocate(size), Deleter{size}) {\n    // 1) Initialize the base class with a nullptr, the number of rows, and the desired stride.\n    // 2) Allocate memory and store it in the unique pointer used as handle. This will also change\n    //    the stride stored in the base class to the actual stride chosen by the allocator.\n    // 3) Get a pointer to the allocated memory and store it in the base class so that calls to\n    //    pointer() actually work.\n    this->pointer_ = handle_.get();\n  }\n\n  Buffer(Buffer&& buffer)\n      : detail::BufferBase<Pointer>(nullptr, buffer.size_), handle_(std::move(buffer.handle_)) {\n    this->pointer_ = handle_.get();\n    buffer.pointer_ = nullptr;\n    buffer.size_ = 0;\n  }\n\n  Buffer& operator=(Buffer&& buffer) {\n    this->handle_ = std::move(buffer.handle_);\n    this->pointer_ = this->handle_.get();\n    this->size_ = buffer.size_;\n    buffer.pointer_ = nullptr;\n    buffer.size_ = 0;\n    return *this;\n  }\n\n  void resize(size_t desired_size) {\n    if (desired_size == this->size()) return;\n    *this = Buffer<Pointer, Allocator>(desired_size);\n  }\n\n  // Disowns the pointer from the buffer. The user is now responsible for deallocation.\n  // WARNING: This is dangerous as the wrong allocator might be called.\n  byte* release() {\n    byte* pointer = handle_.release();\n    this->pointer_ = nullptr;\n    return pointer;\n  }\n\n  // Creates a view which provides read and write access from this buffer object.\n  mutable_view_t view() { return mutable_view_t(this->pointer_, this->size_, this->stride_); }\n  const_view_t view() const {\n    return const_view_t(typename Pointer::const_pointer_t(this->pointer_.get()), this->size_);\n  }\n  // Creates a view which only provides read access from this buffer object.\n  const_view_t const_view() const {\n    return const_view_t(typename Pointer::const_pointer_t(this->pointer_.get()), this->size_);\n  }\n\n private:\n  // A deleter object for the unique pointer to free allocated memory\n  struct Deleter {\n    size_t size;\n    void operator()(byte* pointer) {\n      if (size == 0) return;\n      Allocator::Deallocate(pointer, size);\n    }\n  };\n\n  // A unique pointer is used to handle the allocator and to make this object non-copyable.\n  using handle_t = std::unique_ptr<byte, Deleter>;\n\n  // Memory handle used to automatically deallocate memory and to disallow copy semantics.\n  handle_t handle_;\n};\n\n// -------------------------------------------------------------------------------------------------\n\n// A pointer to host memory\ntemplate <typename K>\nusing HostPointer =\n    detail::TaggedPointer<K, std::integral_constant<BufferStorageMode, BufferStorageMode::Host>>;\n\n// A host buffer with stride which owns its memory\nusing CpuBuffer = Buffer<HostPointer<byte>, CpuAllocator>;\n\n// A host buffer with stride which does not own its memory\nusing CpuBufferView = detail::BufferBase<HostPointer<byte>>;\n\n// A host buffer with stride which does not own its memory and provides only read access\nusing CpuBufferConstView = detail::BufferBase<HostPointer<const byte>>;\n\ntemplate <>\nstruct BufferTraits<Buffer<HostPointer<byte>, CpuAllocator>> {\n  static constexpr BufferStorageMode kStorageMode = BufferStorageMode::Host;\n  static constexpr bool kIsMutable = true;\n  static constexpr bool kIsOwning = true;\n\n  using buffer_view_t = CpuBufferView;\n  using buffer_const_view_t = CpuBufferConstView;\n};\n\ntemplate <typename K>\nstruct BufferTraits<detail::BufferBase<HostPointer<K>>> {\n  static constexpr BufferStorageMode kStorageMode = BufferStorageMode::Host;\n  static constexpr bool kIsMutable = !std::is_const<K>::value;\n  static constexpr bool kIsOwning = false;\n\n  using buffer_view_t = CpuBufferView;\n  using buffer_const_view_t = CpuBufferConstView;\n};\n\n// -------------------------------------------------------------------------------------------------\n\n// A pointer to CUDA memory\ntemplate <typename K>\nusing CudaPointer =\n    detail::TaggedPointer<K, std::integral_constant<BufferStorageMode, BufferStorageMode::Cuda>>;\n\n// A CUDA buffer with stride which owns its memory\nusing CudaBuffer = Buffer<CudaPointer<byte>, CudaAllocator>;\n\n// A CUDA buffer with stride which does not own its memory\nusing CudaBufferView = detail::BufferBase<CudaPointer<byte>>;\n\n// A CUDA buffer with stride which does not own its memory and provides only read access\nusing CudaBufferConstView = detail::BufferBase<CudaPointer<const byte>>;\n\ntemplate <>\nstruct BufferTraits<Buffer<CudaPointer<byte>, CudaAllocator>> {\n  static constexpr BufferStorageMode kStorageMode = BufferStorageMode::Cuda;\n  static constexpr bool kIsMutable = true;\n  static constexpr bool kIsOwning = true;\n\n  using buffer_view_t = CudaBufferView;\n  using buffer_const_view_t = CudaBufferConstView;\n};\n\ntemplate <typename K>\nstruct BufferTraits<detail::BufferBase<CudaPointer<K>>> {\n  static constexpr BufferStorageMode kStorageMode = BufferStorageMode::Cuda;\n  static constexpr bool kIsMutable = !std::is_const<K>::value;\n  static constexpr bool kIsOwning = false;\n\n  using buffer_view_t = CudaBufferView;\n  using buffer_const_view_t = CudaBufferConstView;\n};\n\n// -------------------------------------------------------------------------------------------------\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/buffers/traits.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <type_traits>\n\nnamespace nvidia {\nnamespace isaac {\n\n// Specifies where data is stored, e.g. host or CUDA device\nenum class BufferStorageMode {\n  // host memory\n  Host,\n  // CUDA memory\n  Cuda\n};\n\n// Specifies various properties for buffer types.\n// Example:\n//    template <typename T>\n//    struct BufferTraits<CudaArray<T>> {\n//      static constexpr BufferStorageMode kStorageMode = BufferStorageMode::Cuda;\n//      static constexpr bool kIsMutable = true;\n//      static constexpr bool kIsOwning = true;\n//\n//      static CudaArray<T> Create(size_t size) {\n//        return CudaArray<T>(size);\n//      }\n//    };\ntemplate <typename Container>\nstruct BufferTraits;\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/byte.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cstdint>\n\nnamespace nvidia {\nnamespace isaac {\n\n// Will be oficially defined in C++17\nusing byte = uint8_t;\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/constants.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <type_traits>\n\nnamespace nvidia {\nnamespace isaac {\n\n// The value of pi with 75 digits available for every type.\ntemplate <typename K, typename std::enable_if<std::is_floating_point<K>::value, int>::type = 0>\nconstexpr K Pi = 3.141592653589793238462643383279502884197169399375105820974944592307816406286;\n// The value of 2 * pi (a.k.a tau) with 75 digits available for every type.\ntemplate <typename K, typename std::enable_if<std::is_floating_point<K>::value, int>::type = 0>\nconstexpr K TwoPi = 6.283185307179586476925286766559005768394338798750211641949889184615632812572;\n\n// Converts from degree to radians\ntemplate <typename K, typename std::enable_if<std::is_floating_point<K>::value, int>::type = 0>\nconstexpr K DegToRad(K x) {\n  return x * K(0.017453292519943295769236907684886127134428718885417254560);\n}\n\n// Converts from radians to degrees\ntemplate <typename K, typename std::enable_if<std::is_floating_point<K>::value, int>::type = 0>\nconstexpr K RadToDeg(K x) {\n  return x * K(57.29577951308232087679815481410517033240547246656432154916);\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/epsilon.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cmath>\n#include <cstdint>\n\n// This file provides functions to help comparing floating point numbers for equality.\n// You should have a look at this excellent article for more details:\n// https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/\n\nnamespace nvidia {\nnamespace isaac {\n\n// A very small floating point number close to what can be represented by the corresponding\n// floating point standard.\n// For more details see https://en.wikipedia.org/wiki/Machine_epsilon\ntemplate <typename K>\nconstexpr K MachineEpsilon = K(0);\ntemplate <>\nconstexpr float MachineEpsilon<float> = 5.9604644775390625e-8f;\ntemplate <>\nconstexpr double MachineEpsilon<double> = 1.1102230246251565404236316680908203125e-16;\n\n// Returns true if a floating point value can be considered to be zero under floating point\n// rounding errors. This function compares against a custom epsilon.\ntemplate <typename K>\nbool IsAlmostZero(K x, K epsilon = MachineEpsilon<K>) {\n  return std::abs(x) <= epsilon;\n}\n// Returns true if two floating point values are so close that they can be considered to be equal\n// under floating point rounding errors. This function uses relative comparison technique.\n// Warning: Do not use to compare against small floats, or zero, use IsAlmostZero instead.\ntemplate <typename K>\nbool IsAlmostEqualRelative(K x, K y, K max_rel_diff = K(10) * MachineEpsilon<K>) {\n  const K diff = std::abs(x - y);\n  const K absx = std::abs(x);\n  const K absy = std::abs(y);\n  const K larger = (absx > absy) ? absx : absy;\n  return (diff <= larger * max_rel_diff);\n}\n\n// Returns true if two floating point values are equal up to a certain tolerance.\ntemplate <typename K>\nbool IsAlmostEqualAbsolute(K x, K y, K tolerance) {\n  return std::abs(x - y) <= tolerance;\n}\n\n// Returns true if a floating point value can be considered to be one under floating point\n// rounding errors. This function compares against machine epsilon.\ntemplate <typename K>\nbool IsAlmostOne(K x) {\n  return std::abs(x - K(1)) <= K(10) * MachineEpsilon<K>;\n}\n\nnamespace epsilon_details {\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wstrict-aliasing\"\n\ninline int32_t Float32AsInteger(float x) {\n  return *reinterpret_cast<int32_t*>(&x);\n}\n\ninline int64_t Float64AsInteger(double x) {\n  return *reinterpret_cast<int64_t*>(&x);\n}\n\n#pragma GCC diagnostic pop\n}  // namespace epsilon_details\n\n// Checks if two 32-bit floats are equal by computing their ULPs difference.\ninline bool IsAlmostEqualUlps(float x, float y, int32_t max_ulps_diff = 2) {\n  const int32_t ix = epsilon_details::Float32AsInteger(x);\n  const int32_t iy = epsilon_details::Float32AsInteger(y);\n  if ((ix < 0) != (iy < 0)) {\n    // In case the sign is different we still need to check if the floats were equal to\n    // make sure -0 is equal to +0.\n    return (x == y);\n  } else {\n    return std::abs(ix - iy) < max_ulps_diff;\n  }\n}\n\n// Checks if two 64-bit floats are equal by computing their ULPs difference.\ninline bool IsAlmostEqualUlps(double x, double y, int64_t max_ulps_diff = 2) {\n  const int64_t ix = epsilon_details::Float64AsInteger(x);\n  const int64_t iy = epsilon_details::Float64AsInteger(y);\n  if ((ix < 0) != (iy < 0)) {\n    // In case the sign is different we still need to check if the floats were equal to\n    // make sure -0 is equal to +0.\n    return (x == y);\n  } else {\n    return std::abs(ix - iy) < max_ulps_diff;\n  }\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/image/image.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <algorithm>\n#include <utility>\n\n#include \"gems/core/assert.hpp\"\n#include \"gems/core/buffers/buffer.hpp\"\n#include \"gems/core/byte.hpp\"\n#include \"gems/core/math/types.hpp\"\n#include \"gems/core/tensor/tensor.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// -------------------------------------------------------------------------------------------------\n\n// A pixel which owns its data and can life independently from the image itself.\ntemplate <typename K, int N>\nusing Pixel = std::conditional_t<N == 1, K, Vector<K, N>>;\n\n// A pixel type used for read and write access to an image pixel.\ntemplate <typename K, int N>\nusing PixelRef = std::conditional_t<N == 1, K&, Eigen::Map<Vector<K, N>>>;\n\n// A pixel type used for read-only access to an image pixel.\ntemplate <typename K, int N>\nusing PixelConstRef = std::conditional_t<N == 1, const K&, Eigen::Map<const Vector<K, N>>>;\n\nnamespace detail {\n\n// Helper type to create pixels and pixel references based on a pointer for N > 1\ntemplate <typename K, int N>\nstruct PixelCreator {\n  // Creates a non-owning pixel based on a pointer\n  static PixelRef<K, N> CreatePixelRef(K* ptr) { return PixelRef<K, N>(ptr); }\n  // Creates a non-owning, non-mutable pixel based on a pointer\n  static PixelConstRef<K, N> CreatePixelConstRef(const K* ptr) { return PixelConstRef<K, N>(ptr); }\n};\n\n// Specialized version of PixelCreator for N = 1\ntemplate <typename K>\nstruct PixelCreator<K, 1> {\n  static PixelRef<K, 1> CreatePixelRef(K* ptr) { return *ptr; }\n  static PixelConstRef<K, 1> CreatePixelConstRef(const K* ptr) { return *ptr; }\n};\n\n// Helper type to find the correct tensor type based on the number of channels.\ntemplate <typename K, int N, typename BufferType>\nstruct ImageTensorBase {\n  using type = TensorBase<K, tensor::Dimensions<tensor::kDynamic, tensor::kDynamic, N>, BufferType>;\n};\ntemplate <typename K, typename BufferType>\nstruct ImageTensorBase<K, 1, BufferType> {\n  using type = TensorBase<K, tensor::Rank<2>, BufferType>;\n};\n\n}  // namespace detail\n\n// -------------------------------------------------------------------------------------------------\n\n// A two-dimensional array of pixels\n//\n// Each pixel contains `N` elements and all elements are of the same type `K`. Elements for all\n// pixels are stored continuously. Access to pixels returns mapped memory to provide read access\n// without unnecessary copies and direct write access. Pixels are stored interleaved and in\n// row-major storage order.\n//\n// The image type uses a tensor to store data. If `N` is equal to 1 a rank 2 tensor is used,\n// otherwise a rank 3 tensor is used. The tensor uses the given `BufferType` and more information\n// is provided in the Tensor type.\n//\ntemplate <typename K, int N, typename BufferType>\nclass ImageBase {\n public:\n  // The number of channels for an image must be greater than 0.\n  static_assert(N > 0, \"Number of channels must be positive\");\n  // The number of channels for each pixels. This is also the last dimension of the underlying\n  // tensor.\n  static constexpr int kChannels = N;\n\n  // Tensor type used to store the array of elements for this image.\n  using tensor_t = typename detail::ImageTensorBase<K, kChannels, BufferType>::type;\n\n  using buffer_t = BufferType;\n  using buffer_view_t = typename tensor_t::buffer_view_t;\n  using buffer_const_view_t = typename tensor_t::buffer_const_view_t;\n  static constexpr bool kIsMutable = tensor_t::kIsMutable;\n  static constexpr bool kIsOwning = tensor_t::kIsOwning;\n\n  using index_t = typename tensor_t::index_t;\n  using coordinate_t = Vector2<index_t>;  // Note: Only rows and col are used for coordinates\n  using dimensions_t = Vector2<index_t>;  // Note: Only rows and col are used for dimensions\n\n  using element_t = typename tensor_t::element_t;\n  using element_ptr_t = typename tensor_t::element_ptr_t;\n  using element_const_ptr_t = typename tensor_t::element_const_ptr_t;\n\n  using raw_ptr_t = typename tensor_t::raw_ptr_t;\n  using raw_const_ptr_t = typename tensor_t::raw_const_ptr_t;\n\n  // Type used for a pixel owning its memory and thus able to life outside of this image\n  using pixel_t = Pixel<K, kChannels>;\n  // Type used when accessing pixels which provides read and write access to pixel data.\n  using pixel_ref_t = PixelRef<K, kChannels>;\n  // Type used when accessing pixels which provides only read access to pixel data.\n  using pixel_const_ref_t = PixelConstRef<K, kChannels>;\n\n  // Type for a mutable view on this image\n  using image_view_t = ImageBase<K, kChannels, buffer_view_t>;\n  // Type for a non-mutable view on this image\n  using image_const_view_t = ImageBase<K, kChannels, buffer_const_view_t>;\n\n  // Create an empty image object\n  ImageBase() = default;\n\n  // Create an image object of given dimensions. This will create a new storage container and is\n  // only available for storage container types which own their memory. This function is templated\n  // on the integer type so that we can distable this constructor for non-owning storage types.\n  ImageBase(const dimensions_t& dimensions) : tensor_(dimensions[0], dimensions[1]) {}\n  ImageBase(index_t rows, index_t cols) : tensor_(rows, cols) {}\n\n  // Create an image object of given dimensions and with given storage container\n  ImageBase(buffer_t buffer, const dimensions_t& dimensions)\n      : tensor_(std::move(buffer), dimensions[0], dimensions[1]) {}\n  ImageBase(buffer_t buffer, index_t rows, index_t cols) : tensor_(std::move(buffer), rows, cols) {}\n\n  // Creates an image from a tensor.\n  ImageBase(tensor_t tensor) : tensor_(std::move(tensor)) {}\n\n  // Copy construction uses the default behavior\n  ImageBase(const ImageBase& other) = default;\n  // Copy assignment uses the default behavior\n  ImageBase& operator=(const ImageBase& other) = default;\n  // Move construction uses the default behavior\n  ImageBase(ImageBase&& other) = default;\n  // Move assignment uses the default behavior\n  ImageBase& operator=(ImageBase&& other) = default;\n\n  // Creates a mutable view on this tensor\n  template <bool X = kIsMutable>\n  std::enable_if_t<X, image_view_t> view() {\n    return image_view_t({this->data().begin(), this->data().size()}, this->dimensions());\n  }\n  // Creates a non-mutable view on this tensor\n  image_const_view_t const_view() const {\n    return image_const_view_t({this->data().begin(), this->data().size()}, this->dimensions());\n  }\n  image_const_view_t view() const { return const_view(); }\n\n  // Provides conversion to a mutable view for owning images and mutable views.\n  template <bool X = kIsMutable>\n  operator std::enable_if_t<X, image_view_t>() {\n    return view();\n  }\n  // Provides conversion to a non-mutable view\n  operator image_const_view_t() const { return const_view(); }\n\n  // Returns true if this image has dimensions 0\n  bool empty() const { return rows() == 0 || cols() == 0; }\n  // (rows, cols) as a 2-vector\n  dimensions_t dimensions() const { return dimensions_t{rows(), cols()}; }\n  // Returns the number of rows\n  index_t rows() const { return tensor().dimensions()[0]; }\n  // Returns the number of cols\n  index_t cols() const { return tensor().dimensions()[1]; }\n  // The number of channels\n  constexpr int channels() const { return kChannels; }\n  // The total number of pixels in the image\n  index_t num_pixels() const { return rows() * cols(); }\n\n  // Resizes the image\n  template <bool X = kIsOwning>\n  std::enable_if_t<X, void> resize(index_t desired_rows, index_t desired_cols) {\n    tensor_.resize(desired_rows, desired_cols);\n  }\n  template <bool X = kIsOwning>\n  std::enable_if_t<X, void> resize(const dimensions_t& dimensions) {\n    tensor_.resize(dimensions[0], dimensions[1]);\n  }\n\n  // Returns true if the given pixel coordinate references a valid pixel.\n  bool isValidCoordinate(index_t row, index_t col) const {\n    return 0 <= row && row < rows() && 0 <= col && col < cols();\n  }\n\n  // Returns a non mutable reference pixel holder on the position (row, col).\n  pixel_const_ref_t operator()(index_t row, index_t col) const {\n    return detail::PixelCreator<K, kChannels>::CreatePixelConstRef(\n        row_pointer(row) + col * kChannels);\n  }\n  pixel_const_ref_t operator()(const coordinate_t& coordinate) const {\n    return this->operator()(coordinate[0], coordinate[1]);\n  }\n  // Returns a mutable reference pixel holder on the position (row, col).\n  template <bool X = kIsMutable>\n  std::enable_if_t<X, pixel_ref_t> operator()(index_t row, index_t col) {\n    return detail::PixelCreator<K, kChannels>::CreatePixelRef(row_pointer(row) + col * kChannels);\n  }\n  template <bool X = kIsMutable>\n  std::enable_if_t<X, pixel_ref_t> operator()(const coordinate_t& coordinate) {\n    return this->operator()(coordinate[0], coordinate[1]);\n  }\n\n  // Returns a non mutable reference pixel holder at the position `index`.\n  pixel_const_ref_t operator[](index_t index) const {\n    return detail::PixelCreator<K, kChannels>::CreatePixelConstRef(\n        element_wise_begin() + index * kChannels);\n  }\n  // Returns a mutable reference pixel holder at the position `index`.\n  template <bool X = kIsMutable>\n  std::enable_if_t<X, pixel_ref_t> operator[](index_t index) {\n    return detail::PixelCreator<K, kChannels>::CreatePixelRef(\n        element_wise_begin() + index * kChannels);\n  }\n\n  // Pointers to the first element of the first pixel\n  element_const_ptr_t element_wise_begin() const { return tensor().element_wise_begin(); }\n  element_ptr_t element_wise_begin() { return tensor().element_wise_begin(); }\n  // Pointers behind the last element of the last pixel\n  element_const_ptr_t element_wise_end() const { return tensor().element_wise_end(); }\n  element_ptr_t element_wise_end() { return tensor().element_wise_end(); }\n  // The total number of elements in the image\n  index_t num_elements() const { return num_pixels() * channels(); }\n\n  // Pointer to the first element in the i-th row in an image\n  element_ptr_t row_pointer(index_t row) { return element_wise_begin() + row * cols() * kChannels; }\n  element_const_ptr_t row_pointer(index_t row) const {\n    return element_wise_begin() + row * cols() * kChannels;\n  }\n\n  // Number of bytes used to store one row of the image in memory.\n  size_t getStride() const { return ByteCount(1, cols()); }\n  // Number of bytes which are used for elements of an image of the given number of rows and columns\n  static size_t ByteCount(index_t rows, index_t cols) {\n    ASSERT(rows >= 0, \"Number of rows must not be negative: %d\", rows);\n    ASSERT(cols >= 0, \"Number of cols must not be negative: %d\", cols);\n    // Note: Number of channels are guaranteed to be positive.\n    return static_cast<size_t>(rows * cols * kChannels) * sizeof(K);\n  }\n\n  // The underlying tensor object\n  const tensor_t& tensor() const { return tensor_; }\n  template <bool X = kIsMutable>\n  std::enable_if_t<X, tensor_t&> tensor() {\n    return tensor_;\n  }\n\n  // The underlying buffer object\n  const buffer_t& data() const { return tensor().data(); }\n  template <bool X = kIsMutable>\n  std::enable_if_t<X, buffer_t&> data() {\n    return tensor().data();\n  }\n\n private:\n  tensor_t tensor_;\n};\n\n// -------------------------------------------------------------------------------------------------\n\n// An image which owns it's memory\ntemplate <typename K, int N>\nusing Image = ImageBase<K, N, CpuBuffer>;\n\n// A mutable view on an image which does not own memory but can be used to read and write the\n// data of the underlying image.\ntemplate <typename K, int N>\nusing ImageView = ImageBase<K, N, CpuBufferView>;\n\n// A non-mutable view on an image which does not own the memory and can only be used to read\n// the data of the underlying image.\ntemplate <typename K, int N>\nusing ImageConstView = ImageBase<K, N, CpuBufferConstView>;\n\n#define ISAAC_DECLARE_CPU_IMAGE_TYPES_IMPL(N, T, S)  \\\n  using Image##N##S = Image<T, N>;                   \\\n  using ImageView##N##S = ImageView<T, N>;           \\\n  using ImageConstView##N##S = ImageConstView<T, N>; \\\n  using Pixel##N##S = Pixel<T, N>;                   \\\n  using PixelRef##N##S = PixelRef<T, N>;             \\\n  using PixelConstRef##N##S = PixelConstRef<T, N>;\n\n#define ISAAC_DECLARE_CPU_IMAGE_TYPES(N)                \\\n  template <typename K>                                 \\\n  using Image##N = Image<K, N>;                         \\\n  template <typename K>                                 \\\n  using ImageView##N = ImageView<K, N>;                 \\\n  template <typename K>                                 \\\n  using ImageConstView##N = ImageConstView<K, N>;       \\\n  ISAAC_DECLARE_CPU_IMAGE_TYPES_IMPL(N, uint8_t, ub)    \\\n  ISAAC_DECLARE_CPU_IMAGE_TYPES_IMPL(N, uint16_t, ui16) \\\n  ISAAC_DECLARE_CPU_IMAGE_TYPES_IMPL(N, int, i)         \\\n  ISAAC_DECLARE_CPU_IMAGE_TYPES_IMPL(N, double, d)      \\\n  ISAAC_DECLARE_CPU_IMAGE_TYPES_IMPL(N, float, f)\n\nISAAC_DECLARE_CPU_IMAGE_TYPES(1)\nISAAC_DECLARE_CPU_IMAGE_TYPES(2)\nISAAC_DECLARE_CPU_IMAGE_TYPES(3)\nISAAC_DECLARE_CPU_IMAGE_TYPES(4)\n\n#undef ISAAC_DECLARE_CPU_IMAGE_TYPES\n#undef ISAAC_DECLARE_CPU_IMAGE_TYPES_IMPL\n\n// -------------------------------------------------------------------------------------------------\n// Helper function to create an image view from a pointer using dense storage\ntemplate <typename K, int N>\nImageView<K, N> CreateImageView(\n    K* data, typename ImageView<K, N>::index_t rows, typename ImageView<K, N>::index_t cols) {\n  ASSERT(rows >= 0, \"Number of rows must not be negative - was %d\", rows);\n  ASSERT(cols >= 0, \"Number of columns must not be negative - was %d\", cols);\n  const size_t size = ImageView<K, N>::ByteCount(rows, cols);\n  return ImageView<K, N>(CpuBufferView(reinterpret_cast<byte*>(data), size), rows, cols);\n}\n\n// Helper function to create an image const view from a pointer using dense storage\ntemplate <typename K, int N>\nImageConstView<K, N> CreateImageConstView(\n    const K* data, typename ImageConstView<K, N>::index_t rows,\n    typename ImageConstView<K, N>::index_t cols) {\n  ASSERT(rows >= 0, \"Number of rows must not be negative - was %d\", rows);\n  ASSERT(cols >= 0, \"Number of columns must not be negative - was %d\", cols);\n  const size_t size = ImageConstView<K, N>::ByteCount(rows, cols);\n  return ImageConstView<K, N>(\n      CpuBufferConstView(reinterpret_cast<const byte*>(data), size), rows, cols);\n}\n\n// -------------------------------------------------------------------------------------------------\n\n// An image stored in device memory which owns it's memory\ntemplate <typename K, int N>\nusing CudaImage = ImageBase<K, N, CudaBuffer>;\n\n// A mutable view on an image which is stored on GPU device memory, does not own memory, but can be\n// used to read and write the data of the underlying image.\ntemplate <typename K, int N>\nusing CudaImageView = ImageBase<K, N, CudaBufferView>;\n\n// A non-mutable view on an image which is stored on GPU device memory, does not own its memory, and\n// can only be used to read the data of the underlying image.\ntemplate <typename K, int N>\nusing CudaImageConstView = ImageBase<K, N, CudaBufferConstView>;\n\n// Helper macro for ISAAC_DECLARE_CUDA_IMAGE_TYPES\n#define ISAAC_DECLARE_CUDA_IMAGE_TYPES_IMPL(N, K, S) \\\n  using CudaImage##N##S = CudaImage<K, N>;           \\\n  using CudaImageView##N##S = CudaImageView<K, N>;   \\\n  using CudaImageConstView##N##S = CudaImageConstView<K, N>;\n\n// Helper macro to define various CudaImage types\n#define ISAAC_DECLARE_CUDA_IMAGE_TYPES(N)                 \\\n  template <typename K>                                   \\\n  using CudaImage##N = CudaImage<K, N>;                   \\\n  template <typename K>                                   \\\n  using CudaImageView##N = CudaImageView<K, N>;           \\\n  template <typename K>                                   \\\n  using CudaImageConstView##N = CudaImageConstView<K, N>; \\\n  ISAAC_DECLARE_CUDA_IMAGE_TYPES_IMPL(N, uint8_t, ub)     \\\n  ISAAC_DECLARE_CUDA_IMAGE_TYPES_IMPL(N, uint16_t, ui16)  \\\n  ISAAC_DECLARE_CUDA_IMAGE_TYPES_IMPL(N, int, i)          \\\n  ISAAC_DECLARE_CUDA_IMAGE_TYPES_IMPL(N, double, d)       \\\n  ISAAC_DECLARE_CUDA_IMAGE_TYPES_IMPL(N, float, f)\n\nISAAC_DECLARE_CUDA_IMAGE_TYPES(1)\nISAAC_DECLARE_CUDA_IMAGE_TYPES(2)\nISAAC_DECLARE_CUDA_IMAGE_TYPES(3)\nISAAC_DECLARE_CUDA_IMAGE_TYPES(4)\n\n// -------------------------------------------------------------------------------------------------\n// Helper function to create a CUDA image view from a pointer using dense storage\ntemplate <typename K, int N>\nCudaImageView<K, N> CreateCudaImageView(\n    K* data, typename CudaImageView<K, N>::index_t rows,\n    typename CudaImageView<K, N>::index_t cols) {\n  ASSERT(rows >= 0, \"Number of rows must not be negative - was %d\", rows);\n  ASSERT(cols >= 0, \"Number of columns must not be negative - was %d\", cols);\n  const size_t size = CudaImageView<K, N>::ByteCount(rows, cols);\n  return CudaImageView<K, N>(CudaBufferView(reinterpret_cast<byte*>(data), size), rows, cols);\n}\n\n// Helper function to create a CUDA image const view from a pointer using dense storage\ntemplate <typename K, int N>\nCudaImageConstView<K, N> CreateCudaImageConstView(\n    const K* data, typename CudaImageConstView<K, N>::index_t rows,\n    typename CudaImageConstView<K, N>::index_t cols) {\n  ASSERT(rows >= 0, \"Number of rows must not be negative - was %d\", rows);\n  ASSERT(cols >= 0, \"Number of columns must not be negative - was %d\", cols);\n  const size_t size = CudaImageConstView<K, N>::ByteCount(rows, cols);\n  return CudaImageConstView<K, N>(\n      CudaBufferConstView(reinterpret_cast<const byte*>(data), size), rows, cols);\n}\n\n// -------------------------------------------------------------------------------------------------\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/logger.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cstdarg>\n#include <cstdio>\n#include <vector>\n\n// Logs a debug message\n#define LOG_DEBUG(...)          \\\n  ::nvidia::isaac::logger::Log( \\\n      __FILE__, __LINE__, ::nvidia::isaac::logger::Severity::DEBUG, __VA_ARGS__)\n\n// Logs an informational message\n#define LOG_INFO(...)           \\\n  ::nvidia::isaac::logger::Log( \\\n      __FILE__, __LINE__, ::nvidia::isaac::logger::Severity::INFO, __VA_ARGS__)\n\n// Logs a warning\n#define LOG_WARNING(...)        \\\n  ::nvidia::isaac::logger::Log( \\\n      __FILE__, __LINE__, ::nvidia::isaac::logger::Severity::WARNING, __VA_ARGS__)\n\n// Logs an error\n#define LOG_ERROR(...)          \\\n  ::nvidia::isaac::logger::Log( \\\n      __FILE__, __LINE__, ::nvidia::isaac::logger::Severity::ERROR, __VA_ARGS__)\n\nnamespace nvidia {\nnamespace isaac {\nnamespace logger {\n\n// Indicates the level of severity for a log message\nenum class Severity {\n  // A utility case which can be used for `SetSeverity` to disable all severity levels.\n  NONE = -2,\n  // A utility case which can be used for `Redirect` and `SetSeverity`\n  ALL = -1,\n  // The five different log severities in use from most severe to least severe.\n  PANIC = 0,  // Need to start at 0\n  ERROR,\n  WARNING,\n  INFO,\n  DEBUG,\n  // A utility case representing the number of log levels used internally.\n  COUNT\n};\n\n// Function which is used for logging. It can be changed to intercept the logged messages.\nextern void (*LoggingFunction)(const char* file, int line, Severity severity, const char* log);\n\n// Redirects the output for a given log severity.\nvoid Redirect(std::FILE* file, Severity severity = Severity::ALL);\n\n// Redirects the output for a given file log severity up to and including a given log severity.\nvoid RedirectFileLog(std::FILE* file, Severity severity = Severity::DEBUG);\n\n// Sets global log severity thus effectively disabling all logging with lower severity\nvoid SetSeverity(Severity severity);\n\n// Converts the message and argument into a string and pass it to LoggingFunction.\ntemplate <typename... Args>\nvoid Log(const char* file, int line, Severity severity, const char* txt, ...) {\n  va_list args1;\n  va_start(args1, txt);\n  va_list args2;\n  va_copy(args2, args1);\n  std::vector<char> buf(1 + std::vsnprintf(NULL, 0, txt, args1));\n  va_end(args1);\n  std::vsnprintf(buf.data(), buf.size(), txt, args2);\n  va_end(args2);\n  LoggingFunction(file, line, severity, buf.data());\n}\n\n// Helper datastructure for Singleton\nstruct SeverityContainer {\n  Severity r = Severity::DEBUG;\n};\n\n}  // namespace logger\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/math/macros.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#ifdef __CUDACC__\n#define CUDA_BOTH __host__ __device__\n#define CUDA_BOTH_INLINE __forceinline__ __host__ __device__\n#define CUDA_INLINE __forceinline__ __device__\n#else\n#define CUDA_BOTH\n#define CUDA_BOTH_INLINE __inline__\n#endif  // __CUDACC__\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/math/pose2.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/core/math/so2.hpp\"\n#include \"gems/core/math/types.hpp\"\n#include \"gems/core/math/utils.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Class for 2D transformation.\ntemplate <typename K>\nstruct Pose2 {\n  using Scalar = K;\n  static constexpr int kDimension = 2;\n\n  SO2<K> rotation;\n  Vector2<K> translation;\n\n  // Returns identity transformation\n  static Pose2 Identity() { return Pose2{SO2<K>::Identity(), Vector2<K>::Zero()}; }\n  // Creates a translation transformation\n  static Pose2 Translation(const Vector2<K>& translation) {\n    return Pose2{SO2<K>::Identity(), translation};\n  }\n  static Pose2 Translation(K x, K y) { return Pose2{SO2<K>::Identity(), Vector2<K>{x, y}}; }\n  // Creates a rotation transformation\n  static Pose2 Rotation(const K angle) {\n    return Pose2{SO2<K>::FromAngle(angle), Vector2<K>::Zero()};\n  }\n  // Creates a pose from position and angle\n  static Pose2 FromXYA(K px, K py, K angle) {\n    return Pose2{SO2<K>::FromAngle(angle), Vector2<K>{px, py}};\n  }\n  // Creates a pose from matrix transformation\n  static Pose2 FromMatrix(const Matrix3<K>& matrix) {\n    return Pose2{\n        SO2<K>::FromNormalized(matrix(0, 0), matrix(1, 0)), Vector2<K>(matrix(0, 2), matrix(1, 2))};\n  }\n\n  // Returns the inverse transformation\n  Pose2 inverse() const {\n    const SO2<K> inv = rotation.inverse();\n    return Pose2{inv, -(inv * translation)};\n  }\n\n  // Returns the transformation as a matrix\n  Matrix3<K> matrix() const {\n    Matrix3<K> ret;\n    ret << rotation.cos(), -rotation.sin(), translation.x(), rotation.sin(), rotation.cos(),\n        translation.y(), K(0), K(0), K(1);\n    return ret;\n  }\n\n  // Casts to a different type\n  template <typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>\n  Pose2<S> cast() const {\n    return Pose2<S>{rotation.template cast<S>(), translation.template cast<S>()};\n  }\n  template <typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>\n  const Pose2& cast() const {\n    // Nothing to do as the type does not change\n    return *this;\n  }\n\n  // Composition of Pose2\n  friend Pose2 operator*(const Pose2& lhs, const Pose2& rhs) {\n    return Pose2{lhs.rotation * rhs.rotation, lhs.rotation * rhs.translation + lhs.translation};\n  }\n  // Transforms a vector 2D with the given 2D transformation\n  friend Vector2<K> operator*(const Pose2& pose, const Vector2<K>& vec) {\n    return pose.rotation * vec + pose.translation;\n  }\n  // Exponentaiton of the transformation\n  Pose2 pow(K exponent) const {\n    const K angle = rotation.angle();\n    const K half_angle = angle / K(2);\n    const K csc_sin =\n        IsAlmostZero(angle)\n            ? exponent * (K(1) + (K(1) - exponent * exponent) * half_angle * half_angle / K(6))\n            : std::sin(half_angle * exponent) / std::sin(half_angle);\n    const SO2<K> rot = SO2<K>::FromAngle(half_angle * (exponent - K(1)));\n    return Pose2{SO2<K>::FromAngle(angle * exponent), csc_sin * (rot * translation)};\n  }\n};\n\nusing Pose2d = Pose2<double>;\nusing Pose2f = Pose2<float>;\n\n// The \"exponential\" map from three-dimensional tangent space to SE(2) manifold space\n// For SE(2) this function encodes the tangent space as a three-dimensional vector (tx, ty, a)\n// where (tx, ty) is the translation component and a is the angle.\ntemplate <typename K>\nPose2<K> Pose2Exp(const Vector3<K>& tangent) {\n  return Pose2<K>{SO2<K>::FromAngle(tangent[2]), tangent.template head<2>()};\n}\n\n// The \"logarithmic\" map from manifold to tangent space\n// This computes the tangent for a pose relative to the identity pose. Log and exp are inverse\n// to each other.\ntemplate <typename K>\nVector3<K> Pose2Log(const Pose2<K>& pose) {\n  return Vector3<K>{pose.translation.x(), pose.translation.y(), pose.rotation.angle()};\n}\n\n// Returns true if given pose is almost identity.\ntemplate <typename K>\nbool IsPoseAlmostIdentity(const Pose2<K>& pose) {\n  return IsAlmostZero(pose.translation.x()) && IsAlmostZero(pose.translation.y()) &&\n         IsAlmostOne(pose.rotation.cos());\n}\n\n// Returns distance to identity pose in position and angle.\n// First element of the return value is for position, the seconds is for angle.\ntemplate <typename K>\nVector2<K> PoseMagnitude(const Pose2<K>& pose) {\n  return {pose.translation.norm(), std::abs(pose.rotation.angle())};\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/math/pose3.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/core/math/pose2.hpp\"\n#include \"gems/core/math/so3.hpp\"\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Class for 3D transformation.\ntemplate <typename K>\nstruct Pose3 {\n  using Scalar = K;\n  static constexpr int kDimension = 3;\n\n  SO3<K> rotation;\n  Vector3<K> translation;\n\n  // Returns identity transformation\n  CUDA_BOTH static Pose3 Identity() { return Pose3{SO3<K>::Identity(), Vector3<K>::Zero()}; }\n  // Creates a translation transformation\n  CUDA_BOTH static Pose3 Translation(const Vector3<K>& translation) {\n    return Pose3{SO3<K>::Identity(), translation};\n  }\n  CUDA_BOTH static Pose3 Translation(K x, K y, K z) {\n    return Pose3{SO3<K>::Identity(), Vector3<K>{x, y, z}};\n  }\n  // Creates a translation transformation\n  CUDA_BOTH static Pose3 Rotation(const Vector3<K>& axis, K angle) {\n    return Pose3{SO3<K>::FromAxisAngle(axis, angle), Vector3<K>::Zero()};\n  }\n  // Creates a 3D pose from a 2D pose in the XY plane\n  CUDA_BOTH static Pose3 FromPose2XY(const Pose2<K>& pose) {\n    return Pose3{\n        SO3<K>::FromSO2XY(pose.rotation),\n        Vector3<K>{pose.translation.x(), pose.translation.y(), K(0)}};\n  }\n  // Creates a pose from matrix transformation\n  CUDA_BOTH static Pose3 FromMatrix(const Matrix4<K>& matrix) {\n    return Pose3{\n        SO3<K>::FromNormalizedQuaternion(Quaternion<K>(matrix.template topLeftCorner<3, 3>())),\n        matrix.template topRightCorner<3, 1>()};\n  }\n\n  // Returns the inverse transformation\n  CUDA_BOTH Pose3 inverse() const {\n    const auto inv = rotation.inverse();\n    return Pose3{inv, -(inv * translation)};\n  }\n\n  // Returns the transformation as a matrix\n  CUDA_BOTH Matrix4<K> matrix() const {\n    Matrix4<K> ret;\n    ret.template topLeftCorner<3, 3>() = rotation.matrix();\n    ret.template topRightCorner<3, 1>() = translation;\n    ret.template bottomLeftCorner<1, 3>().setZero();\n    ret(3, 3) = K(1);\n    return ret;\n  }\n\n  // Casts to a different type\n  template <typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>\n  CUDA_BOTH Pose3<S> cast() const {\n    return Pose3<S>{rotation.template cast<S>(), translation.template cast<S>()};\n  }\n  template <typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>\n  CUDA_BOTH const Pose3& cast() const {\n    // Nothing to do as the type does not change\n    return *this;\n  }\n\n  // Converts to a 2D pose in the XY plane\n  CUDA_BOTH Pose2<K> toPose2XY() const {\n    return Pose2<K>{rotation.toSO2XY(), translation.template head<2>()};\n  }\n\n  // Composition of poses\n  CUDA_BOTH friend Pose3 operator*(const Pose3& lhs, const Pose3& rhs) {\n    return Pose3{lhs.rotation * rhs.rotation, lhs.rotation * rhs.translation + lhs.translation};\n  }\n  // Transforms a vector 2D with the given 2D transformation\n  CUDA_BOTH friend Vector3<K> operator*(const Pose3& pose, const Vector3<K>& vec) {\n    return pose.rotation * vec + pose.translation;\n  }\n  // Exponentaiton of the transformation\n  CUDA_BOTH Pose3 pow(K exponent) const {\n    // First step: align the rotation vector with the z axis.\n    const K angle = WrapPi<K>(rotation.angle());\n    if (IsAlmostZero(angle)) {\n      // TODO(bbutin): Use Taylor expension in that case?\n      return Pose3{rotation, exponent * translation};\n    }\n    const Vector3<K> axis = rotation.axis();\n    const Vector3<K> cross = axis.cross(Vector3<K>(K(0), K(0), K(1)));\n    const K cos = axis.z();\n    const K sin = cross.norm();\n\n    Matrix3<K> rot = Matrix3<K>::Identity() * cos;\n    // If axis is align in the Z direction, the matrix above will do the trick, if not we compute\n    // rotation matrix to transform the axis in the Z axis.\n    if (!IsAlmostZero(sin)) {\n      // TODO(bbutin): Use Taylor expension in case sin ~ 0?\n      const Vector3<K> unit = cross / sin;\n      rot += (1.0 - cos) * unit * unit.transpose();\n      rot(0, 1) += -cross.z();\n      rot(0, 2) += cross.y();\n      rot(1, 2) += -cross.x();\n      rot(1, 0) += cross.z();\n      rot(2, 0) += -cross.y();\n      rot(2, 1) += cross.x();\n    }\n    // Now we can compute the exponentation the same way as for the 2d for x and y, while z will\n    // just be linearly FromScaledAxis\n    const K half_angle = angle / K(2);\n    const K csc_sin =\n        IsAlmostZero(angle)\n            ? exponent * (K(1) + (K(1) - exponent * exponent) * half_angle * half_angle / K(6))\n            : std::sin(half_angle * exponent) / std::sin(half_angle);\n    const SO2<K> rot2 = SO2<K>::FromAngle(half_angle * (exponent - K(1)));\n    const Vector3<K> t = rot * translation;\n    const Vector2<K> xy = csc_sin * (rot2 * t.template head<2>());\n    const SO3<K> final_rotation = SO3<K>::FromAngleAxis(angle * exponent, axis);\n    return Pose3{final_rotation, rot.inverse() * Vector3<K>(xy.x(), xy.y(), t.z() * exponent)};\n  }\n};\n\nusing Pose3d = Pose3<double>;\nusing Pose3f = Pose3<float>;\n\n// The \"exponential\" map of SE(3) which maps a tangent space element to the manifold space\n// For SE(3) this function encodes the tangent space as a six-dimensional vector\n// (px, py, pz, rx, ry, rz) where (px, py, pz) is the translation component and (rx, ry, rz) is the\n// scaled rotation axis.\ntemplate <typename K>\nCUDA_BOTH Pose3<K> Pose3Exp(const Vector6<K>& tangent) {\n  return Pose3<K>{SO3<K>::FromScaledAxis(tangent.template tail<3>()), tangent.template head<3>()};\n}\n\n// The \"logarithmic\" map of SE(3) which maps a manifold space element to the tangent space\n// This computes the tangent for a pose relative to the identity pose. Log and exp are inverse\n// to each other.\ntemplate <typename K>\nCUDA_BOTH Vector6<K> Pose3Log(const Pose3<K>& pose) {\n  Vector6<K> result;\n  result.template head<3>() = pose.translation;\n  result.template tail<3>() = pose.rotation.angle() * pose.rotation.axis();\n  return result;\n}\n\n// Returns true if given pose is almost identity.\ntemplate <typename K>\nCUDA_BOTH bool IsPoseAlmostIdentity(const Pose3<K>& pose) {\n  return IsAlmostZero(pose.translation.norm()) && IsAlmostZero(pose.rotation.angle());\n}\n\n// Returns distance to identity pose in position and angle.\n// First element of the return value is for position, the seconds is for angle.\ntemplate <typename K>\nCUDA_BOTH Vector2<K> PoseMagnitude(const Pose3<K>& pose) {\n  return {pose.translation.norm(), static_cast<K>(std::abs(pose.rotation.angle()))};\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/math/so2.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/core/assert.hpp\"\n#include \"gems/core/epsilon.hpp\"\n#include \"gems/core/math/types.hpp\"\n#include \"gems/core/math/utils.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Class for 2D rotation\ntemplate <typename K>\nclass SO2 {\n public:\n  using Scalar = K;\n  static constexpr int kDimension = 2;\n\n  // Creates an undefined rotation\n  // Note: Use Identity to create the identity rotation\n  SO2() {}\n\n  // Return identity rotation\n  CUDA_BOTH static SO2 Identity() {\n    SO2 q;\n    q.cos_sin_ = Vector2<K>(K(1), K(0));\n    return q;\n  }\n  // Creates rotation from an angle\n  // Note: This uses calls to trigonometric functions.\n  CUDA_BOTH static SO2 FromAngle(K angle) {\n    SO2 q;\n    q.cos_sin_ = Vector2<K>(std::cos(angle), std::sin(angle));\n    return q;\n  }\n  // Creates rotation from a not necessarily normalized direction vector\n  CUDA_BOTH static SO2 FromDirection(const Vector2<K>& direction) {\n    SO2 q;\n    const K norm = direction.norm();\n    ASSERT(!IsAlmostZero(norm), \"Direction vector must not be 0\");\n    q.cos_sin_ = direction / norm;\n    return q;\n  }\n  CUDA_BOTH static SO2 FromDirection(K dx, K dy) { return FromDirection(Vector2<K>(dx, dy)); }\n  // Creates from a normalized cos/sin direction angle\n  CUDA_BOTH static SO2 FromNormalized(const Vector2<K>& cos_sin) {\n    ASSERT(\n        IsAlmostEqualRelative(cos_sin.squaredNorm(), K(1)),\n        \"Given cos/sin vector is not normalized: %lf\", cos_sin.norm());\n    SO2 q;\n    q.cos_sin_ = cos_sin;\n    return q;\n  }\n  CUDA_BOTH static SO2 FromNormalized(K cos_angle, K sin_angle) {\n    return FromNormalized(Vector2<K>(cos_angle, sin_angle));\n  }\n\n  // Access to cosine and sine of the rotation angle.\n  // Note this is a simple getter and deos not call trigonometric functions.\n  CUDA_BOTH K cos() const { return cos_sin_[0]; }\n  CUDA_BOTH K sin() const { return cos_sin_[1]; }\n  // Access to cos and sin of the rotation angle as a direction vector.\n  const Vector2<K>& asDirection() const { return cos_sin_; }\n\n  // Returns the rotation angle in range [-PI;PI]\n  // Note: This uses a call to a trigonometric function.\n  CUDA_BOTH K angle() const { return std::atan2(sin(), cos()); }\n  // Return Rotation matrix\n  CUDA_BOTH Matrix2<K> matrix() const {\n    Matrix2<K> m;\n    m(0, 0) = cos();\n    m(0, 1) = -sin();\n    m(1, 0) = sin();\n    m(1, 1) = cos();\n    return m;\n  }\n  // Inverse Rotation\n  CUDA_BOTH SO2 inverse() const { return SO2::FromNormalized(cos(), -sin()); }\n\n  // Casts to a different type\n  template <typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>\n  CUDA_BOTH SO2<S> cast() const {\n    // We need to re-normalize in the new type\n    return SO2<S>::FromDirection(asDirection().template cast<S>());\n  }\n  template <typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>\n  CUDA_BOTH const SO2& cast() const {\n    // Nothing to do as the type does not change\n    return *this;\n  }\n\n  // Rotation composition\n  CUDA_BOTH friend SO2 operator*(const SO2& lhs, const SO2& rhs) {\n    return FromDirection(\n        lhs.cos() * rhs.cos() - lhs.sin() * rhs.sin(),\n        lhs.sin() * rhs.cos() + lhs.cos() * rhs.sin());\n  }\n  // Rotates a vector 2D\n  CUDA_BOTH friend Vector2<K> operator*(const SO2& lhs, const Vector2<K>& vec) {\n    return Vector2<K>(\n        lhs.cos() * vec[0] - lhs.sin() * vec[1], lhs.sin() * vec[0] + lhs.cos() * vec[1]);\n  }\n\n private:\n  Vector2<K> cos_sin_;\n};\n\nusing SO2d = SO2<double>;\nusing SO2f = SO2<float>;\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/math/so3.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/core/math/macros.hpp\"\n#include \"gems/core/math/so2.hpp\"\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Class for 3D rotation\ntemplate <typename K>\nclass SO3 {\n public:\n  using Scalar = K;\n  static constexpr int kDimension = 3;\n\n  // Creates an empty un-initialized quaternion\n  // Note: Use Identity to create an identity rotation.\n  SO3() {}\n\n  // Return identity rotation\n  CUDA_BOTH static SO3 Identity() { return SO3(Quaternion<K>::Identity()); }\n  // Creates a rotation which rotates by around the given axes by the magnitude of the axis\n  CUDA_BOTH static SO3 FromScaledAxis(const Vector3<K>& axis_angle) {\n    const K norm = axis_angle.norm();\n    if (IsAlmostZero(norm)) {\n      return SO3::Identity();\n    } else {\n      return SO3(Quaternion<K>(Eigen::AngleAxis<K>(norm, axis_angle / norm)));\n    }\n  }\n  // Creates a rotation which rotates by an angle around a given (not necessarily normalized) axis\n  CUDA_BOTH static SO3 FromAxisAngle(const Vector3<K>& axis, K angle) {\n    return SO3(Quaternion<K>(Eigen::AngleAxis<K>(angle, axis.normalized())));\n  }\n  CUDA_BOTH static SO3 FromAngleAxis(K angle, const Vector3<K>& axis) {\n    return SO3(Quaternion<K>(Eigen::AngleAxis<K>(angle, axis.normalized())));\n  }\n  // Creates rotation from a (not necessarily normalized) quaternion\n  CUDA_BOTH static SO3 FromQuaternion(const Quaternion<K>& quaternion) {\n    return SO3(quaternion.normalized());\n  }\n  // Creates rotation from a normalized quaternion\n  // Note: This will assert if the quaternion does not have unit length.\n  CUDA_BOTH static SO3 FromNormalizedQuaternion(const Quaternion<K>& quaternion) {\n    ASSERT(\n        IsAlmostEqualRelative(quaternion.squaredNorm(), K(1)),\n        \"Given cos/sin vector is not normalized: %lf\", quaternion.norm());\n    return SO3(quaternion);\n  }\n  // Creates a 3D rotation from a 2D rotation in the XY plane\n  CUDA_BOTH static SO3 FromSO2XY(const SO2<K>& rotation) {\n    return FromAxisAngle({K(0), K(0), K(1)}, rotation.angle());\n  }\n  // Creates rotation from a 3x3 rotation matrix\n  CUDA_BOTH static SO3 FromMatrix(const Matrix<K, 3, 3>& matrix) {\n    return SO3(Quaternion<K>(matrix).normalized());\n  }\n\n  // Returns the rotation axis\n  CUDA_BOTH Vector3<K> axis() const { return quaternion_.coeffs().head(3).normalized(); }\n  // Returns the angle of rotation around the axis\n  // Note: This calls a trigonometric function.\n  CUDA_BOTH K angle() const {\n    return K(2) * std::atan2(quaternion_.coeffs().head(3).norm(), quaternion_.coeffs().w());\n  }\n  // Returns the quaternion representation of the rotation\n  CUDA_BOTH const Quaternion<K>& quaternion() const { return quaternion_; }\n  // Returns the matrix representation of the rotation\n  CUDA_BOTH Matrix3<K> matrix() const { return quaternion_.toRotationMatrix(); }\n\n  // Returns the roll, pitch, yaw euler angles of the rotation\n  CUDA_BOTH Vector3<K> eulerAnglesRPY() const {\n    const Vector3d euler_angles = quaternion_.toRotationMatrix().eulerAngles(0, 1, 2);\n    // Make sure the roll is in range [-Pi/2, Pi/2]\n    if (std::abs(euler_angles[0]) > Pi<K> * K(0.5)) {\n      return Vector3d(\n          WrapPi<K>(euler_angles[0] + Pi<K>), WrapPi<K>(Pi<K> - euler_angles[1]),\n          WrapPi<K>(euler_angles[2] + Pi<K>));\n    }\n    return euler_angles;\n  }\n\n  // Returns the inverse rotation.\n  CUDA_BOTH SO3 inverse() const {\n    // conjugate == inverse iff quaternion_.norm() == 1\n    return SO3(quaternion_.conjugate());\n  }\n\n  // Casts to a different type\n  template <typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>\n  CUDA_BOTH SO3<S> cast() const {\n    // We need to re-normalize in the new type\n    return SO3<S>::FromQuaternion(quaternion().template cast<S>());\n  }\n  template <typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>\n  CUDA_BOTH const SO3& cast() const {\n    // Nothing to do as the type does not change\n    return *this;\n  }\n\n  // Converts to a 2D rotation in the XY plane\n  CUDA_BOTH SO2<K> toSO2XY() const {\n    // 2D rotation matrix:\n    //   cos(a)   -sin(a)\n    //   sin(a)    cos(a)\n    // Quaternion to 3D rotation matrix:\n    //   1 - 2*(qy^2 + qz^2)      2*(qx*qy - qz*qw)   ...\n    //     2*(qx*qy + qz*qw)    1 - 2*(qx^2 + qz^2)   ...\n    // It follows (modulo re-normalization):\n    //   cos(a) = 1 - (qx^2 + qy^2 + 2*qz^2)\n    //   sin(a) = 2*qz*qw\n    // These formulas correspond to the half-angle formulas for sin/cos.\n    const K qx = quaternion_.x();\n    const K qy = quaternion_.y();\n    const K qz = quaternion_.z();\n    const K qw = quaternion_.w();\n    const K cos_a = K(1) - (qx * qx + qy * qy + K(2) * qz * qz);\n    const K sin_a = K(2) * qz * qw;\n    return SO2<K>::FromDirection(cos_a, sin_a);\n  }\n\n  // Rotation composition\n  CUDA_BOTH friend SO3 operator*(const SO3& lhs, const SO3& rhs) {\n    return FromQuaternion(lhs.quaternion_ * rhs.quaternion_);\n  }\n  // Rotates a vector 3D by the given rotation\n  CUDA_BOTH friend Vector3<K> operator*(const SO3& rot, const Vector3<K>& vec) {\n    // TODO: faster implementation\n    return (rot.quaternion_ * Quaternion<K>(K(0), vec.x(), vec.y(), vec.z()) *\n            rot.quaternion_.conjugate())\n        .coeffs()\n        .head(3);\n  }\n\n  // Create rotation from roll/pitch/yaw Euler angles\n  CUDA_BOTH static SO3 FromEulerAnglesRPY(K roll_angle, K pitch_angle, K yaw_angle) {\n    SO3 roll = SO3::FromAngleAxis(roll_angle, {K(1), K(0), K(0)});\n    SO3 pitch = SO3::FromAngleAxis(pitch_angle, {K(0), K(1), K(0)});\n    SO3 yaw = SO3::FromAngleAxis(yaw_angle, {K(0), K(0), K(1)});\n    return roll * pitch * yaw;\n  }\n  // Create rotation from Euler angles in order (roll, pitch, yaw)\n  CUDA_BOTH static SO3 FromEulerAnglesRPY(const Vector3d& roll_pitch_yaw) {\n    return FromEulerAnglesRPY(roll_pitch_yaw[0], roll_pitch_yaw[1], roll_pitch_yaw[2]);\n  }\n\n  // Computes the jacobian of the rotation of normal vector. Plane normal only has rotation\n  // component.\n  CUDA_BOTH Matrix<K, 3, 4> vectorRotationJacobian(const Vector3<K>& n) const {\n    // Ref: https://www.weizmann.ac.il/sci-tea/benari/sites/sci-tea.benari/files/uploads/\n    // softwareAndLearningMaterials/quaternion-tutorial-2-0-1.pdf\n    // Rotation Matrix in Quaternion (R):\n    //     [w^2 + x^2 - y^2 - z^2    2xy - 2wz                 2wy + 2xz]\n    //     [2wz + 2xy                w^2 - x^2 + y^2 - z^2     2yz - 2wx]\n    //     [2xz - 2wy                2wx + 2yz                 w^2 - x^2 - y^2 + z^2]\n    const K qx = quaternion_.x();\n    const K qy = quaternion_.y();\n    const K qz = quaternion_.z();\n    const K qw = quaternion_.w();\n    Matrix<K, 3, 4> result;\n    result << K(2) * (qw * n[0] - qz * n[1] + qy * n[2]),\n        K(2) * (qx * n[0] + qy * n[1] + qz * n[2]), K(-2) * (qy * n[0] - qx * n[1] - qw * n[2]),\n        K(-2) * (qz * n[0] + qw * n[1] - qx * n[2]),\n\n        K(2) * (qz * n[0] + qw * n[1] - qx * n[2]), K(2) * (qy * n[0] - qx * n[1] - qw * n[2]),\n        K(2) * (qx * n[0] + qy * n[1] + qz * n[2]), K(2) * (qw * n[0] - qz * n[1] + qy * n[2]),\n\n        K(-2) * (qy * n[0] - qx * n[1] - qw * n[2]), K(2) * (qz * n[0] + qw * n[1] - qx * n[2]),\n        K(-2) * (qw * n[0] - qz * n[1] + qy * n[2]), K(2) * (qx * n[0] + qy * n[1] + qz * n[2]);\n    return result;\n  }\n\n private:\n  SO3(const Quaternion<K>& quaternion) : quaternion_(quaternion) {}\n\n  Quaternion<K> quaternion_;\n};\n\nusing SO3d = SO3<double>;\nusing SO3f = SO3<float>;\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/math/types.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <Eigen/Eigen>\n\n#include \"gems/core/math/macros.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\ntemplate <typename K, int N, int M>\nusing Matrix = Eigen::Matrix<K, N, M>;\ntemplate <typename K, int N>\nusing Vector = Eigen::Matrix<K, N, 1>;\n\ntemplate <typename K, int N>\nusing RowVector = Eigen::Matrix<K, 1, N>;\ntemplate <typename K, int N>\nusing ColVector = Eigen::Matrix<K, N, 1>;\n\ntemplate <typename K>\nusing VectorX = Vector<K, Eigen::Dynamic>;\nusing VectorXd = VectorX<double>;\nusing VectorXf = VectorX<float>;\nusing VectorXi = VectorX<int>;\nusing VectorXub = VectorX<uint8_t>;\n\nusing VectorXcf = Eigen::VectorXcf;\n\ntemplate <typename K>\nusing RowVectorX = RowVector<K, Eigen::Dynamic>;\nusing RowVectorXf = RowVectorX<float>;\n\ntemplate <typename K>\nusing ColVectorX = ColVector<K, Eigen::Dynamic>;\nusing ColVectorXf = ColVectorX<float>;\n\ntemplate <typename K>\nusing MatrixX = Matrix<K, Eigen::Dynamic, Eigen::Dynamic>;\nusing MatrixXd = MatrixX<double>;\nusing MatrixXf = MatrixX<float>;\nusing MatrixXi = MatrixX<int>;\n\nusing MatrixXcf = Eigen::MatrixXcf;\n\n#define DEFINE_MATRIX_TYPES(N)            \\\n  template <typename K>                   \\\n  using Matrix##N = Matrix<K, N, N>;      \\\n  using Matrix##N##d = Matrix##N<double>; \\\n  using Matrix##N##f = Matrix##N<float>;  \\\n  using Matrix##N##i = Matrix##N<int>;\n\nDEFINE_MATRIX_TYPES(2)\nDEFINE_MATRIX_TYPES(3)\nDEFINE_MATRIX_TYPES(4)\nDEFINE_MATRIX_TYPES(5)\nDEFINE_MATRIX_TYPES(6)\nDEFINE_MATRIX_TYPES(7)\nDEFINE_MATRIX_TYPES(8)\n\n#undef DEFINE_MATRIX_TYPES\n\n// Matrix types with fixed number of rows but dynamic number of columns.\n// Useful to store and efficiently manipulate sets of geometric entities, e.g. points and planes.\n// 2xN: 2D points in Euclidean coordinates\n// 3xN: 2D points in homogeneous coordinates or 3D points in Euclidean coordinates\n// 4xN: planes or 3D points in homogeneous coordinates\n#define DEFINE_FIXED_ROWS_MATRIX_TYPES(N)            \\\n  template <typename K>                              \\\n  using Matrix##N##X = Matrix<K, N, Eigen::Dynamic>; \\\n  using Matrix##N##Xd = Matrix##N##X<double>;        \\\n  using Matrix##N##Xf = Matrix##N##X<float>;         \\\n  using Matrix##N##Xi = Matrix##N##X<int>;\n\nDEFINE_FIXED_ROWS_MATRIX_TYPES(2)\nDEFINE_FIXED_ROWS_MATRIX_TYPES(3)\nDEFINE_FIXED_ROWS_MATRIX_TYPES(4)\nDEFINE_FIXED_ROWS_MATRIX_TYPES(5)\nDEFINE_FIXED_ROWS_MATRIX_TYPES(6)\nDEFINE_FIXED_ROWS_MATRIX_TYPES(7)\nDEFINE_FIXED_ROWS_MATRIX_TYPES(8)\n\n#undef DEFINE_FIXED_ROWS_MATRIX_TYPES\n\ntemplate <typename K>\nusing Matrix34 = Matrix<K, 3, 4>;\nusing Matrix34d = Matrix34<double>;\nusing Matrix34f = Matrix34<float>;\nusing Matrix34i = Matrix34<int>;\n\ntemplate <typename K>\nusing Matrix43 = Matrix<K, 4, 3>;\nusing Matrix43d = Matrix43<double>;\nusing Matrix43f = Matrix43<float>;\nusing Matrix43i = Matrix43<int>;\n\n#define DEFINE_VECTOR_TYPES(N)            \\\n  template <typename K>                   \\\n  using Vector##N = Vector<K, N>;         \\\n  using Vector##N##d = Vector##N<double>; \\\n  using Vector##N##f = Vector##N<float>;  \\\n  using Vector##N##i = Vector##N<int>;    \\\n  using Vector##N##ub = Vector##N<uint8_t>;\n\nDEFINE_VECTOR_TYPES(2)\nDEFINE_VECTOR_TYPES(3)\nDEFINE_VECTOR_TYPES(4)\nDEFINE_VECTOR_TYPES(5)\nDEFINE_VECTOR_TYPES(6)\nDEFINE_VECTOR_TYPES(7)\nDEFINE_VECTOR_TYPES(8)\n\n#undef DEFINE_VECTOR_TYPES\n\ntemplate <typename K>\nusing Quaternion = Eigen::Quaternion<K>;\nusing Quaterniond = Quaternion<double>;\nusing Quaternionf = Quaternion<float>;\n\n// Helper function to compute the sum of two quaternions\ntemplate <typename K>\nCUDA_BOTH Quaternion<K> operator+(const Quaternion<K>& lhs, const Quaternion<K>& rhs) {\n  return Quaternion<K>(lhs.coeffs() + rhs.coeffs());\n}\n// Helper function to compute the difference of two quaternions\ntemplate <typename K>\nCUDA_BOTH Quaternion<K> operator-(const Quaternion<K>& lhs, const Quaternion<K>& rhs) {\n  return Quaternion<K>(lhs.coeffs() - rhs.coeffs());\n}\n// Unary - operator for a quaternion\ntemplate <typename K>\nCUDA_BOTH Quaternion<K> operator-(const Quaternion<K>& q) {\n  return Quaternion<K>(-q.coeffs());\n}\n\ntemplate <typename K>\nusing EigenImage = Eigen::Array<K, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;\ntemplate <typename K>\nusing EigenImageMap = Eigen::Map<EigenImage<K>>;\ntemplate <typename K>\nusing EigenImageConstMap = Eigen::Map<const EigenImage<K>>;\n\n// A base helper for constructing Views of data as eigen matrix maps.\ntemplate <\n    typename K, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic, int MatrixOptions = 0,\n    int MapOptions = 0, int OuterStride = 0, int InnerStride = 0>\nusing EigenMatrixView = Eigen::Map<\n    Eigen::Matrix<K, Rows, Cols, MatrixOptions>, MapOptions,\n    Eigen::Stride<OuterStride, InnerStride>>;\n\n// A base helper for constructing const Views of data as eigen matrix maps.\ntemplate <\n    typename K, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic, int MatrixOptions = 0,\n    int MapOptions = 0, int OuterStride = 0, int InnerStride = 0>\nusing EigenMatrixConstView = Eigen::Map<\n    const Eigen::Matrix<K, Rows, Cols, MatrixOptions>, MapOptions,\n    Eigen::Stride<OuterStride, InnerStride>>;\n\n// A base helper for constructing Views of data as eigen vector maps.\ntemplate <\n    typename K, int Rows = Eigen::Dynamic, int MapOptions = 0, int OuterStride = 0,\n    int InnerStride = 0>\nusing EigenVectorView =\n    Eigen::Map<Eigen::Matrix<K, Rows, 1>, MapOptions, Eigen::Stride<OuterStride, InnerStride>>;\n\n// A base helper for constructing const Views of data as eigen vector maps.\ntemplate <\n    typename K, int Rows = Eigen::Dynamic, int MapOptions = 0, int OuterStride = 0,\n    int InnerStride = 0>\nusing EigenVectorConstView = Eigen::Map<\n    const Eigen::Matrix<K, Rows, 1>, MapOptions, Eigen::Stride<OuterStride, InnerStride>>;\n\n// A base helper for constructing Views of data as eigen row vector maps.\ntemplate <\n    typename K, int Cols = Eigen::Dynamic, int MapOptions = 0, int OuterStride = 0,\n    int InnerStride = 0>\nusing EigenRowVectorView = Eigen::Map<\n    Eigen::Matrix<K, 1, Cols, Eigen::RowMajor>, MapOptions,\n    Eigen::Stride<OuterStride, InnerStride>>;\n\n// A base helper for constructing const Views of data as eigen row vector maps.\ntemplate <\n    typename K, int Cols = Eigen::Dynamic, int MapOptions = 0, int OuterStride = 0,\n    int InnerStride = 0>\nusing EigenRowVectorConstView = Eigen::Map<\n    const Eigen::Matrix<K, 1, Cols, Eigen::RowMajor>, MapOptions,\n    Eigen::Stride<OuterStride, InnerStride>>;\n\n// Creates a Vector from an initializer list\ntemplate <typename K, size_t N>\nCUDA_BOTH Vector<K, N> MakeVector(const K (&elements)[N]) {\n  Vector<K, N> result;\n  for (size_t i = 0; i < N; i++) {\n    result[i] = elements[i];\n  }\n  return result;\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/math/utils.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cmath>\n#include <type_traits>\n\n#include \"gems/core/assert.hpp\"\n#include \"gems/core/constants.hpp\"\n#include \"gems/core/epsilon.hpp\"\n#include \"gems/core/math/macros.hpp\"\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Returns square of a number. This is alternative to using std::pow(number, 2).\ntemplate <typename K>\nCUDA_BOTH K Square(K number) {\n  return number * number;\n}\n\n// Returns an angle within the range ]-Pi, Pi] that is equivalent to `angle`:\n//   WrapPi(angle) == angle + k*2*Pi (k being an integer)\ntemplate <typename K>\nCUDA_BOTH K WrapPi(K angle) {\n  // Use -angle in order to include Pi and exclude -Pi\n  const K d = std::floor((Pi<K> - angle) / TwoPi<K>);\n  return angle + d * TwoPi<K>;\n}\n\n// Returns an angle within the range [0, 2*Pi[ that is equivalent to `angle`:\n//   WrapTwoPi(angle) == angle + k*2*Pi (k being an integer)\ntemplate <typename K>\nCUDA_BOTH K WrapTwoPi(K angle) {\n  while (angle >= TwoPi<K>) angle -= TwoPi<K>;\n  while (angle < K(0)) angle += TwoPi<K>;\n  return angle;\n}\n\n// Returns the difference between two angles in the range ]-Pi, Pi]\n// Warning to not use the result to compare to zero if the angles are big.\ntemplate <typename K>\nCUDA_BOTH K DeltaAngle(K x, K y) {\n  return WrapPi(x - y);\n}\n\n// Returns the average of two angles in the range ]-Pi, Pi]\ntemplate <typename K>\nCUDA_BOTH K MeanAngle(K x, K y) {\n  return WrapPi(y + 0.5 * DeltaAngle(x, y));\n}\n\n// Computes a unit vector at given angle (anti clock-wise from x-axis)\ntemplate <typename K>\nCUDA_BOTH Vector2<K> AngleVector(K angle) {\n  return Vector2<K>{std::cos(angle), std::sin(angle)};\n}\n\n// Limits the absolute of a value\ntemplate <typename K>\nCUDA_BOTH K LimitAbs(K x, K max) {\n  return x < -max ? -max : (x > max ? max : x);\n}\n\n// Clamps a value to the given interval [xmin, xmax]\ntemplate <typename K>\nCUDA_BOTH K Clamp(K x, K xmin, K xmax) {\n  return x < xmin ? xmin : (x > xmax ? xmax : x);\n}\n// Clamps a value to the unit interval [0, 1]\ntemplate <typename K>\nCUDA_BOTH K Clamp01(K x) {\n  return Clamp(x, K(0), K(1));\n}\n\n// Safely cast a floating point number into an integral type and clamp the result in the range\n// [min, max]. If NaN is provided as input, then max will be returned.\ntemplate <\n    typename K, typename I, typename std::enable_if_t<std::is_floating_point<K>::value, int> = 0,\n    typename std::enable_if_t<std::is_integral<I>::value, int> = 0>\nCUDA_BOTH I SafeClampAndCast(K value, I min, I max) {\n  if (value < static_cast<K>(min)) {\n    return min;\n  }\n  if (value < static_cast<K>(max)) {\n    return static_cast<I>(value);\n  }\n  return max;\n}\n\n// Convenience function which calls std::floor and stores the result in a 32-bit integer.\ntemplate <typename K, typename std::enable_if_t<std::is_floating_point<K>::value, int> = 0>\nCUDA_BOTH int FloorToInt(K value) {\n  return static_cast<int>(std::floor(value));\n}\n\n// Rounds a double to a 32-bit integer using a nifty trick. This function only works if the double\n// is within reasonable range.\nCUDA_BOTH_INLINE int FastRoundDoubleToInt32(double x) {\n  union {\n    double real;\n    int integer;\n  } q;\n  q.real = x + 6755399441055744.0;\n  return q.integer;\n}\n\n// Convenience function which calls std::floor and stores the result in a 32-bit integer.\ntemplate <typename K, typename std::enable_if_t<std::is_floating_point<K>::value, int> = 0>\nCUDA_BOTH int FloorToIntWithReminder(K value, K& reminder) {\n  const K floor_value = std::floor(value);\n  reminder = value - floor_value;\n  return static_cast<int>(floor_value);\n}\n\n// Computes 0 <= a such that k*n + a = x for 0 <= k < n\nCUDA_BOTH_INLINE int PositiveModulo(int x, int n) {\n  const int k = x % n;\n  return (k < 0 ? k + n : k);\n}\n\n// Computes smallest q such that q * b >= a for a >= 0 and b > 0. If a or b are out of bounds\n// -1 is returned.\nCUDA_BOTH_INLINE int CeilDivision(int a, int b) {\n  if (a < 0 || b <= 0) return -1;\n  return (a / b) + (a % b != 0 ? 1 : 0);  // Note the assumption is that / and % are computed\n                                          // as part of the same operation.\n}\n\n// Returns whether or not two vectors are colinear\ntemplate <typename K, int N>\nCUDA_BOTH bool IsColinear(const Vector<K, N>& a, const Vector<K, N>& b) {\n  const K dot = a.dot(b);\n  // If dot is null, either one of the vector is null which means the two vectors are colinear, if\n  // not then the two vectors are orthogonal so obviously not colinear.\n  if (IsAlmostZero(dot)) {\n    return IsAlmostZero(a.squaredNorm()) || IsAlmostZero(b.squaredNorm());\n  }\n  // a.dot(b) / (||a|| * ||b||) = cos(alpha) where alpha is the angle between both vector;\n  // Taking the square of it avoid the use of sqrt and both vector are colinear iff |cos(alpha)| = 1\n  return IsAlmostOne(dot * dot / (a.squaredNorm() * b.squaredNorm()));\n}\n\n// Returns the cross product of Vector2 defined as:\n// AxB = ||A||*||B||*sin(A/B);\ntemplate <typename K>\nCUDA_BOTH K CrossProduct(const Vector2<K>& a, const Vector2<K>& b) {\n  return a.x() * b.y() - a.y() * b.x();\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/optional.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#if ((defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) || __cplusplus >= 201703L)\n\n#include <optional>\n\n#else\n\n#include <experimental/optional>\n\nnamespace std {\nusing experimental::make_optional;\nusing experimental::nullopt;\nusing experimental::optional;\n}  // namespace std\n\n#endif\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/tensor/sample_cloud.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2019-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/core/tensor/tensor.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// -------------------------------------------------------------------------------------------------\n\nnamespace sample_cloud_details {\n\n// Helper type to find the correct tensor type based on the number of channels.\ntemplate <typename K, int N, typename Buffer>\nstruct SampleCloudImpl {\n  using type = TensorBase<K, tensor::Dimensions<tensor::kDynamic, N>, Buffer>;\n};\ntemplate <typename K, typename Buffer>\nstruct SampleCloudImpl<K, 1, Buffer> {\n  using type = TensorBase<K, tensor::Rank<1>, Buffer>;\n};\n\n}  // namespace sample_cloud_details\n\n// A sample cloud is identical to a (X, N) tensor.\ntemplate <typename K, int N, typename Buffer>\nusing SampleCloudBase = typename sample_cloud_details::SampleCloudImpl<K, N, Buffer>::type;\n\n// -------------------------------------------------------------------------------------------------\n\ntemplate <typename K, int Channels>\nusing SampleCloud = SampleCloudBase<K, Channels, CpuBuffer>;\n\ntemplate <typename K, int Channels>\nusing SampleCloudView = SampleCloudBase<K, Channels, CpuBufferView>;\n\ntemplate <typename K, int Channels>\nusing SampleCloudConstView = SampleCloudBase<K, Channels, CpuBufferConstView>;\n\n#define ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, T, S) \\\n  using SampleCloud##N##S = SampleCloud<T, N>;         \\\n  using SampleCloudView##N##S = SampleCloudView<T, N>; \\\n  using SampleCloudConstView##N##S = SampleCloudConstView<T, N>;\n\n#define ISAAC_DECLARE_SAMPLE_CLOUD_TYPES(N)                   \\\n  template <typename K>                                       \\\n  using SampleCloud##N = SampleCloud<K, N>;                   \\\n  template <typename K>                                       \\\n  using SampleCloudView##N = SampleCloudView<K, N>;           \\\n  template <typename K>                                       \\\n  using SampleCloudConstView##N = SampleCloudConstView<K, N>; \\\n  ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, uint8_t, ub)       \\\n  ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, uint16_t, ui16)    \\\n  ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, int, i)            \\\n  ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, double, d)         \\\n  ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, float, f)\n\nISAAC_DECLARE_SAMPLE_CLOUD_TYPES(1)\nISAAC_DECLARE_SAMPLE_CLOUD_TYPES(2)\nISAAC_DECLARE_SAMPLE_CLOUD_TYPES(3)\nISAAC_DECLARE_SAMPLE_CLOUD_TYPES(4)\nISAAC_DECLARE_SAMPLE_CLOUD_TYPES(5)\nISAAC_DECLARE_SAMPLE_CLOUD_TYPES(6)\nISAAC_DECLARE_SAMPLE_CLOUD_TYPES(7)\nISAAC_DECLARE_SAMPLE_CLOUD_TYPES(8)\nISAAC_DECLARE_SAMPLE_CLOUD_TYPES(9)\n\n#undef ISAAC_DECLARE_SAMPLE_CLOUD_TYPES\n#undef ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL\n\n// -------------------------------------------------------------------------------------------------\n\ntemplate <typename K, size_t Channels>\nusing CudaSampleCloud = SampleCloudBase<K, Channels, CudaBuffer>;\n\ntemplate <typename K, size_t Channels>\nusing CudaSampleCloudView = SampleCloudBase<K, Channels, CudaBufferView>;\n\ntemplate <typename K, size_t Channels>\nusing CudaSampleCloudConstView = SampleCloudBase<K, Channels, CudaBufferConstView>;\n\n#define ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, T, S)    \\\n  using CudaSampleCloud##N##S = CudaSampleCloud<T, N>;         \\\n  using CudaSampleCloudView##N##S = CudaSampleCloudView<T, N>; \\\n  using CudaSampleCloudConstView##N##S = CudaSampleCloudConstView<T, N>;\n\n#define ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(N)                      \\\n  template <typename K>                                               \\\n  using CudaSampleCloud##N = CudaSampleCloud<K, N>;                   \\\n  template <typename K>                                               \\\n  using CudaSampleCloudView##N = CudaSampleCloudView<K, N>;           \\\n  template <typename K>                                               \\\n  using CudaSampleCloudConstView##N = CudaSampleCloudConstView<K, N>; \\\n  ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, uint8_t, ub)          \\\n  ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, uint16_t, ui16)       \\\n  ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, int, i)               \\\n  ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, double, d)            \\\n  ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, float, f)\n\nISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(1)\nISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(2)\nISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(3)\nISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(4)\nISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(5)\nISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(6)\nISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(7)\nISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(8)\nISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(9)\n\n#undef ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES\n#undef ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL\n\n// -------------------------------------------------------------------------------------------------\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/core/tensor/tensor.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2019-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <array>\n#include <initializer_list>\n#include <type_traits>\n#include <utility>\n#include <vector>\n\n#include \"gems/core/assert.hpp\"\n#include \"gems/core/buffers/buffer.hpp\"\n#include \"gems/core/byte.hpp\"\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// -------------------------------------------------------------------------------------------------\n\nnamespace tensor {\nnamespace details {\n\n// A compile-time sequence of integer\ntemplate <int... I>\nusing int_sequence = std::integer_sequence<int, I...>;\n\n// Helper type to define repeated_int_sequence\ntemplate <int N, int V, int... D>\nstruct repeated_int_sequence_impl {\n  using type = typename repeated_int_sequence_impl<N - 1, V, V, D...>::type;\n};\ntemplate <int V, int... D>\nstruct repeated_int_sequence_impl<0, V, D...> {\n  using type = int_sequence<D...>;\n};\n\n// A compile-time sequence of integers which are all identical\ntemplate <int N, int V>\nusing repeated_int_sequence = typename repeated_int_sequence_impl<N, V>::type;\n\ntemplate <typename IntegerSequence>\nstruct DropFirstImpl;\n\ntemplate <int First, int... Tail>\nstruct DropFirstImpl<int_sequence<First, Tail...>> {\n  using type = int_sequence<Tail...>;\n};\n\n// Compute the logical AND for a list of arguments\ntemplate <typename First, typename... Rest>\nstruct VariadicAnd : std::integral_constant<bool, First::value && VariadicAnd<Rest...>::value> {};\ntemplate <typename First>\nstruct VariadicAnd<First> : std::integral_constant<bool, First::value> {};\n\n}  // namespace details\n\n// This value can be used for tensor dimensions to indicate that the size is not fixed at compile\n// time and will instead be provided at runtime. We use the same value as Eigen to be compatible.\nconstexpr int kDynamic = Eigen::Dynamic;\n\n// A compile-time list of integers used to specify tensor dimensions.\ntemplate <int... I>\nusing Dimensions = details::int_sequence<I...>;\n\n// A compile-time list of integers used to specify tensor dimensions which are all dynamic.\ntemplate <int N>\nusing Rank = details::repeated_int_sequence<N, kDynamic>;\n\ntemplate <typename IntegerSequence>\nusing DropFirst = typename details::DropFirstImpl<IntegerSequence>::type;\n\n// Gets the i-th element in an integer sequence\ntemplate <int... Ints>\nconstexpr int IntSequenceAt(Dimensions<Ints...>, int i) {\n  constexpr int arr[] = {Ints...};\n  return i < 0 ? 0 : arr[i];\n}\n\n// Gets the last element in an integer sequence\ntemplate <int... Ints>\nconstexpr int IntSequenceLast(Dimensions<Ints...>) {\n  constexpr int arr[] = {Ints...};\n  return arr[sizeof...(Ints) - 1];\n}\n\n// Gets a vector of dimensions based on the compile-time dimensions and the dimensions given as\n// arguments. If the compile-time dimension is dynamic (-1) the dimension passed as an argument\n// will be used instead. Otherwise the compile-time dimension must match the argument dimension.\n// Dimensions must not be negative with the exception of using -1 for compile-time dimensions. It is\n// also possible to obmit trailing argument dimensions if they are fixed at compile time.\ntemplate <int... Dimensions, typename... Args>\nVector<int, sizeof...(Dimensions)> GetDimensionsVector(Args... dimensions) {\n  constexpr int kRank = sizeof...(Dimensions);\n  constexpr int kArgumentCount = sizeof...(Args);\n  std::array<int, kRank> compile_time_dimensions{Dimensions...};\n  // TODO(dweikersdorf) Use explicit conversion to int\n  Vector<int, kArgumentCount> runtime_dimensions(dimensions...);\n  Vector<int, kRank> result;\n  for (int i = 0; i < kRank; i++) {\n    if (i >= kArgumentCount) {\n      // The dimension was not specified as an argument. Thus use the compile-time dimension\n      ASSERT(\n          compile_time_dimensions[i] != -1,\n          \"Dimension argument can only be obmitted if the \"\n          \"dimension is specified at compile-time\");\n      ASSERT(\n          compile_time_dimensions[i] > 0,\n          \"Dimension argument can only be obmitted if the \"\n          \"compile-time dimension is positive. Was: %d\",\n          compile_time_dimensions[i]);\n      result[i] = compile_time_dimensions[i];\n    } else {\n      // The dimension was specified as an argument. Use if it the compile-time dimension is -1.\n      // Otherwise they both need to argree.\n      ASSERT(\n          compile_time_dimensions[i] == -1 || compile_time_dimensions[i] == runtime_dimensions[i],\n          \"dimension mismatch: %d vs %d\", compile_time_dimensions[i], runtime_dimensions[i]);\n      ASSERT(runtime_dimensions[i] >= 0, \"dimensions must not be negative: %d\", result[i]);\n      result[i] = runtime_dimensions[i];\n    }\n  }\n  return result;\n}\n\n// Gets a vector of dimensions based the template parameters. For a dimension of dynamic size a\n// size of zero will be chosen.\ntemplate <int... Dimensions>\nVector<int, sizeof...(Dimensions)> GetDimensionsVector() {\n  constexpr int kRank = sizeof...(Dimensions);\n  std::array<int, kRank> compile_time_dimensions{Dimensions...};\n  Vector<int, kRank> result;\n  for (int i = 0; i < kRank; i++) {\n    if (compile_time_dimensions[i] == kDynamic) {\n      result[i] = 0;\n    } else {\n      ASSERT(\n          compile_time_dimensions[i] > 0,\n          \"Compile-time dimensions must be positive or Dynamic \"\n          \"Was: %d\",\n          compile_time_dimensions[i]);\n      result[i] = compile_time_dimensions[i];\n    }\n  }\n  return result;\n}\n\n}  // namespace tensor\n\n// -------------------------------------------------------------------------------------------------\n\ntemplate <typename K, typename Dimensions, typename Buffer>\nstruct TensorBase;\n\n// A multi-dimensional array of elements.\n//\n// All elements are of the same type `K`.\n//\n// The tensor stores a multi-dimensional array of elements. The sizes of the array are specified by\n// the template parameter `Dimensions`. An std::integer_sequence of ints is used as type for the\n// list of dimensions. For each axes the dimension can either be a positive integer which indicates\n// a compile-time dimension or it can be `tensor::Dynamic` (which is -1) which indicates that the\n// dimension is specified at runtime. The template `tensor::Dimensions<...>` can be used as an alias\n// for `std::integer_sequence<int, ...>`. The template `tensor::Rank<N>` can be used as an alias for\n// `std::integer_sequence<int, tensor::Dynamic, ... /*(N times)*/>`.\n//\n// Elements are stored in a buffer of type `Buffer`. Most notably buffers can either own memory or\n// just be a view on memory owned by someone else. Buffers can also either allow or not allow\n// modifications of elements.\ntemplate <typename K, typename Buffer, int... DimensionsPack>\nclass TensorBase<K, tensor::details::int_sequence<DimensionsPack...>, Buffer> {\n public:\n  // Tensor<const int, ...> is not allowed. Use TensorConstView<int, ...> instead.\n  static_assert(!std::is_const<K>::value, \"Tensor can only be used with non-const element type\");\n\n  // The integer sequence containing the sizes of the tensor\n  using Dimensions = tensor::details::int_sequence<DimensionsPack...>;\n  // The rank of the tensor is the number of its dimensions.\n  static constexpr int kRank = sizeof...(DimensionsPack);\n  // The rank must be greater than 0.\n  static_assert(kRank > 0, \"Rank must be at least 1\");\n  // @deprecated\n  static constexpr int Order = kRank;\n\n  // The underlying buffer object used to store elements.\n  using buffer_t = Buffer;\n  // Type for mutable views on the underlying buffer\n  using buffer_view_t = typename BufferTraits<buffer_t>::buffer_view_t;\n  // Type for non-mutable views on the underlying buffer\n  using buffer_const_view_t = typename BufferTraits<buffer_t>::buffer_const_view_t;\n  // A mutable tensor allows modification of elements.\n  static constexpr bool kIsMutable = BufferTraits<buffer_t>::kIsMutable;\n  // An owning tensor owns its memory while a non-owning memory uses memory owned by another party.\n  static constexpr bool kIsOwning = BufferTraits<buffer_t>::kIsOwning;\n\n  // The integral type used for a single row or column index.\n  using index_t = int;\n  // The vector type to use for pixel coordinates.\n  using coordinate_t = Vector<index_t, kRank>;\n  // The vector type to use for pixel dimensions.\n  using dimensions_t = Vector<index_t, kRank>;\n\n  using element_t = std::remove_cv_t<K>;\n  using element_const_ptr_t = std::add_const_t<element_t>*;\n  using element_ptr_t = std::conditional_t<kIsMutable, element_t*, element_const_ptr_t>;\n\n  using element_const_ref_t = std::add_const_t<element_t>&;\n  using element_ref_t = std::conditional_t<kIsMutable, element_t&, element_const_ref_t>;\n\n  using raw_const_ptr_t = std::add_const_t<byte>*;\n  using raw_ptr_t = std::conditional_t<kIsMutable, byte*, raw_const_ptr_t>;\n\n  using tensor_view_t = TensorBase<K, Dimensions, buffer_view_t>;\n  using tensor_const_view_t = TensorBase<K, Dimensions, buffer_const_view_t>;\n\n  TensorBase() {\n    // If no dimensions are provided set to zero.\n    setDimensions(tensor::GetDimensionsVector<DimensionsPack...>());\n  }\n\n  // Constructs a tensor with the given dimensions which owns its data.\n  TensorBase(const dimensions_t& dimensions) { resize(dimensions, true); }\n  template <\n      typename... RequestedDimensionsPack,\n      std::enable_if_t<tensor::details::VariadicAnd<\n          std::is_convertible<RequestedDimensionsPack, int>...>::value>* = nullptr>\n  TensorBase(RequestedDimensionsPack... dimensions)\n      : TensorBase(tensor::GetDimensionsVector<DimensionsPack...>(dimensions...)) {}\n\n  // Constructs a tensor based on an existing buffer object using the given dimensions. This\n  // constructor will throw if the provided buffer does not contain enough data.\n  TensorBase(buffer_t data, const dimensions_t& dimensions) : data_(std::move(data)) {\n    ASSERT(CheckDimensions(dimensions), \"Trying to create tensor with invalid dimensions\");\n    setDimensions(dimensions);\n    ASSERT(\n        data_.size() >= byte_size(), \"Buffer too small: provided %zd, required %zd\", data_.size(),\n        byte_size());\n  }\n  template <\n      typename... RequestedDimensionsPack,\n      std::enable_if_t<tensor::details::VariadicAnd<\n          std::is_convertible<RequestedDimensionsPack, int>...>::value>* = nullptr>\n  TensorBase(buffer_t data, RequestedDimensionsPack... dimensions)\n      : TensorBase(std::move(data), tensor::GetDimensionsVector<DimensionsPack...>(dimensions...)) {\n  }\n\n  // Copy construction uses the default behavior\n  TensorBase(const TensorBase& other) = default;\n  // Copy assignment uses the default behavior1\n  TensorBase& operator=(const TensorBase& other) = default;\n  // Move construction uses the default behavior\n  TensorBase(TensorBase&& other) { *this = std::move(other); }\n  // Move assignment uses the default behavior\n  TensorBase& operator=(TensorBase&& other) {\n    data_ = std::move(other.data_);\n    // The dimensions of the tensor.\n    dimensions_ = other.dimensions_;\n    offsets_ = other.offsets_;\n    other.dimensions_.setZero();\n    other.offsets_.setZero();\n    return *this;\n  }\n\n  // Creates a mutable view on this tensor\n  template <bool X = kIsMutable>\n  std::enable_if_t<X, tensor_view_t> view() {\n    return tensor_view_t({this->data().begin(), this->data().size()}, this->dimensions());\n  }\n  // Creates a non-mutable view on this tensor\n  tensor_const_view_t const_view() const {\n    return tensor_const_view_t({this->data().begin(), this->data().size()}, this->dimensions());\n  }\n\n  // Provides conversion to a mutable view for owning tensors and mutable views.\n  template <bool X = kIsMutable>\n  operator std::enable_if_t<X, tensor_view_t>() {\n    return view();\n  }\n  // Provides conversion to a non-mutable view\n  operator tensor_const_view_t() const { return const_view(); }\n\n  // Returns true if any dimension of this tensor is 0 and thus if the number of elements stored in\n  // this tensor is 0.\n  bool empty() const { return element_count() == 0; }\n  // Returns the rank of the tensor\n  constexpr int rank() const { return kRank; }\n  // Deprecated - use rank() instead\n  constexpr int order() const { return kRank; }\n  // Returns the dimensions of the tensor\n  dimensions_t dimensions() const { return dimensions_; }\n\n  // Resizes the tensor. This operation is destructive.\n  template <bool X = kIsOwning>\n  std::enable_if_t<X, void> resize(\n      const dimensions_t& dimensions, bool force_reallocation = false) {\n    // Only allocate new memory if the dimensions change.\n    if (force_reallocation || dimensions != dimensions_) {\n      ASSERT(CheckDimensions(dimensions), \"Invalid dimensions\");\n      setDimensions(dimensions);\n      data_ = buffer_t(byte_size());\n    }\n  }\n\n  // Similar as `resize` but takes dimensions individually without using the vector type.\n  template <\n      typename... RequestedDimensionsPack, bool X = kIsMutable, std::enable_if_t<X>* = nullptr>\n  void resize(RequestedDimensionsPack... requested_dimensions) {\n    return resize(tensor::GetDimensionsVector<DimensionsPack...>(requested_dimensions...));\n  }\n\n  // Returns true if the element coordinate is within the tensor's dimension\n  bool isValidCoordinate(const coordinate_t& indicies) const {\n    return (indicies.array() >= 0).all() && (indicies.array() < dimensions_.array()).all();\n  }\n\n  // Helper wrappers for isValidCoordinate for the most common orders of tensors\n  template <typename... Args, std::enable_if_t<sizeof...(Args) == kRank>* = nullptr>\n  bool isValidCoordinate(Args... indices) const {\n    return isValidCoordinate(coordinate_t(indices...));  // TODO(dweikersdorf) Use explict casts\n  }\n\n  // Accesses the given index in the tensor with respect to memory ordering\n  template <bool X = std::is_trivial<element_t>::value>\n  std::enable_if_t<X, element_t> operator()(const coordinate_t& indicies) const {\n    return *(this->element_wise_begin() + indexToOffset(indicies));\n  }\n  // Accesses the given index in the tensor with respect to memory ordering\n  template <bool X = !std::is_trivial<element_t>::value>\n  std::enable_if_t<X, element_const_ref_t> operator()(const coordinate_t& indicies) const {\n    return *(this->element_wise_begin() + indexToOffset(indicies));\n  }\n\n  // Accesses the given index in the tensor with respect to memory ordering\n  template <bool X = kIsMutable>\n  std::enable_if_t<X, element_ref_t> operator()(const coordinate_t& indicies) {\n    return *(this->element_wise_begin() + indexToOffset(indicies));\n  }\n\n  // Helper wrappers for operator access for the most common orders of tensors\n  template <\n      typename... Args,\n      std::enable_if_t<sizeof...(Args) == kRank && std::is_trivial<element_t>::value>* = nullptr>\n  element_t operator()(Args... indices) const {\n    return operator()(coordinate_t(indices...));  // TODO(dweikersdorf) Use explict casts\n  }\n  // Helper wrappers for operator access for the most common orders of tensors\n  template <\n      typename... Args,\n      std::enable_if_t<sizeof...(Args) == kRank && !std::is_trivial<element_t>::value>* = nullptr>\n  element_const_ref_t operator()(Args... indices) const {\n    return operator()(coordinate_t(indices...));  // TODO(dweikersdorf) Use explict casts\n  }\n  // Helper wrappers for operator access for the most common orders of tensors\n  template <typename... Args, std::enable_if_t<sizeof...(Args) == kRank && kIsMutable>* = nullptr>\n  element_ref_t operator()(Args... indices) {\n    return operator()(coordinate_t(indices...));  // TODO(dweikersdorf) Use explict casts\n  }\n\n  // Creates a view on a slice with one dimension less. The dimension with highest significance\n  // is sliced.\n  template <bool X = kIsMutable && kRank >= 2>\n  std::enable_if_t<X, TensorBase<K, tensor::DropFirst<Dimensions>, buffer_view_t>> slice(\n      index_t index) {\n    using slice_t = TensorBase<K, tensor::DropFirst<Dimensions>, buffer_view_t>;\n    const size_t slice_size = static_cast<size_t>(offsets_[0]) * sizeof(K);\n    const size_t slice_offset = static_cast<size_t>(index) * slice_size;\n    return slice_t(\n        buffer_view_t{this->data().begin() + slice_offset, slice_size},\n        this->dimensions().template segment<kRank - 1>(1).eval());\n  }\n  template <bool X = !kIsMutable && kRank >= 2>\n  std::enable_if_t<X, TensorBase<K, tensor::DropFirst<Dimensions>, buffer_const_view_t>> slice(\n      index_t index) {\n    return const_slice(index);\n  }\n\n  // Creates a const view on a slice with one dimension less. The dimension with highest\n  // significance is sliced.\n  template <bool X = kRank >= 2>\n  std::enable_if_t<X, TensorBase<K, tensor::DropFirst<Dimensions>, buffer_const_view_t>>\n  const_slice(index_t index) const {\n    using const_slice_t = TensorBase<K, tensor::DropFirst<Dimensions>, buffer_const_view_t>;\n    const size_t slice_size = static_cast<size_t>(offsets_[0]) * sizeof(K);\n    const size_t slice_offset = static_cast<size_t>(index) * slice_size;\n    return const_slice_t(\n        buffer_const_view_t{this->data().begin() + slice_offset, slice_size},\n        this->dimensions().template segment<kRank - 1>(1).eval());\n  }\n\n  // Helper types for Eigen matrix-style access\n  static constexpr int kBackDim2 = tensor::IntSequenceAt(Dimensions{}, kRank - 2);\n  static constexpr int kBackDim1 = tensor::IntSequenceAt(Dimensions{}, kRank - 1);\n  using matrix_view_t = Eigen::Map<Eigen::Matrix<K, kBackDim2, kBackDim1, Eigen::RowMajor>>;\n  using matrix_const_view_t =\n      Eigen::Map<const Eigen::Matrix<K, kBackDim2, kBackDim1, Eigen::RowMajor>>;\n  using vector_view_t = Eigen::Map<Vector<K, kBackDim1>>;\n  using vector_const_view_t = Eigen::Map<const Vector<K, kBackDim1>>;\n\n  // Returns a mutable Eigen matrix view sliced on the last two dimensions\n  template <\n      typename... Args, std::enable_if_t<sizeof...(Args) == kRank - 2 && kIsMutable>* = nullptr>\n  matrix_view_t matrix(Args... indices) {\n    return matrix_view_t(\n        this->element_wise_begin() + indexToOffset(coordinate_t(indices..., 0, 0)),\n        dimensions_[kRank - 2], dimensions_[kRank - 1]);\n  }\n  // Returns a non-mutable Eigen matrix view sliced on the last two dimensions\n  template <typename... Args, std::enable_if_t<sizeof...(Args) == kRank - 2>* = nullptr>\n  matrix_const_view_t matrix(Args... indices) const {\n    return matrix_const_view_t(\n        this->element_wise_begin() + indexToOffset(coordinate_t(indices..., 0, 0)),\n        dimensions_[kRank - 2], dimensions_[kRank - 1]);\n  }\n  // Returns a mutable Eigen vector view sliced on the last dimension\n  template <\n      typename... Args, std::enable_if_t<sizeof...(Args) == kRank - 1 && kIsMutable>* = nullptr>\n  vector_view_t matrix(Args... indices) {\n    return vector_view_t(\n        this->element_wise_begin() + indexToOffset(coordinate_t(indices..., 0)),\n        dimensions_[kRank - 1]);\n  }\n  // Returns a non-mutable Eigen vector view sliced on the last dimension\n  template <typename... Args, std::enable_if_t<sizeof...(Args) == kRank - 1>* = nullptr>\n  vector_const_view_t matrix(Args... indices) const {\n    return vector_const_view_t(\n        this->element_wise_begin() + indexToOffset(coordinate_t(indices..., 0)),\n        dimensions_[kRank - 1]);\n  }\n\n  // When indices are omitted in operator() access it behaves the same as `matrix()`.\n  template <\n      typename... Args,\n      std::enable_if_t<\n          (sizeof...(Args) == kRank - 2 || sizeof...(Args) == kRank - 1) && kIsMutable>* = nullptr>\n  auto operator()(Args... indices) {\n    return matrix(indices...);\n  }\n  // When indices are omitted in operator() access it behaves the same as `matrix()`.\n  template <\n      typename... Args,\n      std::enable_if_t<sizeof...(Args) == kRank - 2 || sizeof...(Args) == kRank - 1>* = nullptr>\n  auto operator()(Args... indices) const {\n    return matrix(indices...);\n  }\n\n  // Const Pointer to the beginning of the data block\n  element_const_ptr_t element_wise_begin() const {\n    return reinterpret_cast<element_const_ptr_t>(data_.begin());\n  }\n  // Pointer to the beginning of the data block\n  element_ptr_t element_wise_begin() { return reinterpret_cast<element_ptr_t>(data_.begin()); }\n  // Const Pointer to the beginning of the data block\n  element_const_ptr_t element_wise_end() const {\n    return reinterpret_cast<element_const_ptr_t>(data_.end());\n  }\n  // Pointer to the beginning of the data block\n  element_ptr_t element_wise_end() { return reinterpret_cast<element_ptr_t>(data_.end()); }\n  // The total number of elements in the tensor\n  index_t num_elements() const { return offsets_[0] * dimensions_[0]; }\n  // @deprecated\n  int element_count() const { return num_elements(); }\n\n  // Returns a non mutable reference pixel holder at the position `index`.\n  element_const_ref_t operator[](index_t index) const { return this->element_wise_begin()[index]; }\n  // Returns a mutable reference pixel holder at the position `index`.\n  template <bool X = kIsMutable>\n  std::enable_if_t<X, element_ref_t> operator[](index_t index) {\n    return this->element_wise_begin()[index];\n  }\n\n  // const access to the underlying buffer object\n  const buffer_t& data() const { return data_; }\n  // access to the underlying buffer object\n  template <bool X = kIsMutable>\n  std::enable_if_t<X, buffer_t&> data() {\n    return data_;\n  }\n  // The total numbe of bytes required to store the tensor\n  size_t byte_size() const { return static_cast<size_t>(element_count()) * sizeof(element_t); }\n\n private:\n  // Returns true if all dimensions are greater than zero or zero\n  static bool CheckDimensions(const dimensions_t& dimensions) {\n    return (dimensions.array() >= 0).all();\n  }\n\n  // Sets the dimensions of the tensor and updates offsets.\n  void setDimensions(const dimensions_t& dimensions) {\n    dimensions_ = dimensions;\n    offsets_[kRank - 1] = 1;  // Rank is guaranteed to be positive\n    for (int i = kRank - 1; i > 0; i--) {\n      offsets_[i - 1] = offsets_[i] * dimensions_[i];\n    }\n  }\n\n  // Computes the position of an element based on its coordinate.\n  int indexToOffset(const coordinate_t& indices) const { return offsets_.dot(indices); }\n\n  // storage for the tensor\n  buffer_t data_;\n  // The dimensions of the tensor.\n  dimensions_t dimensions_;\n  // Dimension offsets for indexing rows of storage\n  dimensions_t offsets_;\n};  // namespace isaac\n\n// -------------------------------------------------------------------------------------------------\n\ntemplate <typename K, int N>\nusing CpuTensor = TensorBase<K, tensor::Rank<N>, CpuBuffer>;\n\ntemplate <typename K, int N>\nusing CpuTensorView = TensorBase<K, tensor::Rank<N>, CpuBufferView>;\n\ntemplate <typename K, int N>\nusing CpuTensorConstView = TensorBase<K, tensor::Rank<N>, CpuBufferConstView>;\n\n#define ISAAC_DECLARE_CPU_TENSOR_TYPES_IMPL(N, T, S) \\\n  using CpuTensor##N##S = CpuTensor<T, N>;           \\\n  using CpuTensorView##N##S = CpuTensorView<T, N>;   \\\n  using CpuTensorConstView##N##S = CpuTensorConstView<T, N>;\n\n#define ISAAC_DECLARE_CPU_TENSOR_TYPES(N)                 \\\n  template <typename K>                                   \\\n  using CpuTensor##N = CpuTensor<K, N>;                   \\\n  template <typename K>                                   \\\n  using CpuTensorView##N = CpuTensorView<K, N>;           \\\n  template <typename K>                                   \\\n  using CpuTensorConstView##N = CpuTensorConstView<K, N>; \\\n  ISAAC_DECLARE_CPU_TENSOR_TYPES_IMPL(N, uint8_t, ub)     \\\n  ISAAC_DECLARE_CPU_TENSOR_TYPES_IMPL(N, uint16_t, ui16)  \\\n  ISAAC_DECLARE_CPU_TENSOR_TYPES_IMPL(N, int, i)          \\\n  ISAAC_DECLARE_CPU_TENSOR_TYPES_IMPL(N, double, d)       \\\n  ISAAC_DECLARE_CPU_TENSOR_TYPES_IMPL(N, float, f)\n\nISAAC_DECLARE_CPU_TENSOR_TYPES(1)\nISAAC_DECLARE_CPU_TENSOR_TYPES(2)\nISAAC_DECLARE_CPU_TENSOR_TYPES(3)\nISAAC_DECLARE_CPU_TENSOR_TYPES(4)\n\n#undef ISAAC_DECLARE_CPU_TENSOR_TYPES\n#undef ISAAC_DECLARE_CPU_TENSOR_TYPES_IMPL\n\n// -------------------------------------------------------------------------------------------------\n\n// An Tensor stored in device memory which owns it's memory\ntemplate <typename K, int N>\nusing GpuTensor = TensorBase<K, tensor::Rank<N>, CudaBuffer>;\n\n// A mutable view on an Tensor which is stored on GPU device memory, does not own memory, but can\n// be used to read and write the data of the underlying Tensor.\ntemplate <typename K, int N>\nusing GpuTensorView = TensorBase<K, tensor::Rank<N>, CudaBufferView>;\n\n// A non-mutable view on an Tensor which is stored on GPU device memory, does not own its memory,\n// and can only be used to read the data of the underlying Tensor.\ntemplate <typename K, int N>\nusing GpuTensorConstView = TensorBase<K, tensor::Rank<N>, CudaBufferConstView>;\n\n// Helper macro for ISAAC_DECLARE_CUDA_TENSOR_TYPES\n#define ISAAC_DECLARE_CUDA_TENSOR_TYPES_IMPL(N, K, S) \\\n  using GpuTensor##N##S = GpuTensor<K, N>;            \\\n  using GpuTensorView##N##S = GpuTensorView<K, N>;    \\\n  using GpuTensorConstView##N##S = GpuTensorConstView<K, N>;\n\n// Helper macro to define various GpuTensor types\n#define ISAAC_DECLARE_CUDA_TENSOR_TYPES(N)                \\\n  template <typename K>                                   \\\n  using GpuTensor##N = GpuTensor<K, N>;                   \\\n  template <typename K>                                   \\\n  using GpuTensorView##N = GpuTensorView<K, N>;           \\\n  template <typename K>                                   \\\n  using GpuTensorConstView##N = GpuTensorConstView<K, N>; \\\n  ISAAC_DECLARE_CUDA_TENSOR_TYPES_IMPL(N, uint8_t, ub)    \\\n  ISAAC_DECLARE_CUDA_TENSOR_TYPES_IMPL(N, uint16_t, ui16) \\\n  ISAAC_DECLARE_CUDA_TENSOR_TYPES_IMPL(N, int, i)         \\\n  ISAAC_DECLARE_CUDA_TENSOR_TYPES_IMPL(N, double, d)      \\\n  ISAAC_DECLARE_CUDA_TENSOR_TYPES_IMPL(N, float, f)\n\nISAAC_DECLARE_CUDA_TENSOR_TYPES(1)\nISAAC_DECLARE_CUDA_TENSOR_TYPES(2)\nISAAC_DECLARE_CUDA_TENSOR_TYPES(3)\nISAAC_DECLARE_CUDA_TENSOR_TYPES(4)\n\n#undef ISAAC_DECLARE_CUDA_TENSOR_TYPES\n#undef ISAAC_DECLARE_CUDA_TENSOR_TYPES_IMPL\n\n// -------------------------------------------------------------------------------------------------\n\n// Creates a CpuTensorView instance from a raw host `data` pointer that allows to alter the data\n// pointed to, although it will still be owned by the entity that allocated it originally. The\n// `elements` parameter denotes the number of elements contained within `data`. The `shape`\n// parameter is a `Vector<int, N>` with `N` being the tensor's rank. Its elements are the dimensions\n// of the tensor. The template parameters denote the variable type the CpuTensorView should\n// represent and the rank of the tensor.\ntemplate <typename K, int N>\nCpuTensorView<K, N> CreateCpuTensorViewFromData(\n    K* data, size_t elements, const Vector<int, N>& shape) {\n  return CpuTensorView<K, N>(\n      CpuBufferView(HostPointer<byte>(reinterpret_cast<byte*>(data)), sizeof(K) * elements), shape);\n}\n\n// Creates a TensorConstView instance from a raw host `data` pointer. The data pointed to will still\n// be owned by the entity that allocated it originally. The `elements` parameter denotes the number\n// of elements contained within `data`. The `shape` parameter is a `Vector<int, N>` with `N` being\n// the tensor's rank. Its elements are the dimensions of the tensor. The template parameters denote\n// the variable type the CpuTensorView should represent and the rank of the tensor.\ntemplate <typename K, int N>\nCpuTensorConstView<K, N> CreateCpuTensorConstViewFromData(\n    const K* data, size_t elements, const Vector<int, N>& shape) {\n  return CpuTensorConstView<K, N>(\n      CpuBufferConstView(\n          HostPointer<const byte>(reinterpret_cast<const byte*>(data)), sizeof(K) * elements),\n      shape);\n}\n\n// Creates a GpuTensorView instance from a raw device `data` pointer that allows to alter the data\n// pointed to, although it will still be owned by the entity that allocated it originally. The\n// `elements` parameter denotes the number of elements contained within `data`. The `shape`\n// parameter is a `Vector<int, N>` with `N` being the tensor's rank. Its elements are the dimensions\n// of the tensor. The template parameters denote the variable type the TensorView should represent\n// and the rank of the tensor.\ntemplate <typename K, int N, typename T>\nGpuTensorView<K, N> CreateGpuTensorViewFromData(\n    T* data, size_t elements, const Vector<int, N>& shape) {\n  return GpuTensorView<K, N>(\n      CudaBufferView(CudaPointer<byte>(reinterpret_cast<byte*>(data)), sizeof(K) * elements),\n      shape);\n}\n\n// Creates a GpuTensorConstView instance from a raw device `data` pointer. The data pointed to\n// will still be owned by the entity that allocated it originally.The `elements` parameter\n// denotes the number of elements contained within `data`. The `shape` parameter is a\n// `Vector<int, N>` with `N` being the tensor's rank. Its elements are the dimensions of the\n// tensor. The template parameters denote the variable type the TensorView should represent and\n// the rank of the tensor.\ntemplate <typename K, int N, typename T>\nGpuTensorConstView<K, N> CreateGpuTensorConstViewFromData(\n    const T* data, size_t elements, const Vector<int, N>& shape) {\n  return GpuTensorConstView<K, N>(\n      CudaBufferConstView(\n          CudaPointer<const byte>(reinterpret_cast<const byte*>(data)), sizeof(K) * elements),\n      shape);\n}\n\n// -------------------------------------------------------------------------------------------------\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/cuda_utils/launch_utils.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2019-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"cuda_runtime.h\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Computes a positive integer x such that (x - 1) * b < a <= x * b\ntemplate<typename T>\nT DivRoundUp(T a, T b) {\n  return (a + b - T{1}) / b;\n}\n\n// Same as DivRoundUp but for all three elements. This is for example useful in computing\n// launch parameters for CUDA kernels.\ninline dim3 DivRoundUp(dim3 a, dim3 b) {\n  return {\n    DivRoundUp(a.x, b.x),\n    DivRoundUp(a.y, b.y),\n    DivRoundUp(a.z, b.z)\n  };\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/cuda_utils/stride_pointer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2019-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"cuda_runtime.h\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// A CUDA-friendly pointer with a stride. Can be used to work with data which uses a grid layout\n// with a non non-trivial offset between rows. This means rows are longer than they would need to be\n// in order to enable higher performance.\ntemplate <typename K>\nstruct StridePointer {\n  // Pointer to the first element\n  K* pointer;\n  // Offset in bytes between consecutive rows.\n  size_t stride;\n\n  // Gets a pointer to the given row\n  inline __host__ __device__\n  K* row_pointer(int row) const {\n    // TODO Properly cast pointer using reinterpret_cast with either char* or const char*\n    return reinterpret_cast<K*>((char*)(pointer) + row * stride);  // NOLINT\n  }\n\n  // Accesses the element at the coordinate (row, col)\n  inline __host__ __device__\n  const K& at(int row, int col) const { return row_pointer(row)[col]; }\n\n  // Accesses the element at the coordinate (row, col)\n  inline __host__ __device__\n  K& at(int row, int col) { return row_pointer(row)[col]; }\n\n  // Accesses the element at the given coordinates\n  inline __host__ __device__\n  const K& at(int2 coordinates) const { return at(coordinates.x, coordinates.y); }\n\n  // Accesses the element at the given coordinates\n  inline __host__ __device__\n  K& at(int2 coordinates) { return at(coordinates.x, coordinates.y); }\n\n  // Identical to `at`\n  inline __host__ __device__\n  const K& operator()(int row, int col) const { return row_pointer(row)[col]; }\n\n  // Identical to `at`\n  inline __host__ __device__\n  K& operator()(int row, int col) { return row_pointer(row)[col]; }\n\n  // Identical to `at`\n  inline __host__ __device__\n  const K& operator()(int2 coordinates) const { return at(coordinates.x, coordinates.y); }\n\n  // Identical to `at`\n  inline __host__ __device__\n  K& operator()(int2 coordinates) { return at(coordinates.x, coordinates.y); }\n\n  // Casts to a given element type. This can for example be used to cast between a\n  // StridePointer<float> used on the CPU and a StridePointer<float4> used on the GPU.\n  template <typename S>\n  __host__ __device__\n  StridePointer<S> cast() const { return {reinterpret_cast<S*>(pointer), stride}; }\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/cuda_utils/stride_pointer_3d.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <array>\n#include <cstddef>\n#include <cstdint>\n\n#include \"cuda_runtime.h\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// TODO(kchinniah): move memory layout & shape defns inside StridePointer3D class\n\n// TODO(kpatzwaldt): merge all the multidimensional indexing methods (ISAFETY-1363)\n\n/// The memory layout of a 3D pointer\nenum class StridePointer3D_MemoryLayout {\n  kNCHW,  ///< batch_size x channel x height x width\n  kHWC,   ///< height x width x channel\n  kNHWC,  ///< batch_size x height x width x channel\n};\n\n/// The shape of a 3D pointer\nstruct StridePointer3D_Shape {\n  /// The height\n  uint32_t height{};\n  /// The width\n  uint32_t width{};\n  /// The number of channels\n  uint32_t channels{};\n};\n\n// A stride pointer that's suitable for indexing 3D data.\n// In particular, this is useful for processing tensors.\n// If no strides are specified, the strides are computed assuming no offsets / padding.\n// WARNING: the NCHW and NHWC drop the N (batch size) dimension, hence\n// assuming that the batch size is 1.\n// and are only included to make converting tensors easier.\n// Additionally, this pointer does not have ownership of the passed pointer\n// Access to this must be given as: (row, col, depth) or (height, width, channel)\n// Bound checking is responsible on the caller of the methods of this class\ntemplate <typename T>\nclass StridePointer3D {\n private:\n  /// The maximum allowable dimension\n  static constexpr size_t kMaxDims{3};\n\n  // Ensure that the received data has a size greater than 0\n  static_assert(sizeof(T) > 0, \"Error: received type who's size is 0\");\n\n public:\n  StridePointer3D() : memory_buffer_{nullptr}, shape_{}, memory_layout_{}, strides_{0, 0, 0} {}\n\n  /**\n  Creates a 3D Stride Pointer using the raw memory, shape and memory layout specified.\n\n  This will compute trivial strides for the strides\n\n  @param T The type of the memory buffer\n  @param memory_buffer The memory buffer that will be traversed\n  @param shape The shape of the memory buffer\n  @param memory_layout The memory layout of the memory buffer\n  **/\n  StridePointer3D(\n      T* memory_buffer, StridePointer3D_Shape shape, StridePointer3D_MemoryLayout memory_layout)\n      : memory_buffer_{memory_buffer}, shape_{shape}, memory_layout_{memory_layout} {\n    computeStrides();\n    setStrideIndices();\n  }\n\n  /**\n  Creates a 3D Stride Pointer using the raw memory, shape, memory layout and strides specified.\n\n  The provided strides must be given in terms of bytes. These will be divided by sizeof(T) for\n  indexing\n\n  The byte_strides must be formatted as:\n  - byte_strides[2] = The amount of bytes necessary to traverse one unit of the least significant\n  dim\n  - byte_strides[1] = The amount of bytes necessary to traverse one unit of the 2nd least\n  significant dim\n  - byte_strides[0] = The amount of bytes necessary to traverse one unit of the 3rd least\n  significant dim\n\n  For example, suppose the memory layout is MemoryLayout::kHWC and that there is no extra padding or\n  offsets,\n  - byte_strides[2] should be sizeof(T) since it requires sizeof(T) units to traverse along the\n  channel axis\n  - byte_strides[1] should be sizeof(T) * shape.channels since this is the amount of bytes required\n  to traverse along the width axis\n    - For example, if there are 3 channels and sizeof(T) = 1, then to go from width = 0 -> width = 1\n  requires traversal of 3 units\n  - byte_strides[0] should be sizeof(T) * shape.channels * shape.width since this is the amount of\n  bytes required to traverse along the height axis\n    - For example, if there are 3 channels, sizeof(T) = 1 and the width is 5, then to go from width\n  = 0 -> 1 requires sizeof(T) * 3 channels, then since we would like to traverse from height 0 ->\n  height 1, we must traverse 5 widths worth of memory, hence sizeof(T) * 3 channels * 5 width units\n  is required\n\n  @param T The type of the memory buffer\n  @param memory_buffer The memory buffer that will be traversed\n  @param shape The shape of the memory buffer\n  @param memory_layout The memory layout of the memory buffer\n  @param byte_strides The strides of each dimension in bytes\n  **/\n  StridePointer3D(\n      T* memory_buffer, StridePointer3D_Shape shape, StridePointer3D_MemoryLayout memory_layout,\n      const std::array<size_t, kMaxDims>& byte_strides)\n      : memory_buffer_{memory_buffer}, shape_{shape}, memory_layout_{memory_layout} {\n    for (size_t i = 0; i < kMaxDims; ++i) {\n      strides_[i] = byte_strides[i] / sizeof(T);\n    }\n    setStrideIndices();\n  }\n\n  /**\n  This is identical to the std::array version of the constructor\n  **/\n  StridePointer3D(\n      T* memory_buffer, StridePointer3D_Shape shape, StridePointer3D_MemoryLayout memory_layout,\n      size_t byte_strides[kMaxDims])\n      : memory_buffer_{memory_buffer}, shape_{shape}, memory_layout_{memory_layout} {\n    for (size_t i = 0; i < kMaxDims; ++i) {\n      strides_[i] = byte_strides[i] / sizeof(T);\n    }\n    setStrideIndices();\n  }\n\n  /**\n  Returns a mutable reference at the specified (row, col, depth).\n\n  NOTE: (row, col, depth) corresponds to (height, width, channel) indexing\n\n  WARNING: this method does not do bounds checking\n\n  @param row The row (height) index\n  @param col The column (width) index\n  @param depth The depth (channel) index\n  **/\n  __device__ __host__ T& operator()(uint32_t row, uint32_t col, uint32_t depth) {\n    return memory_buffer_[computeFlattenedIndex(row, col, depth)];\n  }\n\n  /**\n  Returns a mutable reference at the specified (row, col). This assumes the depth is 0.\n\n  This is useful for processing, grayscale images, for example\n\n  NOTE: (row, col) corresponds to (height, width) indexing\n\n  WARNING: this method does not do bounds checking\n\n  @param row The row (height) index\n  @param col The column (width) index\n  **/\n  __device__ __host__ T& operator()(uint32_t row, uint32_t col) {\n    return memory_buffer_[computeFlattenedIndex(row, col, 0)];\n  }\n\n  /**\n  Returns a constant reference at the specified (row, col, depth).\n\n  NOTE: (row, col, depth) corresponds to (height, width, channel) indexing\n\n  WARNING: this method does not do bounds checking\n\n  @param row The row (height) index\n  @param col The column (width) index\n  @param depth The depth (channel) index\n  **/\n  __device__ __host__ const T& operator()(uint32_t row, uint32_t col, uint32_t depth) const {\n    return memory_buffer_[computeFlattenedIndex(row, col, depth)];\n  }\n\n  /**\n  Returns a constant reference at the specified (row, col). This assumes the depth is 0.\n\n  This is useful for processing, grayscale images, for example\n\n  WARNING: this method does not do bounds checking\n\n  NOTE: (row, col) corresponds to (height, width) indexing\n\n  @param row The row (height) index\n  @param col The column (width) index\n  **/\n  __device__ __host__ const T& operator()(uint32_t row, uint32_t col) const {\n    return memory_buffer_[computeFlattenedIndex(row, col, 0)];\n  }\n\n  /// Returns the memory layout\n  __device__ __host__ StridePointer3D_MemoryLayout memory_layout() const { return memory_layout_; }\n\n  /// Returns the shape\n  __device__ __host__ StridePointer3D_Shape shape() const { return shape_; }\n\n  /// Returns the memory buffer\n  __device__ __host__ T* pointer() const { return memory_buffer_; }\n\n  /**\n  Returns the memory buffer at the specified (row, col, depth).\n\n  NOTE: (row, col, depth) corresponds to (height, width, channel) indexing\n\n  WARNING: this method does not do bounds checking\n\n  @param row The row (height) index\n  @param col The column (width) index\n  @param depth The depth (channel) index\n  **/\n  __device__ __host__ T* pointerAt(uint32_t row, uint32_t col, uint32_t depth) const {\n    return memory_buffer_ + computeFlattenedIndex(row, col, depth);\n  }\n\n  /**\n  Returns the memory buffer at the specified (row, col). This assumes the depth is 0.\n\n  This is useful for processing, grayscale images, for example\n\n  NOTE: (row, col) corresponds to (height, width) indexing\n\n  WARNING: this method does not do bounds checking\n\n  @param row The row (height) index\n  @param col The column (width) index\n  **/\n  __device__ __host__ T* pointerAt(uint32_t row, uint32_t col) const {\n    return memory_buffer_ + computeFlattenedIndex(row, col, 0);\n  }\n\n  /**\n  Returns the flattened index at the specified (row, col, depth).\n\n  NOTE: (row, col, depth) corresponds to (height, width, channel) indexing\n\n  @param row The row (height) index\n  @param col The column (width) index\n  @param depth The depth (channel) index\n  **/\n  __device__ __host__ size_t indexAt(uint32_t row, uint32_t col, uint32_t depth) const {\n    return computeFlattenedIndex(row, col, depth);\n  }\n\n  /**\n  Returns the flattened index at the specified (row, col). This assumes the depth is 0.\n\n  This is useful for processing, grayscale images, for example\n\n  NOTE: (row, col) corresponds to (height, width) indexing\n\n  @param row The row (height) index\n  @param col The column (width) index\n  **/\n  __device__ __host__ size_t indexAt(uint32_t row, uint32_t col) const {\n    return computeFlattenedIndex(row, col, 0);\n  }\n\n private:\n  /// The raw memory buffer\n  T* memory_buffer_{};\n\n  /// The shape of the memory buffer\n  StridePointer3D_Shape shape_{};\n\n  /// The memory layout of the memory buffer\n  StridePointer3D_MemoryLayout memory_layout_;\n\n  /// The stride\n  size_t strides_[kMaxDims];\n\n  /// Maps a dimension to the stride array\n  struct StrideIndexMapping {\n    /// The height index mapping\n    size_t height_idx{};\n\n    /// The width index mapping\n    size_t width_idx{};\n\n    /// The channel index mapping\n    size_t channel_idx{};\n  } stride_index_mapping_;\n\n  /**\n  Computes the strides of the data based on the memory layout,\n  assuming no extra memory offset or padding\n\n  NOTE: the strides are divided by sizeof(T).\n  This is why, for example, strides_[2] = 1\n\n  The computed strides can be interpreted as follows:\n  - strides_[2] -> the stride required to traverse along the least significant dimension\n  - strides_[1] -> the stride required to traverse along the 2nd least significant dimension\n  - strides_[0] -> the stride required to traverse along the 3rd least significant dimension\n  **/\n  void computeStrides() {\n    switch (memory_layout_) {\n      case StridePointer3D_MemoryLayout::kNHWC:\n      case StridePointer3D_MemoryLayout::kHWC:\n        strides_[2] = 1;\n        strides_[1] = shape_.channels * strides_[2];\n        strides_[0] = shape_.width * strides_[1];\n        break;\n      case StridePointer3D_MemoryLayout::kNCHW:\n        strides_[2] = 1;\n        strides_[1] = shape_.width * strides_[2];\n        strides_[0] = shape_.height * strides_[1];\n        break;\n    }\n  }\n\n  /**\n  Sets the index stride mapping based on the memory layout.\n\n  For NHWC and HWC, this is height = 0, width = 1, channel = 2\n  For NCHW, this is channel = 0, width = 1, height = 2\n  **/\n  void setStrideIndices() {\n    switch (memory_layout_) {\n      case StridePointer3D_MemoryLayout::kNHWC:\n      case StridePointer3D_MemoryLayout::kHWC:\n        stride_index_mapping_.height_idx = 0;\n        stride_index_mapping_.width_idx = 1;\n        stride_index_mapping_.channel_idx = 2;\n        break;\n      case StridePointer3D_MemoryLayout::kNCHW:\n        stride_index_mapping_.channel_idx = 0;\n        stride_index_mapping_.height_idx = 1;\n        stride_index_mapping_.width_idx = 2;\n        break;\n    }\n  }\n\n  /**\n  Flattens a 3 dimensional index into a 1 dimension one\n\n  @param row The row (height) index\n  @param col the col (width) index\n  @param depth The depth (channel) index\n  **/\n  __device__ __host__ size_t\n  computeFlattenedIndex(uint32_t row, uint32_t col, uint32_t depth) const {\n    return row * strides_[stride_index_mapping_.height_idx] +\n           col * strides_[stride_index_mapping_.width_idx] +\n           depth * strides_[stride_index_mapping_.channel_idx];\n  }\n};\n\n#define ISAAC_DECLARE_STRIDE_POINTER_3D_TYPES(T, S) using StridePointer3D##S = StridePointer3D<T>;\n\nISAAC_DECLARE_STRIDE_POINTER_3D_TYPES(uint8_t, ub)\nISAAC_DECLARE_STRIDE_POINTER_3D_TYPES(uint16_t, ui16)\nISAAC_DECLARE_STRIDE_POINTER_3D_TYPES(int, i)\nISAAC_DECLARE_STRIDE_POINTER_3D_TYPES(double, d)\nISAAC_DECLARE_STRIDE_POINTER_3D_TYPES(float, f)\n\n#undef ISAAC_DECLARE_STRIDE_POINTER_3D_TYPES\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/cuda_utils/vector_math.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2019-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"cuda_runtime.h\"\n\nnamespace nvidia {\nnamespace isaac {\n\ninline __host__ __device__ float3 operator-(const float3& a, const float3& b) {\n  return make_float3(a.x - b.x, a.y - b.y, a.z - b.z);\n}\n\ninline __host__ __device__ float2 operator-(const float2& a, const float2& b) {\n  return make_float2(a.x - b.x, a.y - b.y);\n}\n\ninline __host__ __device__ float2 operator-(const float2& a, const int2& b) {\n  return make_float2(a.x - static_cast<float>(b.x), a.y - static_cast<float>(b.y));\n}\n\ninline __host__ __device__ float2 operator-(const int2& a, const float2& b) {\n  return make_float2(static_cast<float>(a.x) - b.x, static_cast<float>(a.y) - b.y);\n}\n\ninline __host__ __device__ int3 operator-(const int3& a, const int3& b) {\n  return make_int3(a.x - b.x, a.y - b.y, a.z - b.z);\n}\n\ninline __host__ __device__ int2 operator-(const int2& a, const int2& b) {\n  return make_int2(a.x - b.x, a.y - b.y);\n}\n\ninline __host__ __device__ float3 operator+(const float3& a, const float3& b) {\n  return make_float3(a.x + b.x, a.y + b.y, a.z + b.z);\n}\n\ninline __host__ __device__ float2 operator+(const float2& a, const float2& b) {\n  return make_float2(a.x + b.x, a.y + b.y);\n}\n\ninline __host__ __device__ int2 operator+(const int2& a, const int2& b) {\n  return make_int2(a.x + b.x, a.y + b.y);\n}\n\ninline __host__ __device__ float3 operator*(const float3& a, const float& s) {\n  return make_float3(s*a.x, s*a.y, s*a.z);\n}\n\ninline __host__ __device__ float3 operator*(const float& s, const float3& a) {\n  return make_float3(s*a.x, s*a.y, s*a.z);\n}\n\ninline __host__ __device__ float2 operator*(const float2& a, const float& s) {\n  return make_float2(s*a.x, s*a.y);\n}\n\ninline __host__ __device__ float2 operator*(const float& s, const float2& a) {\n  return make_float2(s*a.x, s*a.y);\n}\n\n// Coeffient-wise multiplication for two 3-vectors\ninline __host__ __device__ float3 CwiseMult(const float3& a, const float3& b) {\n  return make_float3(a.x*b.x, a.y*b.y, a.z*b.z);\n}\n\n// Dot product for two float3 vectors\ninline __host__ __device__ float Dot(const float3& a, const float3 b) {\n  return a.x*b.x + a.y*b.y + a.z*b.z;\n}\n\n// Dot product for two float2 vectors\ninline __host__ __device__ float Dot(const float2& a, const float2 b) {\n  return a.x*b.x + a.y*b.y;\n}\n\n// Dot product for two int3 vectors\ninline __host__ __device__ int Dot(const int3& a, const int3& b) {\n  return a.x*b.x + a.y*b.y + a.z*b.z;\n}\n\n// Dot product for two int2 vectors\ninline __host__ __device__ int Dot(const int2& a, const int2& b) {\n  return a.x*b.x + a.y*b.y;\n}\n\n// Cross product for two float3 vectors\ninline __host__ __device__ float3 Cross(const float3& a, const float3 b) {\n  return make_float3(a.y * b.z - a.z * b.y,\n                     a.z * b.x - a.x * b.z,\n                     a.x * b.y - a.y * b.x);\n}\n\n// Square of 2-norm for a float3 vector\ninline __host__ __device__ float SquareNorm(const float3& a) {\n  return Dot(a, a);\n}\n\n// Square of 2-norm for a int3 vector\ninline __host__ __device__ int SquareNorm(const int3& a) {\n  return Dot(a, a);\n}\n\n// Square of 2-norm for a float2 vector\ninline __host__ __device__ float SquareNorm(const float2& a) {\n  return Dot(a, a);\n}\n\n// 2-norm for a float3 vector\ninline __host__ __device__ float Norm(const float3& a) {\n  return sqrtf(SquareNorm(a));\n}\n\n// 2-norm for a float2 vector\ninline __host__ __device__ float Norm(const float2& a) {\n  return sqrtf(SquareNorm(a));\n}\n\n// Coefficient-wise sum for a float3 vector\ninline __host__ __device__ float Sum(const float3& a) {\n  return a.x + a.y + a.z;\n}\n\ninline __host__ __device__ int2 Floorf(const float2& a) {\n  return make_int2(floorf(a.x), floorf(a.y));\n}\n\n// Coefficient-wise conversion to int for a uchar3 vector\ninline __host__ __device__ int3 ToInt(uchar3 a) {\n  return make_int3(static_cast<int>(a.x), static_cast<int>(a.y), static_cast<int>(a.z));\n}\n\n// Coefficient-wise conversion to float for a int3 vector\ninline __host__ __device__ float3 ToFloat(int3 a) {\n  return make_float3(static_cast<float>(a.x), static_cast<float>(a.y), static_cast<float>(a.z));\n}\n\n// Coefficient-wise conversion to float for a int2 vector\ninline __host__ __device__ float2 ToFloat(int2 a) {\n  return make_float2(static_cast<float>(a.x), static_cast<float>(a.y));\n}\n\n// Accesses the index-th element of a float4 vector. Assumes index = 0 if index not in {0, 1, 2, 3}.\ninline __device__ float& Coefficient(float4& a, int index) {\n  if (index == 0) return a.x;\n  else if (index == 1) return a.y;\n  else if (index == 2) return a.z;\n  else if (index == 3) return a.w;\n  else return a.x;  // NOLINT\n}\n\n// Accesses the index-th element of a short4 vector. Assumes index = 0 if index not in {0, 1, 2, 3}.\ninline __device__ short& Coefficient(short4& a, int index) {  // NOLINT\n  if (index == 0) return a.x;\n  else if (index == 1) return a.y;\n  else if (index == 2) return a.z;\n  else if (index == 3) return a.w;\n  else return a.x;  // NOLINT\n}\n\n// Rotate a float2 unit vector by another float2 vector. The vector elements represent the cos and\n// sin of the rotation angle of the vector.\ninline __host__ __device__ float2 Rotate(const float2& a, const float2& b) {\n  return make_float2(a.x * b.x - a.y * b.y, a.y * b.x + a.x * b.y);\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/flatscan/flatscan_info.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\nnamespace nvidia {\nnamespace isaac {\n\n// Data structure holding meta information about flatscan messages. Every flatscan message must\n// contain an instance of this message.\nstruct FlatscanInfo {\n  // If the beam free range end is greater than or equal to this value the beam did not hit an\n  // obstacle at the end of the free range.\n  double out_of_range;\n  // Horizontal width of every beam in radians.\n  double beam_width;\n  // The lower z value of the height slice.\n  double height_min;\n  // The upper z value of the height slice.\n  double height_max;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/flatscan/flatscan_types.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/composite/composite_view.hpp\"\n#include \"gems/composite/schema.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Describes each beam in a flatscan\nstruct FlatscanIndices {\n  enum {\n    // Angle of the beam\n    kAngle,\n    // Start of the free range\n    kRangeStart,\n    // End of the free range\n    kRangeEnd,\n    // Visibility range of the flat ray, e.g., the furthest distance the ray could reach within\n    // the height slice without any obstacles\n    kVisibilityRange,\n    // The relative time for this beam to the message timestamp, unit in seconds\n    // beam_absolute_time = message_time + relative_time\n    kRelativeTime,\n    kSize\n  };\n};\n\ninline composite::Schema FlatscanCompositeSchema() {\n  return composite::Schema({\n      composite::Quantity::Scalar(\"angle\", composite::Measure::kRotation),\n      composite::Quantity::Scalar(\"range_start\", composite::Measure::kPosition),\n      composite::Quantity::Scalar(\"range_end\", composite::Measure::kPosition),\n      composite::Quantity::Scalar(\"visibility_range\", composite::Measure::kPosition),\n      composite::Quantity::Scalar(\"relative_time\", composite::Measure::kTime),\n  });\n}\n\ntemplate <typename K, typename Base>\nstruct FlatscanImmutableInterface : public Base {\n  using Base::Base;\n  using Base::operator=;\n\n  K angle() const { return get<FlatscanIndices::kAngle>(*this); }\n  K range_start() const { return get<FlatscanIndices::kRangeStart>(*this); }\n  K range_end() const { return get<FlatscanIndices::kRangeEnd>(*this); }\n  K visibility_range() const { return get<FlatscanIndices::kVisibilityRange>(*this); }\n  K relative_time() const { return get<FlatscanIndices::kRelativeTime>(*this); }\n};\n\ntemplate <typename K, typename Base>\nstruct FlatscanMutableInterface : public FlatscanImmutableInterface<K, Base> {\n  using FlatscanImmutableInterface<K, Base>::FlatscanImmutableInterface;\n  using FlatscanImmutableInterface<K, Base>::operator=;\n\n  K& angle() { return get<FlatscanIndices::kAngle>(*this); }\n  K& range_start() { return get<FlatscanIndices::kRangeStart>(*this); }\n  K& range_end() { return get<FlatscanIndices::kRangeEnd>(*this); }\n  K& visibility_range() { return get<FlatscanIndices::kVisibilityRange>(*this); }\n  K& relative_time() { return get<FlatscanIndices::kRelativeTime>(*this); }\n};\n\ntemplate <typename K>\nusing FlatscanConstView = CompositeConstView<K, FlatscanIndices, FlatscanImmutableInterface>;\n\ntemplate <typename K>\nusing FlatscanView = CompositeView<K, FlatscanIndices, FlatscanMutableInterface>;\n\ntemplate <typename K>\nusing Flatscan = Composite<K, FlatscanIndices, FlatscanMutableInterface>;\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/geometry/line.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <algorithm>\n#include <cmath>\n#include <type_traits>\n\n#include \"gems/core/math/types.hpp\"\n#include \"gems/core/math/utils.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace geometry {\n\n// Represent a line in dimension N.\n// The line is defined with a point and a ray. If IsFullLine is set to false then the line is a\n// half line starting from the origin and in the direction of the ray.\ntemplate <typename K, int N, bool IsFullLine>\nclass Line {\n public:\n  // Type of a point.\n  using Vector_t = Vector<K, N>;\n  using Scalar = K;\n  constexpr static int kDimension = N;\n\n  // Empty constructor to allow allocation\n  Line() {}\n\n  // Helper to create a direction from a point and a direction.\n  static Line FromDirection(const Vector_t& pt, const Vector_t& direction) {\n    return Line{pt, direction};\n  }\n  // Helper to create a direction from two points.\n  static Line FromPoints(const Vector_t& a, const Vector_t& b) {\n    return Line{a, b-a};\n  }\n\n  // Returns a point on the line.\n  const Vector_t& origin() const {\n    return origin_;\n  }\n\n  // Returns the direction starting from the extremity 'origin'.\n  const Vector_t& direction() const {\n    return direction_;\n  }\n\n  // Clamp the value of lambda such as origin() + lambda * direction() belong on the Line.\n  K clamp(K lambda) const {\n    if (IsFullLine) {\n      return lambda;\n    } else {\n      return std::max(K(0), lambda);\n    }\n  }\n\n  // Casts to a different type\n  template <typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>\n  Line<S, N, IsFullLine> cast() const {\n    return Line<S, N, IsFullLine>::FromDirection(\n        origin_.template cast<S>(), direction_.template cast<S>());\n  }\n  template<typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>\n  const Line& cast() const {\n    // Nothing to do as the type does not change\n    return *this;\n  }\n\n private:\n  // Private constructor to avoid confusion between direction initialization and points\n  // initialization. Use FromDirection or FromPoints to create a line.\n  Line(const Vector_t& origin, const Vector_t& direction) : origin_(origin), direction_(direction) {\n    ASSERT(!IsAlmostZero(direction.squaredNorm()), \"direction must not be null\");\n  }\n\n  Vector_t origin_;\n  Vector_t direction_;\n};\n\nusing Line2d = Line<double, 2, true>;\nusing Line3d = Line<double, 3, true>;\nusing Line4d = Line<double, 4, true>;\nusing Line2f = Line<float, 2, true>;\nusing Line3f = Line<float, 3, true>;\nusing Line4f = Line<float, 4, true>;\n\nusing Ray2d = Line<double, 2, false>;\nusing Ray3d = Line<double, 3, false>;\nusing Ray4d = Line<double, 4, false>;\nusing Ray2f = Line<float, 2, false>;\nusing Ray3f = Line<float, 3, false>;\nusing Ray4f = Line<float, 4, false>;\n\n}  // namespace geometry\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/geometry/line_segment.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cmath>\n#include <type_traits>\n\n#include \"gems/core/math/types.hpp\"\n#include \"gems/core/math/utils.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace geometry {\n\n// Represent a segment in dimension N.\n// The segment is defined with a couple of points (both extremities).\ntemplate <typename K, int N>\nclass LineSegment {\n public:\n  // Type of a point.\n  using Vector_t = Vector<K, N>;\n  using Scalar = K;\n  constexpr static int kDimension = N;\n\n  // Empty constructor to allow allocation.\n  LineSegment() {}\n\n  // Constructor.\n  LineSegment(const Vector_t& a, const Vector_t& b) : a_(a), b_(b) {\n  }\n\n  // Helper to create a segment from two points.\n  static LineSegment FromPoints(const Vector_t& a, const Vector_t& b) {\n    return LineSegment{a, b};\n  }\n\n  // Returns one extremity such as origin() + direction() == the other extremity.\n  const Vector_t& origin() const {\n    return a_;\n  }\n\n  // Returns the direction starting from the extremity 'origin'.\n  const Vector_t direction() const {\n    return b_ - a_;\n  }\n\n  // Returns one extremity of the segment.\n  const Vector_t& a() const {\n    return a_;\n  }\n\n  // Returns one extremity of the segment.\n  Vector_t& a() {\n    return a_;\n  }\n\n  // Returns the other extremity of the segment.\n  const Vector_t& b() const {\n    return b_;\n  }\n\n  // Returns the other extremity of the segment.\n  Vector_t& b() {\n    return b_;\n  }\n\n  // Clamp the value of lambda such as origin() + lambda * direction() belong on the LineSegment.\n  K clamp(K lambda) const {\n    return lambda <= K(0) ? K(0) : (lambda < K(1) ? lambda : K(1));\n  }\n\n  // Casts to a different type\n  template <typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>\n  LineSegment<S, N> cast() const {\n    return LineSegment<S, N>(a_.template cast<S>(), b_.template cast<S>());\n  }\n  template<typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>\n  const LineSegment& cast() const {\n    // Nothing to do as the type does not change\n    return *this;\n  }\n\n private:\n  Vector_t a_;\n  Vector_t b_;\n};\n\nusing LineSegment2i = LineSegment<int, 2>;\nusing LineSegment3i = LineSegment<int, 3>;\nusing LineSegment4i = LineSegment<int, 4>;\nusing LineSegment2d = LineSegment<double, 2>;\nusing LineSegment3d = LineSegment<double, 3>;\nusing LineSegment4d = LineSegment<double, 4>;\nusing LineSegment2f = LineSegment<float, 2>;\nusing LineSegment3f = LineSegment<float, 3>;\nusing LineSegment4f = LineSegment<float, 4>;\n\n}  // namespace geometry\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/geometry/line_utils.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cmath>\n#include <type_traits>\n\n#include \"gems/core/assert.hpp\"\n#include \"gems/core/math/types.hpp\"\n#include \"gems/core/math/utils.hpp\"\n#include \"gems/geometry/polyline.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace geometry {\n\n// Returns the closest point of a segment (ab) to a point `p`.\n// NOTE: It can be used with Line/HalfLine/Segment.\ntemplate<typename Line>\ntypename Line::Vector_t ClosestPointToLine(const Line& line, const typename Line::Vector_t& p);\n\n// Returns the squared distance between a point and a Line/HalfLine/Segment.\n// Avoid any sqrt by using the pytagorian theorem.\ntemplate<typename Line>\ntypename Line::Scalar SquaredDistancePointToLine(const Line& line,\n                                                 const typename Line::Vector_t& p);\n\n// Returns the distance between a point and a Line/HalfLine/Segment.\ntemplate<typename Line>\ntypename Line::Scalar DistancePointToLine(const Line& line, const typename Line::Vector_t& p);\n\n// Returns whether two Line/HalfLine/Segment are intersecting.\n// In addition if the line intersect lambda_{A/B} would be filled such as:\n// A.origin() + lambda_a A.direction() == B.origin() + lambda_b B.direction()\ntemplate<typename LineT1, typename LineT2>\nbool AreLinesIntersecting(const LineT1& line_a, const LineT2& line_b,\n                          typename LineT1::Scalar* lambda_a = nullptr,\n                          typename LineT1::Scalar* lambda_b = nullptr);\n\n// Returns the closest point of two lines. If the lines are intersecting, the intersection will be\n// returned, otherwise (i.e. when the lines are parallel), any point of any line would work. Note\n// that if a line segment is passed this function only looks at the underlying line and ignores the\n// start and end of the segment.\ntemplate <typename LineT1, typename LineT2>\nVector2<typename LineT1::Scalar> Intersection(const LineT1& line_a, const LineT2& line_b);\n\n// Returns the closest point of a segment [ab] to a point `p`.\ntemplate<typename K, int N>\nVector<K, N> ClosestPointToSegment(const Vector<K, N>& a, const Vector<K, N>& b,\n                                   const Vector<K, N>& p);\n\n// Returns the closest point of a polyline [ab...yz] to a point `p`.\n// If `segment_index` is provided, it will contain the index of the segment the closest to the\n// point.\ntemplate<typename K, int N>\nVector<K, N> ClosestPointToPolyline(const Polyline<K, N>& polyline, const Vector<K, N>& p,\n                                    size_t* segment_index = nullptr);\n\n// Returns the squared distance of a point `p` to a segment [ab].\ntemplate<typename K, int N>\nK DistanceSquaredPointToSegment(const Vector<K, N>& a, const Vector<K, N>& b,\n                                const Vector<K, N>& p);\n\n// Returns the distance of a point `p` to a segment [ab].\ntemplate<typename K, int N>\nK DistancePointToSegment(const Vector<K, N>& a, const Vector<K, N>& b, const Vector<K, N>& p);\n\n// -------------------------------------------------------------------------------------------------\n\ntemplate<typename Line>\ntypename Line::Vector_t ClosestPointToLine(const Line& line, const typename Line::Vector_t& p) {\n  const typename Line::Vector_t& ray = line.direction();\n  const typename Line::Vector_t ap = p - line.origin();\n  const typename Line::Scalar l = line.clamp(ray.dot(ap) / ray.squaredNorm());\n  return line.origin() + l * ray;\n}\n\ntemplate<typename Line>\ntypename Line::Scalar SquaredDistancePointToLine(const Line& line,\n                                                 const typename Line::Vector_t& p) {\n  using K = typename Line::Scalar;\n  const typename Line::Vector_t& ray = line.direction();\n  const typename Line::Vector_t ap = p - line.origin();\n  const K ray_sq_norm = ray.squaredNorm();\n  // Check this is not an empty segment, in which case the distance is the distance to one of the\n  // extremities.\n  if (IsAlmostZero(ray_sq_norm)) return ap.squaredNorm();\n  const K dot = ray.dot(ap);\n  const K l = dot / ray_sq_norm;\n  const K cl = line.clamp(l);\n  const K cl2 = cl * cl * ray_sq_norm;\n  return ap.squaredNorm() + cl2 - K(2) * l * cl * ray_sq_norm;\n}\n\ntemplate<typename Line>\ntypename Line::Scalar DistancePointToLine(const Line& line, const typename Line::Vector_t& p) {\n  return std::sqrt(SquaredDistancePointToLine(line, p));\n}\n\n// P(x, y) is on a line iff (P-A)xR_a = 0\n// We are looking for lambda such as (B + lambda R_b - A) x R_a = 0\n// lambda = (A-B) x R_a / (R_b x R_a)\n// x denotes the cross product here.\ntemplate<typename LineT1, typename LineT2>\nbool AreLinesIntersecting(const LineT1& line_a, const LineT2& line_b,\n                          typename LineT1::Scalar* lambda_a,\n                          typename LineT1::Scalar* lambda_b) {\n  static_assert(LineT1::kDimension == 2 && LineT2::kDimension == 2,\n                \"AreLinesIntersecting only works in 2D\");\n  static_assert(std::is_same<typename LineT1::Scalar, typename LineT2::Scalar>::value,\n                \"Type mismatch\");\n  using K = typename LineT1::Scalar;\n  K lA, lB;\n  if (lambda_a == nullptr) lambda_a = &lA;\n  if (lambda_b == nullptr) lambda_b = &lB;\n  const Vector2<K>& R_a = line_a.direction();\n  const Vector2<K>& R_b = line_b.direction();\n  const K ray_cross = CrossProduct(R_b, R_a);\n  // The two lines are parallel, they could still intersect in one or an infinity of points.\n  if (IsAlmostZero(ray_cross)) {\n    const Vector2<K> pt = ClosestPointToLine(line_a, line_b.origin());\n    constexpr K kEpsilonThreshold = MachineEpsilon<K> * 1000.0;\n    if (SquaredDistancePointToLine(line_b, pt) >= kEpsilonThreshold) return false;\n    const K R_a_norm = line_a.direction().squaredNorm();\n    const K R_b_norm = line_b.direction().squaredNorm();\n    *lambda_a = IsAlmostZero(R_a_norm) ? K(0) : (pt - line_a.origin()).dot(R_a) / R_a_norm;\n    *lambda_b = IsAlmostZero(R_b_norm) ? K(0) : (pt - line_b.origin()).dot(R_b) / R_b_norm;\n  } else {\n    const Vector2<K> BA = line_a.origin() - line_b.origin();\n    *lambda_b = CrossProduct(BA, R_a) / ray_cross;\n    *lambda_a = CrossProduct(BA, R_b) / ray_cross;\n  }\n  // Checks that both lambda is within the range of the line.\n  return line_a.clamp(*lambda_a) == *lambda_a && line_b.clamp(*lambda_b) == *lambda_b;\n}\n\ntemplate<typename LineT1, typename LineT2>\nVector2<typename LineT1::Scalar> Intersection(const LineT1& line_a, const LineT2& line_b) {\n  static_assert(std::is_same<typename LineT1::Scalar, typename LineT2::Scalar>(),\n                \"both line need to have the same type\");\n  using K = typename LineT1::Scalar;\n  // Line equation is given by:\n  // line.direction.tangent.dot(x, y) = line.direction.tangent.dot(line.origin)\n  // We have a set of two equations, with two unknowns:\n  // ( d1_y  -d1_x)   (x)   (a)\n  // (            ) * ( ) = ( )\n  // (-d2_y   d2_x)   (y)   (b)\n  const Vector2<K> t1(line_a.direction().y(), -line_a.direction().x());\n  const Vector2<K> t2(-line_b.direction().y(), line_b.direction().x());\n  const K det = t1.dot(line_b.direction());\n  // In this case the line are parallel.\n  if (IsAlmostZero(det)) return line_a.origin();\n\n  Matrix2<K> matrix_inverse;\n  matrix_inverse << t2.y(), -t1.y(), -t2.x(), t1.x();\n  const Vector2<K> result(line_a.origin().dot(t1), line_b.origin().dot(t2));\n  return matrix_inverse * result / det;\n}\n\ntemplate<typename K, int N>\nVector<K, N> ClosestPointToSegment(const Vector<K, N>& a, const Vector<K, N>& b,\n                                   const Vector<K, N>& p) {\n  const Vector<K, N> ab = b - a;\n  const Vector<K, N> ap = p - a;\n  const K l = ab.dot(ap);\n  if (l <= K(0)) {\n    return a;\n  }\n  const K ab_n2 = ab.squaredNorm();\n  if (l >= ab_n2) {\n    return b;\n  }\n  return a + (l / ab_n2) * ab;\n}\n\ntemplate<typename K, int N>\nVector<K, N> ClosestPointToPolyline(const Polyline<K, N>& polyline, const Vector<K, N>& p,\n                                    size_t* segment_index) {\n  ASSERT(polyline.size() > 0, \"Polyline is empty\");\n  if (segment_index) *segment_index = 0;\n  K best_dist = (p - polyline.front()).squaredNorm();\n  Vector<K, N> ret = polyline.front();\n  for (size_t i = 1; i < polyline.size(); i++) {\n    const auto vec = ClosestPointToSegment(polyline[i - 1], polyline[i], p);\n    const K dist = (vec - p).squaredNorm();\n    if (dist < best_dist) {\n      best_dist = dist;\n      ret = vec;\n      if (segment_index) *segment_index = i-1;\n    }\n  }\n  return ret;\n}\n\ntemplate<typename K, int N>\nK DistanceSquaredPointToSegment(const Vector<K, N>& a, const Vector<K, N>& b,\n                                const Vector<K, N>& p) {\n  const Vector<K, N> ab = b - a;\n  const Vector<K, N> ap = p - a;\n  const K l = ab.dot(ap);\n  if (l <= K(0)) {\n    return ap.squaredNorm();\n  }\n  const K ab_n2 = ab.squaredNorm();\n  if (l >= ab_n2) {\n    return (b - p).squaredNorm();\n  }\n  return ap.squaredNorm() - l * l / ab_n2;\n}\n\ntemplate<typename K, int N>\nK DistancePointToSegment(const Vector<K, N>& a, const Vector<K, N>& b, const Vector<K, N>& p) {\n  return std::sqrt(DistanceSquaredPointToSegment(a, b, p));\n}\n\n}  // namespace geometry\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/geometry/n_cuboid.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cmath>\n#include <type_traits>\n#include <utility>\n\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace geometry {\n\n// Represent a Rectangular Cuboid in dimension N (https://en.wikipedia.org/wiki/Cuboid).\ntemplate <typename K, int N>\nclass NCuboid {\n public:\n  // Type of a point.\n  using Vector_t = Vector<K, N>;\n  using Scalar = K;\n  constexpr static int kDimension = N;\n\n  // Creates a NCuboid from the boundaries:\n  // A point will belong in the cuboid if for each dimension dims[i][0] <= pt[i] <= dims[i][1].\n  static NCuboid FromBoundingCuboid(const std::array<Vector2<K>, N>& dims) {\n    Vector_t min, max;\n    for (size_t dim = 0; dim < N; dim++) {\n      min[dim] = dims[dim][0];\n      max[dim] = dims[dim][1];\n    }\n    return NCuboid{min, max};\n  }\n\n  // Creates a NCuboid from the boundaries:\n  // A point will belong in the cuboid if for each dimension min[i] <= pt[i] <= max[i].\n  static NCuboid FromOppositeCorners(const Vector_t& corner1, const Vector_t& corner2) {\n    return NCuboid{corner1, corner2};\n  }\n\n  // Creates a NCuboid from the center and the size of each dimensions.\n  // A point will belong in the cuboid if for each dimension:\n  //    center[i] - sizes[i]/2 <= pt[i] <= center[i] + sizes[i]/2\n  static NCuboid FromSizes(const Vector_t& center, const Vector_t& sizes) {\n    const Vector_t min = center - sizes / K(2);\n    return NCuboid{min, (min + sizes).eval()};\n  }\n\n  // Empty constructor to allow allocation\n  NCuboid() {}\n\n  // Accessor\n  const Vector_t& min() const { return min_; }\n  Vector_t& min() { return min_; }\n  const Vector_t& max() const { return max_; }\n  Vector_t& max() { return max_; }\n\n  // Returns the center point of the cuboid\n  Vector_t center() const { return (min_ + max_) / K(2); }\n\n  // Returns sizes of each dimension of the cuboid\n  Vector_t sizes() const { return max_ - min_; }\n\n  // Returns whether a point is inside the cuboid\n  bool isInside(const Vector_t& pt) const {\n    for (size_t dim = 0; dim < kDimension; dim++) {\n      if (pt[dim] < min_[dim] || pt[dim] > max_[dim]) {\n        return false;\n      }\n    }\n    return true;\n  }\n\n  // Casts to a different type\n  template <typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>\n  NCuboid<S, N> cast() const {\n    return NCuboid<S, N>::FromOppositeCorners(min_.template cast<S>(), max_.template cast<S>());\n  }\n  template <typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>\n  const NCuboid& cast() const {\n    // Nothing to do as the type does not change\n    return *this;\n  }\n\n  // Computes Volume of cuboid. Computes the area if N is 2.\n  Scalar volume() const {\n    Scalar volume = Scalar(1);\n    for (size_t dim = 0; dim < kDimension; dim++) {\n      volume *= max_[dim] - min_[dim];\n    }\n    return volume;\n  }\n\n  // Extends this n_cuboid to contain the other n_cuboid\n  void encapsulate(const NCuboid<K, N>& other) {\n    min_ = min_.cwiseMin(other.min());\n    max_ = max_.cwiseMax(other.max());\n  }\n\n  // Extends this n_cuboid to contain point\n  void encapsulate(const Vector_t& point) {\n    min_ = min_.cwiseMin(point);\n    max_ = max_.cwiseMax(point);\n  }\n\n  // Translates the box\n  void translate(const Vector_t& translation) {\n    min_ += translation;\n    max_ += translation;\n  }\n\n private:\n  // Private constructor to avoid confusion between corners initialization and sizes initialization.\n  // Use FromOppositeCorners or FromSizes to create a line.\n  NCuboid(const Vector_t& corner1, const Vector_t& corner2) {\n    for (size_t dim = 0; dim < kDimension; dim++) {\n      if (corner1[dim] < corner2[dim]) {\n        min_[dim] = corner1[dim];\n        max_[dim] = corner2[dim];\n      } else {\n        min_[dim] = corner2[dim];\n        max_[dim] = corner1[dim];\n      }\n    }\n  }\n  Vector_t min_, max_;\n};\n\ntemplate <typename K>\nusing Rectangle = NCuboid<K, 2>;\ntemplate <typename K>\nusing Box = NCuboid<K, 3>;\n\nusing RectangleD = Rectangle<double>;\nusing RectangleF = Rectangle<float>;\nusing RectangleI = Rectangle<int>;\nusing BoxD = Box<double>;\nusing BoxF = Box<float>;\nusing BoxI = Box<int>;\n\n}  // namespace geometry\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/geometry/n_sphere.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cmath>\n#include <type_traits>\n\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace geometry {\n\n// Represent a sphere in dimension N.\n// Note: the mathematical definition of N-Sphere belongs in a space at N+1 dimensions. The N\n// provided as template argument corresponds to the number of dimension.\n// Therefore a circle is 1-sphere mathematically but NSphere<K, 2> here.\n// Same for the regular sphere: 2-sphere mathematically but NSphere<K, 3> here.\ntemplate <typename K, int N>\nstruct NSphere {\n public:\n  // Type of a point.\n  using Vector_t = Vector<K, N>;\n  using Scalar = K;\n  constexpr static int kDimension = N;\n\n  Vector_t center;\n  Scalar radius;\n\n  // Returns whether a point is inside the sphere\n  bool isInside(const Vector_t& pt) const {\n    return (center - pt).squaredNorm() <= radius * radius;\n  }\n\n  // Casts to a different type\n  template <typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>\n  NSphere<S, N> cast() const {\n    return NSphere<S, N>{center.template cast<S>(), static_cast<S>(radius)};\n  }\n  template<typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>\n  const NSphere& cast() const {\n    // Nothing to do as the type does not change\n    return *this;\n  }\n};\n\ntemplate <typename K>\nusing Circle = NSphere<K, 2>;\ntemplate <typename K>\nusing Sphere = NSphere<K, 3>;\n\nusing CircleD = Circle<double>;\nusing CircleF = Circle<float>;\nusing SphereD = Sphere<double>;\nusing SphereF = Sphere<float>;\n\n}  // namespace geometry\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/geometry/pinhole.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <type_traits>\n\n#include \"gems/core/assert.hpp\"\n#include \"gems/core/epsilon.hpp\"\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace geometry {\n\n// A pinhole camera model describes how points in 3D space are projected onto a 2D image plane.\n//\n// In Isaac we choose the following coordinate frames:\n//   * Camera coordinate, i.e. a point in front of the pinhole which will be projected onto the\n//     image plane, are given as:\n//        X right, Y down, Z forward.\n//   * Image coordinates, i.e. a point in pixel coordinates, are given as:\n//        0: rows, 1: columns\n//\n// The pinhole type is templated on the scalar type used for optical center and focal length.\n// This type must be a floating point type like float or double.\ntemplate <typename K, typename std::enable_if<std::is_floating_point<K>::value, int>::type = 0>\nstruct Pinhole {\n  // The dimensions of the camera image plane in pixels in the order (rows, columns)\n  Vector2i dimensions;\n  // Focal length of the projection (in pixels) in the order (row, column)\n  Vector2<K> focal;\n  // Optical center of the projection (in pixels) in the order (row, column)\n  Vector2<K> center;\n\n  // Creates a pinhole camera model from horizontal field of view with center in the middle or the\n  // image plane.\n  static Pinhole FromHorizontalFieldOfView(const Vector2i& dimensions, K fov_horizontal) {\n    ASSERT(fov_horizontal > K(0), \"Field of view must be greater than 0\");\n    const K focal = static_cast<K>(dimensions[1]) / (K(2) * std::tan(K(0.5) * fov_horizontal));\n    return Pinhole{dimensions, {focal, focal}, K(0.5) * dimensions.cast<K>()};\n  }\n  static Pinhole FromHorizontalFieldOfView(int rows, int cols, K fov_horizontal) {\n    return FromHorizontalFieldOfView({rows, cols}, fov_horizontal);\n  }\n  // Similar to `FromHorizontalFieldOfView`, but the field of view angle indicates the vertical\n  // field of view.\n  static Pinhole FromVerticalFieldOfView(const Vector2i& dimensions, K fov_vertical) {\n    ASSERT(fov_vertical > K(0), \"Field of view must be greater than 0\");\n    const K focal = static_cast<K>(dimensions[0]) / (K(2) * std::tan(K(0.5) * fov_vertical));\n    return Pinhole{dimensions, {focal, focal}, K(0.5) * dimensions.cast<K>()};\n  }\n  static Pinhole FromVerticalFieldOfView(int rows, int cols, K fov_vertical) {\n    return FromVerticalFieldOfView({rows, cols}, fov_vertical);\n  }\n\n  // Projects a 3D point in camera coordinates onto the pixel plane and returns the pixel coordinate\n  // as fractional values.\n  template <typename Derived, typename std::enable_if<\n                                  std::is_same<typename Derived::Scalar, K>::value, int>::type = 0>\n  Vector2<K> project(const Eigen::MatrixBase<Derived>& point) const {\n    ASSERT(!IsAlmostZero(point[2]), \"Invaid point with z almost 0 (%f)\", point[2]);\n    return ((point.template head<2>() / point[2]).array()).reverse() * focal.array() +\n           center.array();\n  }\n  // Projects a 3D point in camera coordinates onto the pixel plane and rouns the pixel coordinate\n  // to the nearest pixel on the image plane (rounding down).\n  template <typename Derived, typename std::enable_if<\n                                  std::is_same<typename Derived::Scalar, K>::value, int>::type = 0>\n  Vector2i projectToInt(const Eigen::MatrixBase<Derived>& point) const {\n    const Vector2<K> pixel = project(point);\n    return {FloorToInt(pixel[0]), FloorToInt(pixel[1])};\n  }\n\n  // Casts a ray through the given pixel coordinate and gives the point on the ray at the given\n  // distance `depth`. The point is given in camera coordinates.\n  template <typename Derived, typename std::enable_if<\n                                  std::is_same<typename Derived::Scalar, K>::value, int>::type = 0>\n  Vector3<K> unproject(const Eigen::MatrixBase<Derived>& pixel, K depth) const {\n    const Vector2<K> a = ((pixel - center).array() / focal.array()) * depth;\n    return {a[1], a[0], depth};\n  }\n  // Special version for integer-coordinate pixels. We assume that pixel rays go through the center\n  // of the pixel.\n  template <\n      typename Derived,\n      typename std::enable_if<std::is_same<typename Derived::Scalar, int>::value, int>::type = 0>\n  Vector3<K> unproject(const Eigen::MatrixBase<Derived>& pixel, K depth) const {\n    return unproject(pixel.template cast<K>() + Vector2<K>{K(0.5), K(0.5)}, depth);\n  }\n  Vector3<K> unproject(int px, int py, K depth) const {\n    return unproject(Vector2i{px, py}.template cast<K>() + Vector2<K>{K(0.5), K(0.5)}, depth);\n  }\n\n  // Computes the area of a pixel at given depth assuming a planar patch\n  K pixelAreaAtDepth(K depth) const { return depth * depth / (focal[0] * focal[1]); }\n\n  // Scale the pinhole. The scale factor is different for rows and cols\n  Pinhole<K> scale(const Vector2<K>& scale_factor) const {\n    ASSERT((scale_factor.array() >= 0).all(), \"Invalid scale factor\");\n    return { (dimensions.template cast<K>().cwiseProduct(scale_factor)).template cast<int>(),\n             focal.cwiseProduct(scale_factor),\n             center.cwiseProduct(scale_factor)};\n  }\n\n  // Scale the pinhole\n  Pinhole<K> scale(K scale_factor) const {\n    ASSERT(scale_factor > K(0), \"Invalid scale factor\");\n    return scale(Vector2<K>{scale_factor, scale_factor});\n  }\n\n  // First crop the intrinsics and then scale them\n  Pinhole<K> cropAndScale(const Vector2i& crop_start, const Vector2i& crop_size,\n                          const Vector2i& scaled_size) const {\n    ASSERT((crop_start.array() >= 0).all(), \"Invalid crop start\");\n    ASSERT((crop_start.array() < dimensions.array()).all(), \"Invalid crop start\");\n    ASSERT((crop_size.array() > 0).all(), \"Invalid crop size\");\n    const Pinhole<K> cropped_pinhole{crop_size, focal, center - crop_start.cast<K>()};\n    return cropped_pinhole.scale(scaled_size.cast<K>().cwiseQuotient(crop_size.cast<K>()));\n  }\n\n  // Computes the field of view angles (in radians) in the order (vertical, horizontal)\n  Vector2<K> computeFieldOfView() const {\n    return { K(2) * std::atan2(K(0.5) * static_cast<K>(dimensions[0] - 1), focal[0]),\n             K(2) * std::atan2(K(0.5) * static_cast<K>(dimensions[1] - 1), focal[1]) };\n  }\n\n  // Returns the 3x3 projection matrix\n  Matrix3<K> matrix() const {\n    Matrix3<K> mat;\n    mat << K(0), focal[0], center[0], focal[1], K(0), center[1], K(0), K(0), K(1);\n    return mat;\n  }\n\n  // Casts the pinhole to a different numeric type. For example from double to float.\n  template <typename S>\n  Pinhole<S> cast() const {\n    return {dimensions, focal.template cast<S>(), center.template cast<S>()};\n  }\n};\n\n// Typedef for pinhole with using 64-bit floating points\nusing PinholeD = Pinhole<double>;\n\n}  // namespace geometry\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/geometry/plane.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"Eigen/Eigen\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace geometry {\n\n// A plane in N dimensions represented by a normal vector and an offset.\ntemplate<typename K, int N>\nusing Hyperplane = class Eigen::Hyperplane<K, N>;\n\ntemplate<typename K>\nusing Plane = Hyperplane<K, 3>;\nusing PlaneF = Plane<float>;\nusing PlaneD = Plane<double>;\n\n}  // namespace geometry\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/geometry/polygon.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <algorithm>\n#include <cmath>\n#include <limits>\n#include <random>\n#include <type_traits>\n#include <utility>\n#include <vector>\n\n#include \"gems/core/math/types.hpp\"\n#include \"gems/geometry/line_segment.hpp\"\n#include \"gems/geometry/line_utils.hpp\"\n#include \"gems/geometry/n_cuboid.hpp\"\n#include \"gems/geometry/n_sphere.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace geometry {\n\n// A polygon in 2D, the last point and first point are connected\ntemplate <typename K>\nstruct Polygon2 {\n  // Type of a point.\n  using Vector_t = Vector2<K>;\n  using Scalar = K;\n\n  std::vector<Vector_t> points;\n\n  // Returns whether a point is inside the polygon\n  // Note that this method is not accurate if the point lies too close to one edge, Especially if\n  // the point is really close to the edge or of a vertex, it might return true.\n  // This function used the strategy of counting the intersection between the segments of the\n  // polygon and a virtual segment from the `pt` to a point outside of the polygon. While fast, this\n  // method does not work well when the virtual segment is too close to one of the vertices. In this\n  // case it fallbacks to the slower implementation below.\n  bool isInside(const Vector_t& pt) const {\n    if (points.empty()) return false;\n    // Select a point outside of the polygon (max(x) + 1, max(y) + 2).\n    // Picking a different constant for x and y helps reduce the chance the point is aligned with\n    // one vertex.\n    Vector_t max = points.front();\n    for (const auto& point : points) {\n      max.x() = std::max(max.x(), point.x() + K(1));\n      max.y() = std::max(max.y(), point.y() + K(2));\n    }\n    // If the maximum value of x+1 is less or equal to our point, then we are obviously outside.\n    if (max.x() <= pt.x()) return false;\n    if (max.y() <= pt.y()) return false;\n    const LineSegment<K, 2> seg(pt, max);\n    // Count how many segment it intersects, if it's an odd number then the point is inside or on\n    // one edge.\n    int counter = 0;\n    for (size_t ix = 0; ix < points.size(); ix++) {\n      const LineSegment<K, 2> edge(points[ix], points[(ix + 1) % points.size()]);\n      K lambda;\n      if (AreLinesIntersecting(seg, edge, nullptr, &lambda)) {\n        counter++;\n        // If our lines are too close, we fallback to the slow but accurate method.\n        if (IsAlmostZero(lambda) || IsAlmostOne(lambda)) {\n          return slowIsInside(pt);\n        }\n      }\n    }\n    return counter % 2 == 1;\n  }\n\n  // Returns whether a point is inside the polygon\n  // This function is a bit slow and should not be called in critical path too often.\n  // This function used the strategy to measure the sum of the field of view the segment are\n  // observed. It was the most accurate solution, however it requires using trigonometric functions,\n  // which are quite slow.\n  bool slowIsInside(const Vector_t& pt) const {\n    if (points.empty()) return false;\n    K sum_angle = K(0);\n    Vector_t delta = points.back() - pt;\n    // If delta == 0, it means pt is the same as one of the vertex.\n    if (IsAlmostZero(delta.squaredNorm())) return true;\n    K last_angle = std::atan2(delta.y(), delta.x());\n    for (size_t ix = 0; ix < points.size(); ix++) {\n      delta = points[ix] - pt;\n      // If delta == 0, it means pt is the same as one of the vertex.\n      if (IsAlmostZero(delta.squaredNorm())) return true;\n      const K angle = std::atan2(delta.y(), delta.x());\n      const K delta_angle = DeltaAngle(angle, last_angle);\n      // If delta == +/- pi, it means pt is on one of the segment.\n      if (IsAlmostEqualRelative(std::abs(delta_angle), Pi<K>)) {\n        return true;\n      }\n      sum_angle += delta_angle;\n      last_angle = angle;\n    }\n    return std::abs(sum_angle) > Pi<K>;\n  }\n\n  // Returns the distance of a point from the polygon edges\n  K distance(const Vector_t& pt, Vector_t* grad = nullptr) const {\n    K squared_dist = std::numeric_limits<K>::max();\n    for (size_t ix = 0; ix < points.size(); ix++) {\n      const LineSegment<K, 2> edge(points[ix], points[(ix + 1) % points.size()]);\n      const Vector_t closest = ClosestPointToLine(edge, pt);\n      const K dist = (pt - closest).squaredNorm();\n      if (dist < squared_dist) {\n        squared_dist = dist;\n        if (grad) *grad = pt - closest;\n      }\n    }\n    if (grad) grad->normalize();\n    return std::sqrt(squared_dist);\n  }\n\n  // Returns the signed distance of a point from the polygon (negative means inside the polygon)\n  K signedDistance(const Vector_t& pt, Vector_t* grad = nullptr) const {\n    const K dist = distance(pt, grad);\n    // If the point is inside the polygon, we need to revert the direction of the gradient.\n    if (isInside(pt)) {\n      if (grad) *grad = -(*grad);\n      return -dist;\n    }\n    return dist;\n  }\n\n  // Returns the signed area of the polygon\n  // https://mathworld.wolfram.com/PolygonArea.html\n  K signedArea() const {\n    K area = K(0);\n    for (size_t curr = 0, prev = points.size()-1; curr < points.size(); curr++) {\n      area += CrossProduct(points[prev], points[curr]);\n      prev = curr;\n    }\n    return area / K(2);\n  }\n\n  // Returns the centroid of the polygon\n  // https://en.wikipedia.org/wiki/Centroid#Of_a_polygon\n  Vector_t centroid() const {\n    if (points.size() < 3) {\n      if (points.empty()) return Vector_t::Zero();\n      return (points.front() + points.back()) / K(2);\n    }\n    Vector_t centroid = Vector_t::Zero();\n    for (size_t curr = 0, prev = points.size()-1; curr < points.size(); curr++) {\n      const K cross_product = CrossProduct(points[prev], points[curr]);\n      centroid += (points[prev] + points[curr]) * cross_product;\n      prev = curr;\n    }\n    return centroid / (K(6) * signedArea());\n  }\n\n  // Returns the minimum circle that contains the polygon.\n  Circle<K> boundingCircle() const {\n    std::vector<size_t> indices(points.size());\n    std::vector<size_t> border;\n    for (size_t idx = 0; idx < points.size(); idx++) {\n      indices[idx] = idx;\n    }\n    // Random order to improve performance\n    std::random_device rd;\n    std::mt19937 gen(rd());\n    std::shuffle(indices.begin(), indices.end(),  gen);\n    // Compute the center when the number of vertices is less or equal to 3\n    auto trivialCenter = [&](const std::vector<size_t>& indices) -> Circle<K> {\n      switch (indices.size()) {\n        // Edge case for empty polygon, there is no real good answer, but any circle of radius 0\n        // is technically a good answer.\n        case 0: return {Vector_t::Zero(), K(0)};\n        // If the polygon has a single point, then it's the center of the circle of radius 0.\n        case 1: return {points[indices[0]], K(0)};\n        // For two points, the center is the center of the segment, and the radius can be computed\n        // using one of the extremity.\n        case 2: {\n          const Vector_t delta = (points[indices[1]] - points[indices[0]]) / K(2);\n          return {points[indices[0]] + delta, delta.norm()};\n        }\n        // There is a formula for a triangle:\n        // https://en.wikipedia.org/wiki/Circumscribed_circle#Cartesian_coordinates_2\n        case 3: {\n          const Vector_t& A = points[indices[0]];\n          const Vector_t& B = points[indices[1]] - A;\n          const Vector_t& C = points[indices[2]] - A;\n          const K b_squared_norm = B.squaredNorm();\n          const K c_squared_norm = C.squaredNorm();\n          const K d = K(2) * CrossProduct(B, C);\n          const Vector_t center = Vector_t(\n              b_squared_norm * C.y() - c_squared_norm * B.y(),\n              c_squared_norm * B.x() - b_squared_norm * C.x()) / d;\n          return {A + center, center.norm()};\n        }\n        default:\n          PANIC(\"Invalid size: %zd\", indices.size());\n      }\n      return {Vector_t::Zero(), K(0)};\n    };\n    // Implementation of the welzl algorithm:\n    // https://en.wikipedia.org/wiki/Smallest-circle_problem#Welzl's_algorithm\n    auto welzl = [&](auto welzl, std::vector<size_t>& indices, std::vector<size_t>& border) {\n      if (indices.empty() || border.size() == 3) return trivialCenter(border);\n      size_t p = indices.back();\n      indices.pop_back();\n      auto circle = welzl(welzl, indices, border);\n      if (circle.isInside(points[p])) {\n        indices.push_back(p);\n        return circle;\n      }\n      border.push_back(p);\n      circle = welzl(welzl, indices, border);\n      border.pop_back();\n      indices.push_back(p);\n      return circle;\n    };\n    return welzl(welzl, indices, border);\n  }\n\n  // Returns whether both polygons intersect. If one polygon is fully inside the other, then the\n  // function returns false.\n  bool intersect(const Polygon2& polygon) const {\n    return intersectImpl<1>(polygon.points);\n  }\n\n  // Returns whether this polygon intersect with the polyline.\n  bool intersect(const Polyline<K, 2>& polyline) const {\n    return intersectImpl<0>(polyline);\n  }\n\n  // Returns the distance between two polygons.\n  // If one polygon is stricly included in the other, the distance will be negative.\n  K distance(const Polygon2& polygon) const {\n    const K squared_dist = squaredDistanceImpl<1>(polygon.points);\n    if (squared_dist == K(0)) return K(0);\n    const auto bounding_box_a = getBoundingBox();\n    const auto bounding_box_b = polygon.getBoundingBox();\n    // If the bounding box A encapsulates B, then B might be inside A\n    if (bounding_box_a.min().x() < bounding_box_b.min().x() &&\n        bounding_box_b.max().x() < bounding_box_a.max().x() &&\n        bounding_box_a.min().y() < bounding_box_b.min().y() &&\n        bounding_box_b.max().y() < bounding_box_a.max().y()) {\n      return isInside(polygon.points.front()) ? -std::sqrt(squared_dist) : std::sqrt(squared_dist);\n    }\n\n    // If the bounding box B encapsulates A, then A might be inside B\n    if (bounding_box_b.min().x() < bounding_box_a.min().x() &&\n        bounding_box_a.max().x() < bounding_box_b.max().x() &&\n        bounding_box_b.min().y() < bounding_box_a.min().y() &&\n        bounding_box_a.max().y() < bounding_box_b.max().y()) {\n      return polygon.isInside(points.front()) ? -std::sqrt(squared_dist) : std::sqrt(squared_dist);\n    }\n\n    return std::sqrt(squared_dist);\n  }\n\n  // Returns the distance between this polygon and a polyline.\n  // If the polyline is stricly included in this polygon, the distance will be negative.\n  K distance(const Polyline<K, 2>& polyline) const {\n    const K squared_dist = squaredDistanceImpl<0>(polyline);\n    if (squared_dist == K(0)) return K(0);\n    return isInside(polyline.front()) ? -std::sqrt(squared_dist) : std::sqrt(squared_dist);\n  }\n\n  // Returns the bounding box, aligned with the axis, that contain this polygon.\n  Rectangle<K> getBoundingBox() const {\n    if (points.empty()) {\n      return Rectangle<K>::FromOppositeCorners(Vector2<K>::Zero(), Vector2<K>::Zero());\n    }\n    Vector2<K> min = points.front();\n    Vector2<K> max = points.front();\n    for (const auto& pt : points) {\n      min = min.cwiseMin(pt);\n      max = max.cwiseMax(pt);\n    }\n    return Rectangle<K>::FromOppositeCorners(min, max);\n  }\n\n  // Casts to a different type\n  template <typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>\n  Polygon2<S> cast() const {\n    std::vector<Vector2<S>> pts;\n    pts.reserve(points.size());\n    for (const auto& pt : points) {\n      pts.emplace_back(pt.template cast<S>());\n    }\n    return Polygon2<S>{std::move(pts)};\n  }\n  template<typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>\n  const Polygon2& cast() const {\n    // Nothing to do as the type does not change\n    return *this;\n  }\n\n private:\n  // Implementation of the squaredDistance using a polyline.\n  // If loop is 1, then we close the polyline (making it a polygon).\n  template<int Loop>\n  K squaredDistanceImpl(const Polyline<K, 2>& polyline) const {\n    static_assert(Loop >= 0 && Loop <= 1, \"Loop must be 0 or 1\");\n    // Check for an intersection\n    if (intersectImpl<Loop>(polyline)) return K(0);\n    // Find the closest point of either polygon to the other one.\n    K squared_dist = std::numeric_limits<K>::max();\n    // For all the points of this polygon, we check the distance to all the segments of the other\n    // polygon.\n    for (size_t ax = 0; ax < points.size(); ax++) {\n      const Vector2<K>& a = points[ax];\n      const Vector2<K>& b = points[(ax + 1) % points.size()];\n      for (size_t bx = 0; bx < polyline.size(); bx++) {\n        squared_dist = std::min(squared_dist, DistanceSquaredPointToSegment(a, b, polyline[bx]));\n      }\n    }\n    // For all the points of the other polygon, we check the distance to all the segments of this\n    // polygon.\n    for (size_t ax = 1; ax < polyline.size() + Loop; ax++) {\n      const Vector2<K>& a = polyline[ax - 1];\n      const Vector2<K>& b = polyline[ax % polyline.size()];\n      for (size_t bx = 0; bx < points.size(); bx++) {\n        squared_dist = std::min(squared_dist, DistanceSquaredPointToSegment(a, b, points[bx]));\n      }\n    }\n    return squared_dist;\n  }\n\n  // Implementation of the intersect function with a polyline.\n  // If loop is 1, then we close the polyline (making it a polygon).\n  template<int Loop>\n  K intersectImpl(const Polyline<K, 2>& polyline) const {\n    static_assert(Loop >= 0 && Loop <= 1, \"Loop must be 0 or 1\");\n    const auto bounding_box_a = getBoundingBox();\n    const auto bounding_box_b = Polygon2{polyline}.getBoundingBox();\n    if (bounding_box_a.min().x() > bounding_box_b.max().x() ||\n        bounding_box_b.min().x() > bounding_box_a.max().x() ||\n        bounding_box_a.min().y() > bounding_box_b.max().y() ||\n        bounding_box_b.min().y() > bounding_box_a.max().y()) {\n      return false;\n    }\n    for (size_t ax = 0; ax < points.size(); ax++) {\n      const LineSegment<K, 2> a(points[ax], points[(ax + 1) % points.size()]);\n      for (size_t bx = 1; bx < polyline.size() + Loop; bx++) {\n        const LineSegment<K, 2> b(polyline[bx - 1], polyline[bx % polyline.size()]);\n        if (AreLinesIntersecting(a, b)) return true;\n      }\n    }\n    return false;\n  }\n};\n\n// A polygon in 3D, the last point and first point are connected\ntemplate <typename K>\nstruct Polygon3 {\n  // Type of a point.\n  using Vector_t = Vector3<K>;\n  using Scalar = K;\n\n  std::vector<Vector_t> points;\n};\n\nusing Polygon2D = Polygon2<double>;\nusing Polygon2F = Polygon2<float>;\nusing Polygon3D = Polygon3<double>;\nusing Polygon3F = Polygon3<float>;\n\n}  // namespace geometry\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/geometry/polyline.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <vector>\n\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace geometry {\n\n// A polyline is currently simply a list of points\ntemplate <typename K, int N>\nusing Polyline = std::vector<Vector<K, N>>;\n\nusing Polyline2d = Polyline<double, 2>;\nusing Polyline2f = Polyline<float, 2>;\nusing Polyline3d = Polyline<double, 3>;\nusing Polyline3f = Polyline<float, 3>;\n\n}  // namespace geometry\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/geometry/types.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/geometry/line.hpp\"\n#include \"gems/geometry/line_segment.hpp\"\n#include \"gems/geometry/n_cuboid.hpp\"\n#include \"gems/geometry/n_sphere.hpp\"\n#include \"gems/geometry/plane.hpp\"\n#include \"gems/geometry/polygon.hpp\"\n#include \"gems/geometry/polyline.hpp\"\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/gxf_helpers/expected_macro_abstract.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n#include <type_traits>\n#include <utility>\n\n#include \"gems/core/logger.hpp\"\n\n// This file defines macro helpers that can be used to check the result of an expression. Note that\n// this is an abstract implementation that has no knowledge of the result types it can be used with.\n// In order to use the macro it has to be configured first for a specific result type.\n//\n// Taxonomy:\n// - Result Type: The union of both status and unwrappable types.\n// - Status Type: A type that is used as the return type of an expression and holds the return\n//                status of the expression, eg. bool, int, enum etc.\n// - Unwrappable Type: A type that is used as the return type of an expression and holds both the\n//                     return status and the return value of the expression, eg. std::optional,\n//                     nvidia::Expected etc.\n\n// Concatenates its two arguments.\n#define EXPECTED_MACRO_INTERNAL_CONCAT(a, b) EXPECTED_MACRO_INTERNAL_CONCAT_IMPL(a, b)\n#define EXPECTED_MACRO_INTERNAL_CONCAT_IMPL(a, b) a##b\n\n// Converts its argument to a string at compile time.\n#define EXPECTED_MACRO_INTERNAL_TO_STRING(x) EXPECTED_MACRO_INTERNAL_TO_STRING_IMPL(x)\n#define EXPECTED_MACRO_INTERNAL_TO_STRING_IMPL(x) #x\n\n// Gets the current location in the source code in the format \"file:line\".\n#define EXPECTED_MACRO_INTERNAL_FILE_LINE() __FILE__ \":\" EXPECTED_MACRO_INTERNAL_TO_STRING(__LINE__)\n\n// Helper to support logging a default message and an optional custom message.\n#define EXPECTED_MACRO_INTERNAL_LOG_IN_EXPECT_MACRO(expression_result, expression_string, ...) \\\n  nvidia::expected_macro::LogHelper(                                                           \\\n      __FILE__, __LINE__, expression_result, expression_string, ##__VA_ARGS__);\n\n// Helper to check the return type of the expression used in RETURN_IF_ERROR.\n#define EXPECTED_MACRO_INTERNAL_CHECK_EXPRESSION_IS_RESULT(expression_result) \\\n  static_assert(                                                                                   \\\n      nvidia::expected_macro::config::IsStatus<decltype(expression_result)>::value ||              \\\n      nvidia::expected_macro::config::IsUnwrappable<decltype(expression_result)>::value ,          \\\n      EXPECTED_MACRO_INTERNAL_FILE_LINE()                                                          \\\n          \": RETURN_IF_ERROR can only be used with expressions that return a result type. You \"    \\\n          \"can register a type as a result type unwrappable with one of\"                           \\\n          \"`template <> struct IsUnwrappable<MyType> : std::true_type {};` or \"                    \\\n          \"`template <> struct IsStatus<MyType> : std::true_type {}`. \");\n\n// Helper to check the return type of the expression used in UNWRAP_OR_RETURN.\n#define EXPECTED_MACRO_INTERNAL_CHECK_EXPRESSION_IS_UNWRAPPABLE(expression_result) \\\n  static_assert(                                                                                   \\\n      nvidia::expected_macro::config::IsUnwrappable<decltype(expression_result)>::value,           \\\n      EXPECTED_MACRO_INTERNAL_FILE_LINE()                                                          \\\n          \": UNWRAP_OR_RETURN can only be used with expressions that return an unwrappable type. \" \\\n          \"You can register a type as being unwrappable with \"                                     \\\n          \"`template <> struct IsUnwrappable<MyType> : std::true_type {};`. For expressions \"      \\\n          \"returning a status instead of an unwrappable use `RETURN_IF_ERROR` instead.\");\n\n// Evaluates an expression that returns a result type. If the returned result contains an error it\n// returns the error. This macro can only be used in functions that also return a result type.\n//\n// Per default the macro already creates an error message that includes the evaluated expression. If\n// needed an optional string can be passed that will be appended to the default error message. It is\n// also possible to use format specifiers to customize the string.\n//\n// It is also possible to pass the Severity used for logging as an additional argument. This is\n// required if using a custom error message.\n//\n// Example:\n// Expected<void> DoSomething();\n// Expected<void> DoAnotherThing();\n//\n// Expected<void> foo(){\n//   RETURN_IF_ERROR(DoSomething());\n//   RETURN_IF_ERROR(DoAnotherThing(), Severity::WARNING);\n//   RETURN_IF_ERROR(DoAnotherThing(), Severity::WARNING, \"Custom error message.\");\n// }\n#define RETURN_IF_ERROR(expression, ...)                                                    \\\n  do {                                                                                      \\\n    auto maybe_result = (expression);                                                       \\\n    EXPECTED_MACRO_INTERNAL_CHECK_EXPRESSION_IS_RESULT(maybe_result)                        \\\n    if (!nvidia::expected_macro::config::IsValid(maybe_result)) {                           \\\n      EXPECTED_MACRO_INTERNAL_LOG_IN_EXPECT_MACRO(maybe_result, #expression, ##__VA_ARGS__) \\\n      return nvidia::expected_macro::ProxyFactory::FromStatusOrUnwrappable(maybe_result);   \\\n    }                                                                                       \\\n  } while (0)\n\n// Evaluates an expression that returns a type that can be casted to a boolean. If the expression is\n// false it returns an error. This macro can only be used in functions returning a result type.\n//\n// The difference to RETURN_IF_ERROR is that this macro will always cast the expression to a\n// boolean. Thus if your expression returns a status that can be more than just true/false you will\n// loose the additional information. In those cases RETURN_IF_ERROR is preferred. Use this macro\n// only to check preconditions like a < b, or c != nullptr.\n//\n// Per default the macro already creates an error message that includes the evaluated expression. If\n// needed an optional string can be passed that will be appended to the default error message. It is\n// also possible to use format specifiers to customize the string.\n//\n// It is also possible to pass the Severity used for logging as an additional argument. This is\n// required if using a custom error message.\n//\n// Example:\n// Expected<void> foo(){\n//   RETURN_IF_FALSE(1 > 2);\n//   RETURN_IF_FALSE(1 > 2, Severity::WARNING);\n//   RETURN_IF_FALSE(1 > 2, Severity::WARNING, \"1 is not bigger than 2\");\n// }\n#define RETURN_IF_FALSE(expression, ...)                                              \\\n  do {                                                                                \\\n    const bool result = static_cast<bool>(expression);                                \\\n    if (!result) {                                                                    \\\n      EXPECTED_MACRO_INTERNAL_LOG_IN_EXPECT_MACRO(result, #expression, ##__VA_ARGS__) \\\n      return nvidia::expected_macro::ProxyFactory::FromStatusOrUnwrappable(result);   \\\n    }                                                                                 \\\n  } while (0)\n\n// Evaluates an expression that returns an unwrappable type. If the returned type contains an error\n// it returns the error, else it unwraps the value. This macro can only be used in functions\n// returning a result type.\n//\n// Per default the macro already creates an error message that includes the evaluated expression. If\n// needed an optional string can be passed that will be appended to the default error message. It is\n// also possible to use format specifiers to customize the string.\n//\n// It is also possible to pass the Severity used for logging as an additional argument.\n//\n// Note that this macro uses expression-statements (i.e. the ({ }) surrounding the macro) which are\n// a non-standard functionality. However they are present in almost all compilers. We currently only\n// know of MSVC that does not support this.\n//\n// Example:\n// Expected<std::string> GetString();\n// Expected<std::string> GetAnotherString();\n//\n// Expected<int> CountCombinedStringLength(){\n//   const std::string str1 = UNWRAP_OR_RETURN(GetString());\n//   std::string str2;\n//   str2 = UNWRAP_OR_RETURN(GetAnotherString(), \"This should not fail. Str1 has value %s.\",\n//       str1.c_str());\n//   const std::string str3 = UNWRAP_OR_RETURN(GetAnotherString(), Severity::WARNING);\n//   const std::string str4 = UNWRAP_OR_RETURN(GetAnotherString(), Severity::WARNING,\n//       \"Custom error message\");\n//   return str1.size() + str2.size() + str3.size() + str4.size();\n// }\n#define UNWRAP_OR_RETURN(expression, ...)                                                   \\\n  ({                                                                                        \\\n    auto maybe_result = (expression);                                                       \\\n    EXPECTED_MACRO_INTERNAL_CHECK_EXPRESSION_IS_UNWRAPPABLE(maybe_result)                   \\\n    if (!nvidia::expected_macro::config::IsValid(maybe_result)) {                           \\\n      EXPECTED_MACRO_INTERNAL_LOG_IN_EXPECT_MACRO(maybe_result, #expression, ##__VA_ARGS__) \\\n      return nvidia::expected_macro::ProxyFactory::FromUnwrappable(maybe_result);           \\\n    }                                                                                       \\\n    std::move(maybe_result.value());                                                        \\\n  })\n\n// All functions in the following namespace have to be specialized for a given result type in order\n// to enable the macro for the type.\nnamespace nvidia::expected_macro::config {\n\n// Type trait to check if a type can be used as a status.\ntemplate <typename /*Type*/>\nstruct IsStatus : std::false_type {};\n\n// Type trait to check if a type can be used as an unwrappable.\ntemplate <typename /*Type*/>\nstruct IsUnwrappable : std::false_type {};\n\n// The value that represents the default error for a given status type.\ntemplate <typename Status>\nconstexpr Status DefaultError();\n\n// Check if a result types is valid.\ntemplate <typename Result>\nconstexpr bool IsValid(const Result& result) {\n  return static_cast<bool>(result);\n}\n\n// Helper struct to get the status type of a result. This is needed for the StatusValue helper below\n// to automatically infer the return type.\n//\n// Per default we use the same type as the\n// result, which holds for all status types. Thus this only has to be specialized for unwrappable\n// types.\ntemplate <typename Result>\nstruct StatusType {\n  using Type = Result;\n};\n\n// Helper function to get the status of a result. We have to use a static member function instead of\n// a free function because we want the possibility for a user to only provide a partial template\n// specialization of this.\ntemplate <typename Result>\nstruct StatusValue {\n  static constexpr typename StatusType<Result>::Type Get(const Result& result) { return result; }\n};\n\n// Get the name corresponding to a status, s.t. it can be printed.\ntemplate <typename Status, typename = void>\nstruct StatusName {\n  static std::string Get(Status status);\n};\n\n// Get the invalid unwrappable corresponding to an unwrappable. Eg. std::nullopt for std::optional.\ntemplate <typename Unwrappable, typename Status, typename = void>\nstruct InvalidUnwrappable {\n  static Unwrappable Get(Status status);\n};\n\n}  // namespace nvidia::expected_macro::config\n\nnamespace nvidia::expected_macro {\n\n// The default severity used for logging.\nconstexpr ::nvidia::isaac::logger::Severity kDefaultSeverity =\n    ::nvidia::isaac::logger::Severity::ERROR;\n\n// Syntactic sugar to get the status of a result without having to explicitly specify all template\n// arguments.\ntemplate <typename Result>\nconstexpr typename config::StatusType<Result>::Type GetStatus(const Result& result) {\n  return config::StatusValue<Result>::Get(result);\n}\n\n// Syntactic sugar to get the status name without having to explicitly specify all template\n// arguments.\ntemplate <typename Status>\nstd::string GetStatusName(Status status) {\n  return config::StatusName<Status>::Get(status);\n}\n\n// Syntactic sugar to get the invalid unwrappable without having to explicitly specify all template\n// arguments.\ntemplate <typename Unwrappable, typename Status>\nUnwrappable GetInvalidUnwrappable(Status status) {\n  return config::InvalidUnwrappable<Unwrappable, Status>::Get(status);\n}\n\ntemplate <typename Status>\nclass ResultProxy;\n\n// ProxyFactory to create an ResultProxy. We create a separate class for these functions\n// because ResultProxy is templated and thus could not be used without explicitly\n// specifying the template.\nclass ProxyFactory {\n public:\n  // Constructs the proxy from an unwrappable type. The unwrappable has to be in an error state. We\n  // do not check this because the macro should have already done this check. We use static\n  // methods instead of constructors to explicitly disallow construction from certain types in\n  // different situations.\n  template <\n      typename Unwrappable, typename = std::enable_if_t<config::IsUnwrappable<Unwrappable>::value>>\n  static ResultProxy<typename config::StatusType<Unwrappable>::Type> FromUnwrappable(\n      const Unwrappable& unwrappable) {\n    return ResultProxy(GetStatus(unwrappable));\n  }\n\n  // Constructs the proxy from a status type. The status has to be in an error state. We do not\n  // check this because the macro should have already done this check. We use static methods instead\n  // of constructors to explicitly disallow construction from certain types in different situations.\n  template <typename Result>\n  static ResultProxy<typename config::StatusType<Result>::Type> FromStatusOrUnwrappable(\n      const Result& result) {\n    return ResultProxy(GetStatus(result));\n  }\n};\n\n// A proxy class to allow implicit casting of one result type to another result type. Thus in a\n// function that returns a result type one can simply return this proxy and then it will implicitly\n// cast to the appropriate return type.\ntemplate <typename Status>\nclass ResultProxy {\n public:\n  // Casts the proxy to a status or unwrappable type. Note that this cast is not allowed to be\n  // explicit.\n  template <\n      typename T,\n      typename = std::enable_if_t<config::IsStatus<T>::value || config::IsUnwrappable<T>::value>>\n  constexpr operator T() const {\n    if constexpr (config::IsStatus<T>::value) {\n      return castToStatus<T>();\n    } else {\n      return castToInvalidUnwrappable<T>();\n    }\n  }\n\n private:\n  // Constructs the proxy from a status type. The status has to be in an error state.\n  // We do not check for this because we rely on the macro already having done that check.\n  constexpr explicit ResultProxy(Status error) : error_(error) {}\n\n  // Casts the proxy to a status type. If the error type is not equal to the proxy's error type,\n  // a default error is used.\n  template <typename OtherStatus>\n  constexpr OtherStatus castToStatus() const {\n    static_assert(config::IsStatus<OtherStatus>::value, \"OtherStatus has to be a status type.\");\n    if constexpr (std::is_same_v<OtherStatus, Status>) {\n      return error_;\n    } else {\n      return config::DefaultError<OtherStatus>();\n    }\n  }\n\n  // Casts the proxy to an invalid unwrappable type (eg. std::nullopt or nvidia::Unexpected).\n  template <typename Unwrappable>\n  constexpr Unwrappable castToInvalidUnwrappable() const {\n    static_assert(\n        config::IsUnwrappable<Unwrappable>::value, \"Unwrappable has to be a status type.\");\n    using OtherStatus = typename config::StatusType<Unwrappable>::Type;\n    return GetInvalidUnwrappable<Unwrappable>(castToStatus<OtherStatus>());\n  }\n\n  Status error_;\n  // TODO: enable the following.\n  // static_assert(config::IsStatus<Status>::value, \"Status has to be a status type.\");\n\n  friend ProxyFactory;\n};\n\n// Helper function for the logging in the above macros. This version should be used when the user\n// also specifies the logging severity. The variadic arguments can be used to do string\n// interpolation in the custom_text variable.\ntemplate <typename ExpressionResult, typename... Args>\nvoid LogHelper(\n    const char* file, int line, const ExpressionResult& expression_result,\n    const std::string& expression_string, ::nvidia::isaac::logger::Severity severity,\n    const std::string& custom_txt = \"\", Args... args) {\n  const auto error = GetStatus(expression_result);\n  const std::string text = \"Expression '\" + expression_string + \"' failed with error '\" +\n                           GetStatusName(error) + \"'. \" + custom_txt;\n\n  // GCC is not able to do format security validation when the string\n  // is coming from a variadic template, even if the string is\n  // originally a char* ignore this warning until a more recent GCC\n  // version fixes this behavior\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wformat-security\"\n  ::nvidia::isaac::logger::Log(file, line, severity, text.c_str(), args...);\n#pragma GCC diagnostic pop\n}\n\n// Overload of the LogHelper above. This version does not take the severity as an argument and used\n// the default severity instead.\ntemplate <typename ExpressionResult, typename... Args>\nvoid LogHelper(\n    const char* file, int line, const ExpressionResult& expression_result,\n    const std::string& expression_string, const std::string& custom_text = \"\", Args... args) {\n  LogHelper(\n      file, line, expression_result, expression_string, kDefaultSeverity, custom_text, args...);\n}\n\n}  // namespace nvidia::expected_macro\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/gxf_helpers/expected_macro_common.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <optional>\n#include <string>\n\n#include \"gems/gxf_helpers/expected_macro_abstract.hpp\"\n#include \"magic_enum.hpp\"  // NOLINT(build/include)\n\n// This file contains the configuration to use the expected macro with different return types like\n// bool, int, std::optional (and in the future std::expected).\n\n//////////////////////////////////////////\n// Configuration for using bool as status:\n//////////////////////////////////////////\nnamespace nvidia::expected_macro::config {\ntemplate <>\nstruct IsStatus<bool> : std::true_type {};\n\ntemplate <>\nconstexpr bool DefaultError<bool>() {\n  return false;\n}\n\ntemplate <>\nstruct StatusName<bool> {\n  static std::string Get(bool status) { return status ? \"Success\" : \"Failure\"; }\n};\n\n/////////////////////////////////////////\n// Configuration for using int as status:\n/////////////////////////////////////////\ntemplate <>\nstruct IsStatus<int> : std::true_type {};\n\ntemplate <>\nconstexpr bool IsValid<int>(const int& status) {\n  return status == 0;\n}\n\ntemplate <>\nconstexpr int DefaultError<int>() {\n  return 1;\n}\n\ntemplate <>\nstruct StatusName<int> {\n  static std::string Get(int status) { return std::to_string(status); }\n};\n\n//////////////////////////////////////////\n// Configuration for using enum as status:\n//////////////////////////////////////////\ntemplate <typename EnumStatus>\nstruct StatusName<EnumStatus, typename std::enable_if_t<std::is_enum_v<EnumStatus>>> {\n  static std::string Get(EnumStatus status) { return std::string(magic_enum::enum_name(status)); }\n};\n\n////////////////////////////////////////////////////////\n// Configuration for using std::optional as unwrappable:\n////////////////////////////////////////////////////////\ntemplate <typename Value>\nstruct IsUnwrappable<std::optional<Value>> : std::true_type {};\n\ntemplate <typename Value>\nstruct StatusType<std::optional<Value>> {\n  using Type = bool;\n};\n\ntemplate <typename Value>\nstruct StatusValue<std::optional<Value>> {\n  static constexpr bool Get(const std::optional<Value>& optional) { return optional.has_value(); }\n};\n\ntemplate <typename Value>\nstruct InvalidUnwrappable<std::optional<Value>, bool> {\n  static std::optional<Value> Get(bool status) { return std::nullopt; }\n};\n\n}  // namespace nvidia::expected_macro::config\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/gxf_helpers/expected_macro_cuda.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n\n#include \"gxf/core/expected_macro.hpp\"\n\nnamespace nvidia::expected_macro {\n\ntemplate <>\nstruct IsStatus<cudaError_t> : std::true_type {};\n\ntemplate <>\nconstexpr bool IsValid<cudaError_t>(const cudaError_t& status) {\n  return status == cudaSuccess;\n}\n\ntemplate <>\nconstexpr cudaError_t DefaultError<cudaError_t>() {\n  return cudaErrorUnknown;\n}\n\ntemplate <>\nstruct StatusName<cudaError_t> {\n  static std::string Get(cudaError_t status) { return std::string(cudaGetErrorString(status)); }\n};\n\n}  // namespace nvidia::expected_macro\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/gxf_helpers/expected_macro_gxf.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n\n#include \"common/expected.hpp\"\n#include \"gxf/core/expected_macro.hpp\"\n#include \"gxf/core/gxf.h\"\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/gxf_helpers/expected_macro_isaac.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n\n#include \"gems/core/expected.hpp\"\n#include \"gxf/core/expected_macro.hpp\"\n\n// This customizes the expected macro, s.t. it can be used with ::nvidia::isaac::Expected.\nnamespace nvidia::expected_macro {\n\n//////////////////////////////////////////////////////////////////\n// Configuration for using nvidia::isaac::Expected as unwrappable:\n//////////////////////////////////////////////////////////////////\ntemplate <typename Value, typename Status>\nstruct IsUnwrappable<::nvidia::isaac::Expected<Value, Status>> : std::true_type {};\n\ntemplate <typename Value, typename Status>\nstruct StatusType<::nvidia::isaac::Expected<Value, Status>> {\n  using Type = Status;\n};\n\ntemplate <typename Value, typename Status>\nstruct StatusValue<::nvidia::isaac::Expected<Value, Status>> {\n  static constexpr Status Get(const ::nvidia::isaac::Expected<Value, Status>& expected) {\n    return expected.error();\n  }\n};\n\ntemplate <typename Value, typename Status>\nstruct InvalidUnwrappable<::nvidia::isaac::Expected<Value, Status>, Status> {\n  static ::nvidia::isaac::Expected<Value, Status> Get(Status status) {\n    return ::nvidia::isaac::Unexpected<Status>(status);\n  }\n};\n\n}  // namespace nvidia::expected_macro\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/gxf_helpers/image_view.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/core/image/image.hpp\"\n#include \"gems/core/math/types.hpp\"\n#include \"gems/core/tensor/tensor.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/tensor.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Creates an Isaac image view of a GXF host tensor\n// Note that N here is the number of CHANNELS, not the number of dimensions.\n// N = 1: single-channel image, N = 3: 3-channel image, etc.\ntemplate <typename K, int N>\ngxf::Expected<::nvidia::isaac::ImageView<K, N>> ToIsaacImageView(gxf::Tensor& tensor) {\n  if (tensor.storage_type() != gxf::MemoryStorageType::kHost) {\n    return gxf::Unexpected{GXF_INVALID_DATA_FORMAT};\n  }\n\n  // Check if shape is compatible\n  if (N < 1) {\n    GXF_LOG_ERROR(\"Channel count (%d) must be at least 1\", N);\n    return gxf::Unexpected{GXF_INVALID_DATA_FORMAT};\n  } else if (N == 1) {\n    if (tensor.rank() != 2 && !(tensor.rank() == 3 && tensor.shape().dimension(2) == 1)) {\n      GXF_LOG_ERROR(\"Either rank is 2, or rank is 3 and the number of channels is 1. \"\n                    \"Channel count: %d, rank: %d\", N, tensor.rank());\n      return gxf::Unexpected{GXF_INVALID_DATA_FORMAT};\n    }\n  } else {  // N > 1\n    if (tensor.rank() != 3) {\n      GXF_LOG_ERROR(\"If channel count is greater than 1, rank must be 3.\"\n                    \"Channel count: %d, rank: %d\", N, tensor.rank());\n      return gxf::Unexpected{GXF_INVALID_DATA_FORMAT};\n    }\n  }\n  const size_t rows = tensor.shape().dimension(0);\n  const size_t cols = tensor.shape().dimension(1);\n\n  auto maybe_ptr = tensor.data<K>();\n  if (!maybe_ptr) {\n    return gxf::ForwardError(maybe_ptr);\n  }\n\n  return ::nvidia::isaac::CreateImageView<K, N>(maybe_ptr.value(), rows, cols);\n}\n\n// Creates an Isaac image const view of a GXF host tensor\n// Note that N here is the number of CHANNELS, not the number of dimensions.\n// N = 1: single-channel image, N = 3: 3-channel image, etc.\ntemplate <typename K, int N>\ngxf::Expected<::nvidia::isaac::ImageConstView<K, N>> ToIsaacImageView(const gxf::Tensor& tensor) {\n  if (tensor.storage_type() != gxf::MemoryStorageType::kHost) {\n    return gxf::Unexpected{GXF_INVALID_DATA_FORMAT};\n  }\n  if ((N == 1 && tensor.rank() != 2) || (N > 1 && tensor.rank() != 3)) {\n    GXF_LOG_ERROR(\"N: %d Tensor rank: %d\", N, tensor.rank());\n    return gxf::Unexpected{GXF_INVALID_DATA_FORMAT};\n  }\n  const size_t rows = tensor.shape().dimension(0);\n  const size_t cols = tensor.shape().dimension(1);\n\n  auto maybe_ptr = tensor.data<K>();\n  if (!maybe_ptr) {\n    return gxf::ForwardError(maybe_ptr);\n  }\n\n  return ::nvidia::isaac::CreateImageConstView<K, N>(maybe_ptr.value(), rows, cols);\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/image/color.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cmath>\n#include <random>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"gems/core/image/image.hpp\"\n#include \"gems/core/math/utils.hpp\"\n#include \"gems/core/tensor/tensor.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Maps the unit interval [0,1[ to a 8-bit unsigned integer clamping the value if out of bounds\n// This is the inverse function of MapUint8ToUnit.\ninline unsigned char MapUnitToUint8(float x) {\n  return static_cast<unsigned char>(Clamp(static_cast<int>(255.0f * x + 0.5f), 0, 255));\n}\n\n// Maps an 8-bit unsigned integer to the unit interval [0,1[\n// This is the inverse function of MapUnitToUint8.\ninline float MapUint8ToUnit(unsigned char c) {\n  return static_cast<float>(c) / 255.0f;\n}\n\n// Converts a 3f pixel to a 3ub pixel by mapping [0,1] to [0,255]\ninline Pixel3ub ToPixel3ub(const Pixel3f& color) {\n  return Pixel3ub{MapUnitToUint8(color[0]), MapUnitToUint8(color[1]), MapUnitToUint8(color[2])};\n}\n\n// Combines a color with an alpha value.\ninline Pixel4ub WithAlpha(const Pixel3ub& color, uint8_t alpha) {\n  return Pixel4ub{color[0], color[1], color[2], alpha};\n}\n\n// Converts a 3ub pixel to a 3f pixel by mapping [0,255] to [0,1]\ninline Pixel3f ToPixel3f(const Pixel3ub& color) {\n  return Pixel3f{MapUint8ToUnit(color[0]), MapUint8ToUnit(color[1]), MapUint8ToUnit(color[2])};\n}\n\n// Interpolates a color\ninline Pixel3f Interpolate(const float p, const Pixel3f& a, const Pixel3f& b) {\n  const float p0 = 1.0f - p;\n  return Pixel3f{p0 * a[0] + p * b[0], p0 * a[1] + p * b[1], p0 * a[2] + p * b[2]};\n}\n\n// Converts a color to hex representation, e.g. (255,128,0) -> \"#ff8000\"\nstd::string ToHexString(const Pixel3ub& color);\nstd::string ToHexString(const Pixel3ub& color, unsigned char alpha);\n\n// Some special colors\nstruct Colors {\n  static Pixel3ub Black() { return Pixel3ub{0, 0, 0}; }\n  static Pixel3ub White() { return Pixel3ub{255, 255, 255}; }\n  static Pixel3ub Pink() { return Pixel3ub{255, 20, 147}; }\n  static Pixel3ub NvidiaGreen() { return Pixel3ub{118, 185, 0}; }\n  static Pixel3ub NvidiaGreen2() { return Pixel3ub{0, 72, 49}; }\n  static Pixel3ub NvidiaGreen3() { return Pixel3ub{0, 132, 113}; }\n  static Pixel3ub Red() { return Pixel3ub{255, 0, 0}; }\n  static Pixel3ub Orange() { return Pixel3ub{255, 128, 0}; }\n  static Pixel3ub Yellow() { return Pixel3ub{255, 255, 0}; }\n  static Pixel3ub Green() { return Pixel3ub{0, 255, 0}; }\n  static Pixel3ub Cyan() { return Pixel3ub{0, 255, 255}; }\n  static Pixel3ub Blue() { return Pixel3ub{0, 0, 255}; }\n  static Pixel3ub Magenta() { return Pixel3ub{255, 0, 255}; }\n};\n\n// A color gradient which can be used to interpolate a color based on a floating point number.\nclass ColorGradient {\n public:\n  // Creates a black -> white gradient\n  ColorGradient() { setColors({Colors::Black(), Colors::White()}); }\n  // Creates a gradient using the given colors\n  ColorGradient(const std::vector<Pixel3ub>& colors) { setColors(colors); }\n  ColorGradient(const std::initializer_list<Pixel3ub>& colors) { setColors(colors); }\n  // Creates a gradient using the given colors\n  ColorGradient(const std::vector<Pixel3f>& colors) : colors_(std::move(colors)) {}\n  ColorGradient(std::initializer_list<Pixel3f> colors) : colors_(colors.begin(), colors.end()) {}\n\n  // Sets the colors of the gradient\n  void setColors(const std::vector<Pixel3ub>& colors) { setColors(colors.begin(), colors.end()); }\n  void setColors(const std::initializer_list<Pixel3ub>& colors) {\n    setColors(colors.begin(), colors.end());\n  }\n  template <typename It>\n  void setColors(It begin, It end);\n\n  // Get a color on the gradient for a value p between 0 and 1. p will be clamped if out of bounds.\n  // If the gradient is empty pink will be returned.\n  Pixel3ub operator()(float p) const;\n\n private:\n  std::vector<Pixel3f> colors_;\n};\n\n// Greyscale black to white color gradient\nColorGradient BlackWhiteColorGradient();\n// Color gradient inspired by \"The Starry Night\" painting\nColorGradient StarryNightColorGradient();\n// Color gradient to match the one above on positive value and extend it in the negative value\nColorGradient StarryNightExtendedColorGradient();\n// Color gradient going from black to Nvidia green\nColorGradient BlackGreenColorGradient();\n// All rainbow colors red to magenta\nColorGradient RainbowColorGradient();\n// Rainbow colors from green to magenta\nColorGradient RainbowGreenMagentaColorGradient();\n// From white to orange to black\nColorGradient ErrorColorGradient();\n// A gradient to visualize distances (from black to brown to white to light blue)\nColorGradient DistanceColorGradient();\n// A gradient going from red to blue over broken white\nColorGradient RedBlueColorGradient();\n\n// Colorizes an image of floats using a color gradient\nvoid Colorize(\n    const ImageConstView1f& input, const ColorGradient& gradient, float min, float max,\n    Image3ub& colored);\n// Colorizes an image of doubles using a color gradient\nvoid Colorize(\n    const ImageConstView1d& input, const ColorGradient& gradient, double min, double max,\n    Image3ub& colored);\n// Colorizes a tensor of floats using a color gradient\nvoid Colorize(\n    const CpuTensorConstView2f& input, const ColorGradient& gradient, float min, float max,\n    Image3ub& colored);\n// Colorizes a tensor of doubles using a color gradient\nvoid Colorize(\n    const CpuTensorConstView2d& input, const ColorGradient& gradient, double min, double max,\n    Image3ub& colored);\n// Colorizes an image of floats using a color gradient. This asserts if the input and colored image\n// dimensions (rows/columns) do not match\nvoid Colorize(\n    const ImageConstView1f& input, const ColorGradient& gradient, float min, float max,\n    ImageView3ub& colored);\n// Colorizes an image view of doubles using a color gradient. This asserts if the input and\n// colored image dimensions (rows/columns) do not match\nvoid Colorize(\n    const ImageConstView1d& input, const ColorGradient& gradient, double min, double max,\n    ImageView3ub& colored);\n// Colorizes a tensor of floats using a color gradient. This asserts if the input and colored\n// image dimensions (rows/columns) do not match\nvoid Colorize(\n    const CpuTensorConstView2f& input, const ColorGradient& gradient, float min, float max,\n    ImageView3ub& colored);\n// Colorizes a tensor of doubles using a color gradient. This asserts if the input and colored\n// image dimensions (rows/columns) do not match\nvoid Colorize(\n    const CpuTensorConstView2d& input, const ColorGradient& gradient, double min, double max,\n    ImageView3ub& colored);\n\n// A list of colors which can be used to get a color based on an integer\nclass IndexedColors {\n public:\n  // Creates a list with two colors: black and white\n  IndexedColors() : colors_({Pixel3ub::Zero(), Pixel3ub{255, 255, 255}}) {}\n\n  // Initializes the object with the given list of colors\n  IndexedColors(std::initializer_list<Vector3ub> colors) : colors_(colors) {}\n\n  // Creates a list of random colors\n  IndexedColors(size_t count, size_t seed) : colors_(count) {\n    colors_[0] = Pixel3ub::Zero();\n    std::default_random_engine rng(seed);\n    std::uniform_int_distribution<uint8_t> rnd(0, 255);\n    for (size_t i = 1; i < colors_.size(); i++) {\n      colors_[i] = Pixel3ub{rnd(rng), rnd(rng), rnd(rng)};\n    }\n  }\n\n  // Retrieves the color for the given index. If there are not enough colors the index is wrapped.\n  const Pixel3ub& operator()(int index) const {\n    const int value = index % colors_.size();\n    return colors_[value >= 0 ? value : (value + colors_.size())];\n  }\n\n private:\n  std::vector<Pixel3ub> colors_;\n};\n\n// 256 random colors using a default seed\nIndexedColors Random256IndexedColors();\n\n// 22 distinct colors after Sasha Trubetskoy\nIndexedColors Distinct22IndexedColors();\n\n// Colorizes an image of indices using a list of colors\ntemplate <typename Index>\nvoid ColorizeIndices(\n    const ImageConstView<Index, 1>& indices, const IndexedColors& colors, Image3ub& result) {\n  const size_t rows = indices.rows();\n  const size_t cols = indices.cols();\n  result.resize(indices.dimensions());\n  for (size_t row = 0; row < rows; row++) {\n    for (size_t col = 0; col < cols; col++) {\n      result(row, col) = colors(indices(row, col));\n    }\n  }\n}\n\n// Colorizes an image of indices using a list of colors. This asserts if the input and colored\n// image dimensions (rows/columns) do not match\ntemplate <typename Index>\nvoid ColorizeIndices(\n    const ImageConstView<Index, 1>& indices, const IndexedColors& colors, ImageView3ub& result) {\n  ASSERT(\n      result.rows() == indices.rows() && result.cols() == indices.cols(),\n      \"Output dimension (%d, %d) != Input dimension (%d, %d)\", result.rows(), result.cols(),\n      indices.rows(), indices.cols());\n\n  const size_t rows = indices.rows();\n  const size_t cols = indices.cols();\n  for (size_t row = 0; row < rows; row++) {\n    for (size_t col = 0; col < cols; col++) {\n      result(row, col) = colors(indices(row, col));\n    }\n  }\n}\n// -------------------------------------------------------------------------------------------------\n\ntemplate <typename It>\nvoid ColorGradient::setColors(It begin, It end) {\n  colors_.clear();\n  colors_.reserve(std::distance(begin, end));\n  for (; begin != end; ++begin) {\n    colors_.push_back(ToPixel3f(*begin));\n  }\n}\n\ninline Pixel3ub ColorGradient::operator()(float p) const {\n  if (colors_.empty()) {\n    return Colors::Pink();\n  }\n  const float pn = static_cast<float>(colors_.size() - 1) * p;\n  const int pi = std::floor(pn);\n  if (pi < 0) {\n    return ToPixel3ub(colors_.front());\n  }\n  if (static_cast<size_t>(pi + 1) >= colors_.size()) {\n    return ToPixel3ub(colors_.back());\n  }\n  const float pr = pn - static_cast<float>(pi);\n  return ToPixel3ub(Interpolate(pr, colors_[pi], colors_[pi + 1]));\n}\n\n// Converts a pixel from RGB scheme to HSV\n// Reference for conversion: https://en.wikipedia.org/wiki/HSL_and_HSV\nvoid ConvertRGBToHSV(PixelRef3ub pixel);\n\n// Converts a pixel from HSV scheme to RGB\n// Reference for conversion: https://en.wikipedia.org/wiki/HSL_and_HSV\nvoid ConvertHSVToRGB(PixelRef3ub pixel);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/image/io.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n#include <vector>\n\n#include \"gems/algorithm/string_utils.hpp\"\n#include \"gems/core/image/image.hpp\"\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Gets the dimensions of an image. The dimensions are written to `shape` in the order:\n// rows (0), cols (1), channels (2).\nbool LoadImageShape(const std::string& filename, Vector3i& shape);\n\n// Loads an image from a PNG file. The function will return false if the PNG could not be loaded.\n// This can for example happen if the filename is invalid or if the PNG doesn't have the requested\n// format.\nbool LoadPng(const std::string& filename, Image1ub& image);\nbool LoadPng(const std::string& filename, Image1ui16& image);\nbool LoadPng(const std::string& filename, Image3ub& image);\nbool LoadPng(const std::string& filename, Image4ub& image);\nbool LoadPng(const std::string& filename, ImageView1ub& tensor);\nbool LoadPng(const std::string& filename, ImageView3ub& tensor);\nbool LoadPng(const std::string& filename, CpuTensorView2ub& tensor);\n\n// Loads an image from a JPEG file. The function will return false if the JPEG could not be loaded.\n// This can for example happen if the filename is invalid or if the JPEG doesn't have the requested\n// format.\nbool LoadJpeg(const std::string& filename, Image1ub& image);\nbool LoadJpeg(const std::string& filename, Image3ub& image);\nbool LoadJpeg(const std::string& filename, ImageView1ub& image);\nbool LoadJpeg(const std::string& filename, ImageView3ub& image);\n\n// Loads an image from JPEG file or PNG file. The function will return false if the image could not\n// be loaded.\nbool LoadImage(const std::string& filename, Image1ub& image);\nbool LoadImage(const std::string& filename, Image3ub& image);\nbool LoadImage(const std::string& filename, ImageView1ub& image);\nbool LoadImage(const std::string& filename, ImageView3ub& image);\n\n// Saves an image to a file in the PNG format. The function will return false if the PNG could not\n// saved. This might for example happen if the file could not be opened.\nbool SavePng(const ImageConstView1ub& image, const std::string& filename);\nbool SavePng(const ImageConstView3ub& image, const std::string& filename);\nbool SavePng(const ImageConstView4ub& image, const std::string& filename);\nbool SavePng(const ImageConstView1ui16& image, const std::string& filename);\n\n// Saves an image to a file in the JPEG format. The function will return false if the JPEG could not\n// saved. This might for example happen, if the file could not be opened. Quality of 100 means\n// nearly lossless, but slow to encode and result in bigger images. While quality of 1 is fast\n// and produce very small image with a very low quality. (Note: 75 seems a good compromise)\nbool SaveJpeg(const ImageConstView1ub& image, const std::string& filename, const int quality = 75);\nbool SaveJpeg(const ImageConstView3ub& image, const std::string& filename, const int quality = 75);\n\n// Encodes an image into a buffer in the PNG format\nvoid EncodePng(const ImageConstView1ub& image, std::vector<uint8_t>& encoded);\nvoid EncodePng(const ImageConstView3ub& image, std::vector<uint8_t>& encoded);\nvoid EncodePng(const ImageConstView4ub& image, std::vector<uint8_t>& encoded);\n\n// Encodes an image into a file in the JPEG format\n// Quality of 100 is nearly lossless but slow to encode and result in bigger images while 1 is fast\n// and produce very small image with a very low quality. (Note: 75 seems a good compromise)\nvoid EncodeJpeg(const ImageConstView1ub& image, int quality, std::vector<uint8_t>& encoded);\nvoid EncodeJpeg(const ImageConstView3ub& image, int quality, std::vector<uint8_t>& encoded);\n\n// Decodes a PNG encoded byte string. The function will return false if the PNG could not be\n// decoded, for example if the byte string is not a valid PNG.\nbool DecodePng(const std::string& bytes, Image1ub& image);\nbool DecodePng(const std::string& bytes, Image1ui16& image);\nbool DecodePng(const std::string& bytes, Image3ub& image);\nbool DecodePng(const std::string& bytes, Image4ub& image);\nbool DecodePng(const std::string& bytes, ImageView1ub& tensor);\nbool DecodePng(const std::string& bytes, ImageView3ub& tensor);\nbool DecodePng(const std::string& bytes, CpuTensorView2ub& tensor);\n\n// Gets the dimensions of an image from a PNG encoded byte string. The dimensions are written to\n// `shape` in the order: rows (0), cols (1), channels (2).\nbool DecodePngShape(const std::string& bytes, Vector3i& shape);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/image/utils.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <algorithm>\n#include <cmath>\n#include <functional>\n#include <limits>\n#include <utility>\n#include <vector>\n\n#include \"gems/core/assert.hpp\"\n#include \"gems/core/buffers/algorithm.hpp\"\n#include \"gems/core/image/image.hpp\"\n#include \"gems/core/math/types.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Copies an image\ntemplate <typename K, int N, typename SourceContainer, typename TargetContainer>\nvoid Copy(const ImageBase<K, N, SourceContainer>& source, ImageBase<K, N, TargetContainer>& target);\n\n// Sets all pixels of an image to the given value\ntemplate <typename K, int N, typename Container>\nvoid FillPixels(ImageBase<K, N, Container>& image, const Pixel<K, N>& value);\n\n// Sets all elements of all pixels to the given `value`\ntemplate <typename K, int N, typename Container>\nvoid FillElements(ImageBase<K, N, Container>& image, K value);\n\n// Sets all elements of all pixels to 0\ntemplate <typename K, int N, typename Container>\nvoid FillElementsWithZero(ImageBase<K, N, Container>& image) {\n  FillElements(image, K(0));\n}\n\n// Resize down an image by a factor N.\ntemplate <int Factor, typename K, int N, typename Container>\nImage<K, N> Reduce(const ImageBase<K, N, Container>& img);\ntemplate <typename K, int N, typename Container>\nImage<K, N> Reduce(const ImageBase<K, N, Container>& img, int factor);\n\n// Enlarge an image by a factor N\ntemplate <int NRows, int NCols = NRows, class Image>\nImage Enlarge(const Image& img);\n\n// Convert an image from a format to another given a pixel conversion function\ntemplate <typename Out, typename In, typename F>\nOut Convert(const In& img, F convert);\ntemplate <typename Out, typename In, typename F>\nvoid Convert(const In& img, Out& out, F convert);\n\n// Normalizes an image\ntemplate <typename K, typename Container>\nvoid Normalize(const ImageBase<K, 1, Container>& input, Image1ub& output);\ntemplate <typename K, typename Container>\nvoid Normalize(const ImageBase<K, 1, Container>& input, K min, K max, Image1ub& output);\n\n// Crops the image\ntemplate <typename K, int N, typename Container>\nvoid Crop(const ImageBase<K, N, Container>& img, const Vector2i& crop_start,\n          const Vector2i& crop_size, Image<K, N>& dest_img);\n\n// Convert disparity to depth using baseline and focal length.\n// The disparity_img is expected to be in pixels\n// The depth will have same unit as baseline. The baseline, min_depth and max_depth should all be\n// in the same units.\ntemplate <typename K, typename Container,\n          typename std::enable_if<std::is_floating_point<K>::value, int>::type = 0>\nvoid ConvertDisparityToDepth(const ImageBase<K, 1, Container>& disparity_img, K baseline,\n                             K focal_length_px, K min_depth, K max_depth, Image<K, 1>& depth_img);\n\n// Stitches two images (with same number of rows) together side by side\nvoid JoinTwoImagesSideBySide(const ImageConstView3ub& left_image,\n                             const ImageConstView3ub& right_image, Image3ub& joint_image);\n\n// Splits an image into two halfs\nvoid SplitImages(const ImageConstView3ub& joint, Image3ub& left, Image3ub& right);\n\n// -------------------------------------------------------------------------------------------------\n\ntemplate <typename K, int N, typename SourceContainer, typename TargetContainer>\nvoid Copy(const ImageBase<K, N, SourceContainer>& source,\n          ImageBase<K, N, TargetContainer>& target) {\n  // Asserts that images have the same shape\n  ASSERT(source.rows() == target.rows(), \"row count mismatch: %zu vs %zu\",\n         source.rows(), target.rows());\n  ASSERT(source.cols() == target.cols(), \"col count mismatch: %zu vs %zu\",\n         source.cols(), target.cols());\n  // Copy the bytes\n  CopyMatrixRaw(reinterpret_cast<const void*>(source.element_wise_begin()),\n                source.getStride(),\n                BufferTraits<SourceContainer>::kStorageMode,\n                reinterpret_cast<void*>(target.element_wise_begin()),\n                target.getStride(),\n                BufferTraits<TargetContainer>::kStorageMode,\n                source.rows(), source.cols() * N * sizeof(K));\n}\n\ntemplate <typename K, int N, typename Container>\nvoid FillPixels(ImageBase<K, N, Container>& image, const Pixel<K, N>& pixel) {\n  for (int row = 0; row < image.rows(); row++) {\n    for (int col = 0; col < image.cols(); col++) {\n      image(row, col) = pixel;\n    }\n  }\n}\n\ntemplate <typename K, typename Container>\nvoid FillPixels(ImageBase<K, 1, Container>& image, K pixel) {\n  // Use the faster FillElements for 1-channel images\n  FillElements(image, pixel);\n}\n\ntemplate <typename K, int N, typename Container>\nvoid FillElements(ImageBase<K, N, Container>& image, K value) {\n  const int elements_per_row = image.channels() * image.cols();\n  for (int row = 0; row < image.rows(); row++) {\n    K* row_pointer = image.row_pointer(row);\n    std::fill(row_pointer, row_pointer + elements_per_row, value);\n  }\n}\n\ntemplate <int Factor, typename K, int N, typename Container>\nImage<K, N> Reduce(const ImageBase<K, N, Container>& img) {\n  Image<K, N> out(img.rows() / Factor, img.cols() / Factor);\n  for (int row = 0; row < out.rows(); row++) {\n    for (int col = 0; col < out.cols(); col++) {\n      out(row, col) = img(Factor * row, Factor * col);\n    }\n  }\n  return out;\n}\n\ntemplate <typename K, int N, typename Container>\nImage<K, N> Reduce(const ImageBase<K, N, Container>& img, int factor) {\n  Image<K, N> out(img.rows() / factor, img.cols() / factor);\n  for (int row = 0; row < out.rows(); row++) {\n    for (int col = 0; col < out.cols(); col++) {\n      out(row, col) = img(factor * row, factor * col);\n    }\n  }\n  return out;\n}\n\ntemplate <int NRows, int NCols, class Image>\nImage Enlarge(const Image& img) {\n  Image out(img.rows() * NRows, img.cols() * NCols);\n  for (int row = 0; row < out.rows(); row++) {\n    for (int col = 0; col < out.cols(); col++) {\n      out(row, col) = img(row / NRows, col / NCols);\n    }\n  }\n  return out;\n}\n\ntemplate <class Out, class In, typename F>\nOut Convert(const In& img, F convert) {\n  Out out(img.rows(), img.cols());\n  for (int pixel = 0; pixel < out.num_pixels(); pixel++) {\n    out[pixel] = convert(img[pixel]);\n  }\n  return out;\n}\n\ntemplate <class Out, class In, typename F>\nvoid Convert(const In& img, Out& out, F convert) {\n  ASSERT(out.dimensions() == img.dimensions(), \"dimensions mismatch\");\n  for (int pixel = 0; pixel < out.num_pixels(); pixel++) {\n    out[pixel] = convert(img[pixel]);\n  }\n}\n\ntemplate <typename K, typename Container>\nvoid Normalize(const ImageBase<K, 1, Container>& input, Image1ub& output) {\n  K min = input[0];\n  K max = input[0];\n  for (int pixel = 0; pixel < input.num_pixels(); pixel++) {\n    min = std::min(min, input[pixel]);\n    max = std::max(max, input[pixel]);\n  }\n  if (min == max) {\n    min -= K(1.0);\n    max += K(1.0);\n  }\n  output = Convert<Image1ub>(\n      input, [&](K val) { return static_cast<uint8_t>(K(255.9) * (val - min) / (max - min)); });\n}\n\ntemplate <typename K, typename Container>\nvoid Normalize(const ImageBase<K, 1, Container>& input, K min, K max, Image1ub& output) {\n  ASSERT(min < max, \"Invalid range\");\n  Convert<Image1ub>(input, output, [&](K val) {\n    return static_cast<uint8_t>(K(255.9) * Clamp01((val - min) / (max - min)));\n  });\n}\n\ntemplate <typename K, int N, typename Container>\nvoid Crop(const ImageBase<K, N, Container>& img, const Vector2i& crop_start,\n          const Vector2i& crop_size, Image<K, N>& dest_img) {\n  ASSERT((crop_start.array() >= 0).all(), \"Invalid crop start location.\");\n  ASSERT((crop_size.array() > 0).all(), \"Invalid crop size.\");\n  Vector2i img_size{img.rows(), img.cols()};\n  ASSERT(((crop_start + crop_size).array() <= img_size.array()).all(), \"Invalid crop size\");\n\n  EigenImageConstMap<K> eigen_img(img.element_wise_begin(), img.rows(), img.cols() * N);\n  dest_img.resize(crop_size[0], crop_size[1]);\n  EigenImageMap<K> eigen_dest_img(dest_img.element_wise_begin(), dest_img.rows(),\n                                  dest_img.cols() * N);\n  eigen_dest_img = eigen_img.block(crop_start[0], crop_start[1] * N, crop_size[0],\n                                   crop_size[1] * N);\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/pose_tree/pose_tree.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <map>\n#include <memory>\n#include <mutex>\n#include <shared_mutex>  // NOLINT(build/include_order)\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"common/unique_index_map.hpp\"\n#include \"gems/core/math/pose2.hpp\"\n#include \"gems/core/math/pose3.hpp\"\n#include \"gems/gxf_helpers/expected_macro_gxf.hpp\"\n#include \"gems/pose_tree/pose_tree_edge_history.hpp\"\n#include \"gxf/core/component.hpp\"\n#include \"gxf/std/gems/suballocators/first_fit_allocator.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// A temporal pose tree to store relative coordinate system transformations over time.\n// This implementation does not support multiple paths between the same coordinate systems at a\n// given time. It does however allow to disconnect edge and create new connection using a different\n// path. It also allows for multiple \"roots\". In fact the transformation relationships form an\n// acylic, bi-directional, not necessarily fully-connected graph.\n// This PoseTree assigned a different version id to each operation that affects it, and this version\n// can be used to make a query ignore later changes made to the tree.\nclass PoseTree : public gxf::Component {\n public:\n  // Error codes used by this class.\n  enum class Error {\n    // kInvalidArgument is returned when a function is called with argument that does not make sense\n    // such as negative number of frames.\n    kInvalidArgument = 0,\n    // kOutOfMemory is returned if `initialize` failed to allocate the requested memory, or if an\n    // edge/frame can't be added because we run out of the pre-allocated memory.\n    kOutOfMemory = 1,\n    // kFrameNotFound is returned if a query is made with a frame uid that does not match any\n    // existing frame.\n    kFrameNotFound = 2,\n    // kAlreadyExists is returned if a frame or an edge that already exist is added.\n    kAlreadyExists = 3,\n    // kCyclingDependency is returned if a pose is added that would create a cycle in the PoseTree\n    // structure.\n    kCyclingDependency = 4,\n    // kFramesNotLinked is returned if a query is made between two not connected frame or if we\n    // attempt to disconnect/delete an edge that does not exist.\n    kFramesNotLinked = 5,\n    // kPoseOutOfOrder is returned if a query is made to update the three in the past. For example\n    // if we try to disconnect or update a pose at a time older than the latest update on this edge.\n    kPoseOutOfOrder = 6,\n    // kLogicError is used whenever an error that should not have happened happened. This should\n    // never happen and are here only to prevent crashes/assert,\n    kLogicError = 7,\n  };\n\n  // The maximum size for the name of a frame. An additional '\\0' is added at the end to make it\n  // 64 characters long.\n  static constexpr int32_t kFrameNameMaximumLength = 63;\n\n  // Auto generated frame names will start with this prefix, followed by the uid of the frame.\n  static constexpr char const* kAutoGeneratedFrameNamePrefix = \"_frame_\";\n\n  // Expected type used by this class.\n  template <typename T>\n  using Expected = nvidia::Expected<T, Error>;\n  // Unexpected type used by this class.\n  using Unexpected = nvidia::Unexpected<Error>;\n\n  // Type used to uniquely identify a frame.\n  using frame_t = uint64_t;\n  // Type used for versioning the PoseTree.\n  using version_t = uint64_t;\n  // Type used as a key for the PoseTreeEdgeHistory map.\n  using history_t = uint64_t;\n\n  // Type for callback functions that are called every time a frame is created.\n  using CreateFrameCallback = std::function<void(frame_t frame)>;\n  // Type for callback functions that are called every time an edge is set.\n  using SetEdgeCallback = std::function<void(frame_t lhs, frame_t rhs, double time,\n                                             const ::nvidia::isaac::Pose3d& lhs_T_rhs)>;\n\n  // Allocates space for a given number of total frames and total number of edges.\n  // Total amount of memory required is approximately:\n  //  number_frames * 128 + number_edges * 64 + history_length * 72.\n  Expected<void> init(int32_t number_frames, int32_t number_edges, int32_t history_length,\n                      int32_t default_number_edges, int32_t default_history_length,\n                      int32_t edges_chunk_size, int32_t history_chunk_size);\n  void deinit();\n\n  // Returns the current PoseTree version.\n  version_t getPoseTreeVersion() const;\n\n  // Creates a new frame in the PoseTree. An optional name may be given to give a human-readable\n  // name to the frame. The name is a null-terminated string with at most 63 characters. User\n  // defined name cannot start with \"_\", which is reserved for auto generated names such as\n  // \"_frame_i\", where i is the uid of the frame. hint on the maximum number of edges this frame\n  // will be connected to can be provided. Returns the frame id.\n  Expected<frame_t> createFrame(const char* name, int32_t number_edges);\n  Expected<frame_t> createFrame(const char* name);\n  Expected<frame_t> createFrame(int32_t number_edges);\n  Expected<frame_t> createFrame();\n\n  // Finds a frame with the given name, and returns the frame id. If no such frame exist, it returns\n  // Error::kFrameNotFound\n  Expected<frame_t> findFrame(const char* name) const;\n  // Finds a frame with the given name, and returns the frame id. If no such frame exist, it creates\n  // a new frame by calling the equivalent createFrame function.\n  Expected<frame_t> findOrCreateFrame(const char* name);\n  Expected<frame_t> findOrCreateFrame(const char* name, int32_t number_edges);\n\n  // Creates an edge between the left hand side (lhs) frame and the right hand side (rhs) frame.\n  // A hint on the maximum length needed can be provided.\n  // Upon success, it returns the version id of the change.\n  Expected<version_t> createEdges(frame_t lhs, frame_t rhs);\n  Expected<version_t> createEdges(frame_t lhs, frame_t rhs, int32_t maximum_length);\n  Expected<version_t> createEdges(frame_t lhs, frame_t rhs,\n                                  PoseTreeEdgeHistory::AccessMethod method);\n  Expected<version_t> createEdges(frame_t lhs, frame_t rhs, int32_t maximum_length,\n                                  PoseTreeEdgeHistory::AccessMethod method);\n\n  // Deletes a frame in the PoseTree and all its relations to other frames and frees its memory.\n  // This action is permantly erasing the history information.\n  // Upon success, it returns the version id of the change (however query made with a previous\n  // version will also consider the frame as deleted).\n  Expected<version_t> deleteFrame(frame_t uid);\n  // Deletes an edge and frees the memory.\n  // This action is permantly erasing the history.\n  // Upon success, it returns the version id of the change (however query made with a previous\n  // version will also consider the edge as deleted).\n  Expected<version_t> deleteEdge(frame_t lhs, frame_t rhs);\n\n  // Disconnects a frame from all the others starting at a given time.\n  // Upon success, it returns the version id of the change.\n  Expected<version_t> disconnectFrame(frame_t uid, double time);\n  // Disconnects an edge starting at a given time.\n  // Upon success, it returns the version id of the change.\n  Expected<version_t> disconnectEdge(frame_t lhs, frame_t rhs, double time);\n\n  // Disable all the implicit cast (to make sure to catch a call with the wrong type for the time)\n  template <class ... Args> Expected<::nvidia::isaac::Pose3d>\n  disconnectFrame(Args&&... args) = delete;\n  template <class ... Args> Expected<::nvidia::isaac::Pose3d>\n  disconnectEdge(Args&&... args) = delete;\n\n  // Gets the name of a frame.\n  Expected<const char*> getFrameName(frame_t uid) const;\n\n  // Gets the latest pose between two frames as well as the time of that pose.\n  // The two poses needs to be directly linked\n  Expected<std::pair<::nvidia::isaac::Pose3d, double>> getLatest(frame_t lhs, frame_t rhs) const;\n  Expected<std::pair<::nvidia::isaac::Pose3d, double>>\n  getLatest(const char* lhs, const char* rhs) const;\n\n  // Gets the pose lhs_T_rhs between two frames in the PoseTree at the given time. If the poses are\n  // not connected exactly at the given time, the indicated method is used to interpolate the data.\n  Expected<::nvidia::isaac::Pose3d> get(frame_t lhs, frame_t rhs, double time,\n                                PoseTreeEdgeHistory::AccessMethod method,\n                                version_t version) const;\n  Expected<::nvidia::isaac::Pose3d> get(frame_t lhs,\n                                        frame_t rhs,\n                                        double time,\n                                        version_t version) const;\n  Expected<::nvidia::isaac::Pose3d> get(frame_t lhs, frame_t rhs, double time,\n                                PoseTreeEdgeHistory::AccessMethod method) const;\n  Expected<::nvidia::isaac::Pose3d> get(frame_t lhs, frame_t rhs, double time) const;\n\n  // Same as above, but using the name as interface.\n  Expected<::nvidia::isaac::Pose3d> get(const char* lhs, const char* rhs, double time,\n                                PoseTreeEdgeHistory::AccessMethod method,\n                                version_t version) const;\n  Expected<::nvidia::isaac::Pose3d> get(const char* lhs, const char* rhs, double time,\n                                version_t version) const;\n  Expected<::nvidia::isaac::Pose3d> get(const char* lhs, const char* rhs, double time,\n                                PoseTreeEdgeHistory::AccessMethod method) const;\n  Expected<::nvidia::isaac::Pose3d> get(const char* lhs, const char* rhs, double time) const;\n\n  // Disable all the implicit cast (to make sure to catch a call with the wrong type for the time)\n  template <class ... Args> Expected<::nvidia::isaac::Pose3d> get(Args&&... args) const = delete;\n\n  // Helper function to get a Pose2d instead of Pose3d\n  template <class ... Args>\n  Expected<::nvidia::isaac::Pose2d> getPose2XY(Args&&... args) const {\n    return get(std::forward<Args>(args)...).map([](const ::nvidia::isaac::Pose3d& pose_3d) {\n      return pose_3d.toPose2XY();\n    });\n  }\n\n  // The last time at which the pose between two frames is specified.\n  // TODO(bbutin): Implement the following methods.\n  // Expected<timed_pose_t> latest(frame_t lhs, frame_t rhs, version_t version) const;\n  // Expected<timed_pose_t> latest(frame_t lhs, frame_t rhs) const;\n\n  // Sets the pose between two frames in the PoseTree. Note that poses can not be changed\n  // retrospectively. Thus for example once the pose at time t=2.0 is set it is no longer allowed\n  // to set the pose for time t <= 2.0. It is not allowed to form cycles. Frames are implicitly\n  // linked. If more than the maximum number of allowed poses are set the oldest pose is deleted.\n  // Upon success, it returns the version id of the change.\n  Expected<version_t> set(frame_t lhs, frame_t rhs, double time,\n                          const ::nvidia::isaac::Pose3d& lhs_T_rhs);\n  // Same as above, but using the name as interface.\n  Expected<version_t> set(const char* lhs, const char* rhs, double time,\n                          const ::nvidia::isaac::Pose3d& lhs_T_rhs);\n\n  // Helper function to set a Pose2d instead of Pose3d\n  Expected<version_t> set(frame_t lhs, frame_t rhs, double time,\n                          const ::nvidia::isaac::Pose2d& lhs_T_rhs) {\n    return set(lhs, rhs, time, ::nvidia::isaac::Pose3d::FromPose2XY(lhs_T_rhs));\n  }\n  // Same as above, but using the name as interface.\n  Expected<version_t> set(const char* lhs, const char* rhs, double time,\n                          const ::nvidia::isaac::Pose2d& lhs_T_rhs) {\n    return set(lhs, rhs, time, ::nvidia::isaac::Pose3d::FromPose2XY(lhs_T_rhs));\n  }\n\n  // Disable all the implicit cast (to make sure to catch a call with the wrong type for the time)\n  // First we define a function that will match all the call with double and forward it to the\n  // appropirate function. This is needed in case the Pose is provided by value.\n  template <typename Frame, typename Pose>\n  Expected<version_t> set(Frame lhs, Frame rhs, double time, const Pose lhs_T_rhs) {\n    return set(lhs, rhs, time, lhs_T_rhs);\n  }\n  // Then we disable all the calls not made with double.\n  template <typename Frame, typename T, typename Pose>\n  Expected<version_t> set(Frame lhs, Frame rhs, T time, const Pose& lhs_T_rhs) = delete;\n\n  // Get list of edges\n  std::vector<std::pair<frame_t, frame_t>> edges() const;\n\n  // Registers a callback function for every time a frame is created. If a callback function\n  // does not already exist for the provided commponent id cid.\n  Expected<void> addCreateFrameCallback(gxf_uid_t cid, CreateFrameCallback callback);\n\n  // Deregisters a callback function for time an edge is set. If there is no callback function for\n  // the component id cid, this function returns an error.\n  Expected<void> removeCreateFrameCallback(gxf_uid_t cid);\n\n  // Registers a callback function for every time an edge is set. If a callback function\n  // does not already exist for the provided commponent id cid.\n  Expected<void> addSetEdgeCallback(gxf_uid_t cid, SetEdgeCallback callback);\n\n  // Deregisters a callback function for time an edge is set. If there is no callback function for\n  // the component id cid, this function returns an error.\n  Expected<void> removeSetEdgeCallback(gxf_uid_t cid);\n\n  // Helper function to transform an error code into an human readable error.\n  static const char* ErrorToStr(Error error);\n\n private:\n  // Helper structure that stores the information about a frame.\n  struct FrameInfo {\n    // Array containg the list of edges.\n    history_t* history;\n    // Current number of edges\n    int32_t number_edges;\n    // Maximum number of edges allowed\n    int32_t maximum_number_edges;\n    // Name of the frame. It has to be null terminated, so it can hold at most 63 characters.\n    char name[kFrameNameMaximumLength + 1];\n    // Hint to quickly find a path:\n    // Store the distance from the node to the root (== 0 if this frame is the root)\n    int32_t distance_to_root;\n    // Frame to follow to reach the root\n    frame_t node_to_root;\n    // Name of the root\n    frame_t root;\n    // Some helper id to computer the path between two nodes\n    mutable version_t hint_version;\n    // Some helper to memorize the path we took during the dfs\n    mutable frame_t dfs_link;\n    // Name of the frame.\n    frame_t uid;\n  };\n  // Helper class to compare to const char*\n  // TODO(ben): Remove once we get rid of map\n  struct CharMapCompare {\n    bool operator()(const char* lhs, const char* rhs) const {\n      return std::strcmp(lhs, rhs) < 0;\n    }\n  };\n\n  // Implementation of findOrCreateFrame\n  Expected<frame_t> findOrCreateFrameImpl(const char* name, int32_t number_edges);\n  // Implementation of findFrame\n  Expected<frame_t> findFrameImpl(const char* name) const;\n  // Implementation of createFrame\n  Expected<frame_t> createFrameImpl(const char* name, int32_t number_edges);\n  // Implementation of createEdges\n  Expected<version_t> createEdgesImpl(frame_t lhs, frame_t rhs, int32_t maximum_length,\n                                      PoseTreeEdgeHistory::AccessMethod method);\n  // Implementation of deleteEdge\n  Expected<version_t> deleteEdgeImpl(frame_t lhs, frame_t rhs, version_t version);\n  // Update the path to the root for a given connected component starting from the given node.\n  Expected<void> updateRoot(frame_t root);\n  // Implementation of get using the pre-computed path to the root as a hint. If it fails, it falls\n  // back to getDfsImpl.\n  Expected<::nvidia::isaac::Pose3d> getImpl(frame_t lhs, frame_t rhs, double time,\n                                    PoseTreeEdgeHistory::AccessMethod method,\n                                    version_t version) const;\n  // Implementation of get that do a dfs to see if a path exists at a given time.\n  Expected<::nvidia::isaac::Pose3d> getDfsImpl(frame_t lhs, frame_t rhs, double time,\n                                       PoseTreeEdgeHistory::AccessMethod method,\n                                       version_t version) const;\n\n  // Lock to protect access to the parameter below.\n  mutable std::shared_timed_mutex mutex_;\n  // Lock to protect access to getDfsImpl. This function is rarely called, but can be called while\n  // mutex_ is lock in read access, and getDfsImpl is modifying the dfs_link of some frames as well\n  // as using the frames_stack_. We need a special protection for this function while not blocking\n  // all the concurrent read which most likely won't call it.\n  mutable std::mutex dfs_mutex_;\n\n  // Lock to protect create_frame_callbacks_\n  mutable std::shared_timed_mutex create_frame_callbacks_mutex_;\n  // Callback functions for the create frame operation.\n  // TODO(dbhaskara): Replace with UniqueIndexMap once it supports iteration through all elements\n  std::unordered_map<gxf_uid_t, CreateFrameCallback> create_frame_callbacks_;\n\n  // Lock to protect set_edge_callbacks_\n  mutable std::shared_timed_mutex set_edge_callbacks_mutex_;\n  // Callback functions for the set edge operation.\n  // TODO(dbhaskara): Replace with UniqueIndexMap once it supports iteration through all elements\n  std::unordered_map<gxf_uid_t, SetEdgeCallback> set_edge_callbacks_;\n\n  // Mapping from a frame to it's index.\n  // TODO(ben): We need to get rid of std::map\n  std::map<std::pair<frame_t, frame_t>, history_t> edges_map_;\n\n  // TODO(ben): We need to get rid of std::map, but for now UniqueIndexMap does not support\n  // iterating through all the elements.\n  std::map<const char*, frame_t, CharMapCompare> name_to_uid_map_;\n\n  // Store the list of the current frame of the PoseTree.\n  UniqueIndexMap<FrameInfo> frame_map_;\n\n  // Used to implement a dfs.\n  std::unique_ptr<frame_t[]> frames_stack_;\n\n  // Store the list of PoseTreeEdgeHistory used by the frames. Each PoseTreeEdgeHistory correspond\n  // to a bi-directional edge.\n  UniqueIndexMap<PoseTreeEdgeHistory> histories_map_;\n\n  // Helper to `allocate` an array of PoseTreeEdgeHistory (storing only the uid).\n  gxf::FirstFitAllocator<history_t> histories_management_;\n\n  // Helper to `allocate` an array of TimedPose.\n  gxf::FirstFitAllocator<PoseTreeEdgeHistory::TimedPose> poses_management_;\n\n  // Current version of the PoseTree.\n  frame_t version_;\n  // Version of the hint. Mostly used to know if a node in the stack has been processed already.\n  mutable frame_t hint_version_;\n\n  // Default maximum number of edges a given frame can have\n  int32_t default_number_edges_;\n  // Default length of the history used by an edge.\n  int32_t default_history_length_;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n\n// We configure the expected macro to work with PoseTree::Error. The configuration has to happen in\n// exactly this namespace.\nnamespace nvidia::expected_macro {\ntemplate <>\nstruct IsStatus<::nvidia::isaac::PoseTree::Error> : std::true_type {};\n}  // namespace nvidia::expected_macro\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/pose_tree/pose_tree_edge_history.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <shared_mutex>\n\n#include \"gems/core/math/pose3.hpp\"\n#include \"gxf/core/expected.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Class that stores an history of Poses (using pre-allocated memory and a cyclic buffer) and\n// provides helper function to add a new pose or query the pose at a given time.\nclass PoseTreeEdgeHistory {\n public:\n  // Interpolation method for accessing poses in the pose tree\n  enum class AccessMethod {\n    // Gets the value of the closest sample\n    kNearest,\n    // Interpolates linearly between adjacent samples. If the query is outside the validity range,\n    // the closest pose will be returned.\n    kInterpolateLinearly,\n    // Inter- or extrapolates linearly based on neighbouring samples. This require at least two\n    // valid poses or Error::kOutOfRange will be returned.\n    kExtrapolateLinearly,\n    // Interpolates with slerp between adjacent samples. If the query is outside the validity range,\n    // the closest pose will be returned.\n    kInterpolateSlerp,\n    // Inter- or extrapolates with slerp based on neighbouring samples. This require at least two\n    // valid poses or Error::kOutOfRange will be returned.\n    kExtrapolateSlerp,\n    // Use the latest Pose before a given time.\n    kPrevious,\n    // Fallback to the default interpolation\n    kDefault,\n  };\n\n  // Error codes used by this class.\n  enum class Error {\n    // kInvalidArgument is returned when a function is called with argument that does not make sense\n    // such as querying a pose outside the valid range or provide a wrong interpolation method.\n    kInvalidArgument,\n    // kOutOfOrder is returned when a set/disconnected called is made with a version or time\n    // lower than the latest TimedPose. Both time and version must be stricly increasing.\n    kOutOfOrder,\n    // kFramesNotLinked is returns if a get query is made and the tree is not connected at the given\n    // time and version of the edge.\n    kFramesNotLinked,\n    // kOutOfRange is returned if not enough poses are available to do extrapolation or if the\n    // available buffer is too small to store a new pose.\n    kOutOfRange,\n  };\n\n  // Expected type used by this class.\n  template <typename T>\n  using Expected = nvidia::Expected<T, Error>;\n  // Unexpected type used by this class.\n  using Unexpected = nvidia::Unexpected<Error>;\n\n  // Type used to uniquely identify a frame.\n  using frame_t = uint64_t;\n  // Type used for versioning the edge.\n  using version_t = uint64_t;\n\n  // Helper structure to store the pose at a given time on the edge.\n  struct TimedPose {\n    // 3D pose that transforms the lhs frame into the rhs frame.\n    ::nvidia::isaac::Pose3d pose;\n    // Time of the pose. Needs to be strictly increasing.\n    double time;\n    // Version ID of the pose. Needs to be strictly increasing.\n    version_t version;\n    // If false, then it marks the edge as being disconnected from this current time. The pose does\n    // not matter.\n    bool valid;\n  };\n\n  // Default constructor used to be able to pre-allocate memory.\n  PoseTreeEdgeHistory() = default;\n\n  // Constructor to actually uses the object:\n  // `buffer` is a pre allocated buffer that can old `maximum_size` elements.\n  PoseTreeEdgeHistory(frame_t lhs, frame_t rhs, int32_t maximum_size, TimedPose* buffer);\n  PoseTreeEdgeHistory(frame_t lhs, frame_t rhs, int32_t maximum_size, AccessMethod access_method,\n                      TimedPose* buffer);\n\n  // Sets the pose at a given amount of time. If the array is empty Error::kOutOfMemory will be\n  // returned, otherwise if a pose already exist and `time` or `version` <= pose.time/version then\n  // Error::kOutOfOrder is returned. Otherwise it will succeed, and if the history already\n  // contained maximum_size_ element, then the oldest pose will be forgotten.\n  Expected<void> set(double time, const ::nvidia::isaac::Pose3d& pose, version_t version);\n\n  // Returns the TimedPose at a given position. If index is negative Error::kInvalidArgument will be\n  // returned, and if index >= size, then Error::kOutOfRange will be returned.\n  Expected<TimedPose> at(int32_t index) const;\n\n  // Returns the Pose3d at a given time using the given version of the PoseTree.\n  // If no pose existed at the given time, Error::kFramesNotLinked will be returned.\n  // The desired method can be provided, for kExtrapolateLinearly, at least two poses are required.\n  Expected<::nvidia::isaac::Pose3d> get(double time, AccessMethod method, version_t version) const;\n\n  // Disconnects a frame at a given time.\n  Expected<void> disconnect(double time, version_t version);\n\n  // Returns whether or not the frame are current connected\n  bool connected() const;\n\n  // Resets the history, all the poses will be erased.\n  void reset();\n\n  // Returns the information about the latest pose\n  Expected<TimedPose> latest() const;\n\n  // Returns a pointer to the buffer.\n  const TimedPose* data() const {\n    return edges_info_;\n  }\n\n  // Returns the current size.\n  int32_t size() const {\n    return size_;\n  }\n\n  // Returns the maximum number of poses this edge can contain.\n  int32_t maximum_size() const {\n    return maximum_size_;\n  }\n\n  // This edge reprensent the transformation from the rhs frame to the lhs frame. This function\n  // returns the uid of the lhs frame.\n  frame_t lhs() const {\n    return lhs_;\n  }\n\n  // This edge reprensent the transformation from the rhs frame to the lhs frame. This function\n  // returns the uid of the rhs frame.\n  frame_t rhs() const {\n    return rhs_;\n  }\n\n private:\n  // Reserves a new pose for a given time/version and returns it's index.\n  // If there exists another pose with a later time/version, it returns kOutOfOrder.\n  Expected<int32_t> reserveNewPose(double time, version_t version);\n\n  // Mutex to protect changes to this object.\n  mutable std::shared_timed_mutex mutex_;\n  // Pointers to the buffer that contains the list of TimedPose (we use a circulat buffer to access\n  // it).\n  TimedPose* edges_info_ = nullptr;\n  // Name of the frame this edge connects to.\n  frame_t lhs_, rhs_;\n  // Size of the buffer, aka maximum number of elements this edge can store in the same time.\n  int32_t maximum_size_ = 0;\n  // Current number of poses on this edge.\n  int32_t size_ = 0;\n  // Position of the first pose in the circular buffer,\n  int32_t pos_ = 0;\n  // Default access method.\n  AccessMethod default_access_method_;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/serialization/base64.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n\n#include \"gems/core/image/image.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace serialization {\n\n// Encodes an Image3ub image into a Bitmap image base 64 encoded ready to be used in HTML code.\n// WARNING: The image is converted to BMP format which requires the memory to be 4-alligned at the\n// beginning of each row. In order to avoid memory copy and keep the code efficient, the encoded\n// image might contain more additional columns filled with random data, it will be the\n// responsability of the consumer to ignore these columns.\nstd::string Base64Encode(const Image1ub& image);\nstd::string Base64Encode(const Image3ub& image);\n// Encodes some data in base 64.\nstd::string Base64Encode(const uint8_t* bytes_to_encode, size_t in_len);\n// Decode a base 64 encoded string into its original format.\nstd::string Base64Decode(const std::string& encoded_string);\n\n}  // namespace serialization\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/serialization/json.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <map>\n#include <optional>\n#include <string>\n#include <utility>\n\n#include \"gems/core/optional.hpp\"\n#include \"third_party/nlohmann/json.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// We are using nlohmann::json as JSON\nusing Json = nlohmann::json;\n\nnamespace serialization {\n\n// Loads and parses a JSON object from a file\nJson LoadJsonFromFile(const std::string& filename);\n// Parses a JSON object from a text string\n// @deprecated\nJson LoadJsonFromText(const std::string& text);\n// Checks if the given file does not exist or is empty\nbool IsFileEmpty(const std::string& filename);\n// Appends two json objects into one: if the duplicate keys are found in the input objects,\n// values are appended as array in the output object, second value is appended at back of array\n// only works for the top level json. Combines the unqiue keys found in the input objects.\nJson AppendJson(const Json& a, const Json& b);\n\n// Loads and parses a JSON object from a file without ASSERT\n// Returns nullopt in case of failure\nstd::optional<Json> TryLoadJsonFromFile(const std::string& filename);\n\n// Writes JSON to a file\nbool WriteJsonToFile(const std::string& filename, const Json& json);\n\n// Merges two JSON objects into one: if duplicate keys are found in the input objects,\n// the value of the second object is written to the merged object.\n// Combines the unqiue keys found in the input objects.\nJson MergeJson(const Json& a, const Json& b);\n\n// Replace keys in the top level of json. In the key_map, the first value is the existing key to\n// replace and second value is the new key with which to replace it.\n// Returns the number of keys that were replaced (renaming a key to same name counts as a\n// replacement).\nint ReplaceJsonKeys(const std::map<std::string, std::string>& key_map, Json& json);\n\n// Parses a JSON object from a text string\nstd::optional<Json> ParseJson(const std::string& text);\n// Parses a JSON object from a string coming from an object such as a JSON or YAML.\n// The string should be enclosed inside \"\" or '' and the internal strings have their delimiter\n// escaped. For example: \"{\\\"age\\\": 42}\".\nstd::optional<Json> ParseJsonFromString(const std::string& str);\n\n// Helper class for combining values in multiple JSON files and/or objects into one JSON object\nclass JsonOperator {\n public:\n  using OperationFunction = std::function<Json(const Json&, const Json&)>;\n\n  JsonOperator(OperationFunction operation_func) : operation_function_(operation_func) {}\n\n  // Add JSON file to be combined\n  JsonOperator& withFile(const std::string& json_filename) & {\n    json_ = operation_function_(json_, LoadJsonFromFile(json_filename));\n    return *this;\n  }\n\n  // Add JSON file to be combined\n  JsonOperator&& withFile(const std::string& json_filename) && {\n    json_ = operation_function_(json_, LoadJsonFromFile(json_filename));\n    return std::move(*this);\n  }\n\n  // Add JSON object to be combined\n  JsonOperator& withJson(const Json& json) & {\n    json_ = operation_function_(json_, json);\n    return *this;\n  }\n\n  // Add JSON object to be combined\n  JsonOperator&& withJson(const Json& json) && {\n    json_ = operation_function_(json_, json);\n    return std::move(*this);\n  }\n\n  // Convert to JSON using a copy\n  operator Json() const& { return json_; }\n\n  // Convert to JSON using a move\n  operator Json() && { return std::move(json_); }\n\n private:\n  // Operation function to combine JSON\n  OperationFunction operation_function_;\n  // JSON being combined\n  Json json_;\n};\n\n}  // namespace serialization\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/serialization/json_formatter.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <array>\n#include <map>\n#include <optional>\n#include <string>\n#include <type_traits>\n#include <utility>\n#include <vector>\n\n#include \"gems/core/image/image.hpp\"\n#include \"gems/core/math/pose2.hpp\"\n#include \"gems/core/math/pose3.hpp\"\n#include \"gems/core/math/so2.hpp\"\n#include \"gems/core/math/so3.hpp\"\n#include \"gems/core/math/types.hpp\"\n#include \"gems/core/optional.hpp\"\n#include \"gems/geometry/line_segment.hpp\"\n#include \"gems/geometry/n_cuboid.hpp\"\n#include \"gems/geometry/n_sphere.hpp\"\n#include \"gems/serialization/json.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nusing Json = nlohmann::json;\n\nnamespace serialization {\n\n// Gets the value from the json for the given key.\ntemplate <typename T>\nstd::optional<T> TryGet(const Json& json);\n\n// Sets a value in the json under the given key.\ntemplate <typename T>\nvoid Set(Json& json, T value);\n\n// Extracts the value for the given key from a JSON object. If the key does not exist in the JSON\n// object, or if the type does not match the desired type nullopt will be returned.\ntemplate <typename T>\nstd::optional<T> TryGetFromMap(const Json& json, const std::string& key) {\n  try {\n    const auto it = json.find(key);\n    if (it == json.end()) {\n      return std::nullopt;\n    } else {\n      return TryGet<T>(*it);\n    }\n  } catch (...) {\n    return std::nullopt;\n  }\n}\n\n// Similar to `TryGetFromMap`, but will use the given default if the value could not be read\ntemplate <typename T>\nT GetFromMapOrDefault(const Json& json, const std::string& key, T backup) {\n  if (auto maybe = TryGetFromMap<T>(json, key)) {\n    return *maybe;\n  } else {\n    return std::move(backup);\n  }\n}\n\n// Given key_prefix, tries to get the angle both in radians and degrees, e.g.,\n// if the key_prefix is \"yaw\", checks both \"yaw_radians\" and \"yaw_degrees\" values.\n// If only one of these keys exist, returns the angle in radians.\ntemplate <class T, typename = std::enable_if_t<std::is_floating_point<T>::value>>\nstd::optional<T> TryGetAngleInRadiansFromMap(const Json& json, const std::string& key_prefix) {\n  const std::string key_radians = key_prefix + \"_radians\";\n  const std::string key_degrees = key_prefix + \"_degrees\";\n  auto maybe_radians = TryGetFromMap<T>(json, key_radians);\n  auto maybe_degrees = TryGetFromMap<T>(json, key_degrees);\n  if (maybe_radians && maybe_degrees) {\n    LOG_ERROR(\"Please set \\\"%s\\\" or \\\"%s\\\" only.\", key_radians.c_str(), key_degrees.c_str());\n    return std::nullopt;\n  } else if (maybe_radians) {\n    return *maybe_radians;\n  } else if (maybe_degrees) {\n    return DegToRad(*maybe_degrees);\n  }\n  // Angle is not set\n  return std::nullopt;\n}\n\n//--------------------------------------------------------------------------------------------------\n\nnamespace json_formatter_details {\n\n// A helper class used to format data for json.\n// Synposis:\n// static std::optional<T> TryGet(const Json::json& json);\n// static void Set(const Json::json& json, T value);\ntemplate <typename T>\nstruct JsonSerializer {\n  static std::optional<T> TryGet(const Json& json) {\n    try {\n      T x;\n      from_json(json, x);\n      return std::move(x);\n    } catch (...) {\n      return std::nullopt;\n    }\n  }\n  static void Set(Json& json, T value) { to_json(json, value); }\n};\n\n// Support for getting JSON from JSON\ntemplate <>\nstruct JsonSerializer<Json> {\n  static const Json& TryGet(const Json& json) { return json; }\n  static void Set(Json& json, Json value) { json = std::move(value); }\n};\n\n// Parameter support for basic types: string, int, float, double, bool\n#define ISAAC_JSON_FORMATTER_FLOAT_PRIMITIVE(TYPE)              \\\n  template <>                                                   \\\n  struct JsonSerializer<TYPE> {                                 \\\n    static std::optional<TYPE> TryGet(const Json& json) {       \\\n      const double* double_ptr = json.get_ptr<const double*>(); \\\n      if (double_ptr != nullptr) {                              \\\n        return static_cast<TYPE>(*double_ptr);                  \\\n      }                                                         \\\n      const int64_t* int_ptr = json.get_ptr<const int64_t*>();  \\\n      if (int_ptr == nullptr) {                                 \\\n        return std::nullopt;                                    \\\n      } else {                                                  \\\n        return static_cast<TYPE>(*int_ptr);                     \\\n      }                                                         \\\n    }                                                           \\\n    static void Set(Json& json, TYPE value) {                   \\\n      json = static_cast<double>(value);                        \\\n    }                                                           \\\n  };\nISAAC_JSON_FORMATTER_FLOAT_PRIMITIVE(float);\nISAAC_JSON_FORMATTER_FLOAT_PRIMITIVE(double);\n// Parameter support for basic types: string, int, float, double, bool\n#define ISAAC_JSON_FORMATTER_PRIMITIVE(TYPE, JTYPE)       \\\n  template <>                                             \\\n  struct JsonSerializer<TYPE> {                           \\\n    static std::optional<TYPE> TryGet(const Json& json) { \\\n      const JTYPE* ptr = json.get_ptr<const JTYPE*>();    \\\n      if (ptr == nullptr) {                               \\\n        return std::nullopt;                              \\\n      } else {                                            \\\n        return static_cast<TYPE>(*ptr);                   \\\n      }                                                   \\\n    }                                                     \\\n    static void Set(Json& json, TYPE value) {             \\\n      json = static_cast<JTYPE>(value);                   \\\n    }                                                     \\\n  };\nISAAC_JSON_FORMATTER_PRIMITIVE(uint64_t, uint64_t);\nISAAC_JSON_FORMATTER_PRIMITIVE(int64_t, int64_t);\nISAAC_JSON_FORMATTER_PRIMITIVE(int, int64_t);\nISAAC_JSON_FORMATTER_PRIMITIVE(bool, bool);\nISAAC_JSON_FORMATTER_PRIMITIVE(std::string, std::string);\n\n// Parameter support for compile-sized Vectors. It uses doubles to store elements in the json.\ntemplate <typename K, int N>\nstruct JsonSerializer<Vector<K, N>> {\n  static std::optional<Vector<K, N>> TryGet(const Json& json) {\n    if (!json.is_array()) {\n      return std::nullopt;\n    }\n    const int json_size = static_cast<int>(json.size());\n    if (N != Eigen::Dynamic && json_size != N) {\n      return std::nullopt;\n    }\n    Vector<K, N> result(json_size);\n    for (int i = 0; i < json_size; i++) {\n      auto maybe = ::nvidia::isaac::serialization::TryGet<K>(json[i]);\n      if (!maybe) {\n        return std::nullopt;\n      }\n      result[i] = *maybe;\n    }\n    return result;\n  }\n  static void Set(Json& json, const Vector<K, Eigen::Dynamic>& value) {\n    json = Json(std::vector<K>(value.data(), value.data() + value.size()));\n  }\n  template <int M = N>\n  static void Set(Json& json, const Vector<K, M>& value) {\n    std::array<K, M> data;\n    for (int i = 0; i < M; i++) {\n      data[i] = value[i];\n    }\n    json = Json(data);\n  }\n};\n\n// For std::vector\ntemplate <typename T>\nstruct JsonSerializer<std::vector<T>> {\n  static std::optional<std::vector<T>> TryGet(const Json& json) {\n    if (!json.is_array()) {\n      return std::nullopt;\n    }\n    const size_t size = json.size();\n    std::vector<T> result;\n    result.reserve(size);\n    for (size_t i = 0; i < size; i++) {\n      auto maybe = ::nvidia::isaac::serialization::TryGet<T>(json[i]);\n      if (!maybe) {\n        return std::nullopt;\n      }\n      result.push_back(*maybe);\n    }\n    return result;\n  }\n  static void Set(Json& json, const std::vector<T>& value) {\n    json = Json::array();\n    for (const auto& v : value) {\n      Json vjson;\n      ::nvidia::isaac::serialization::Set(vjson, v);\n      json.push_back(vjson);\n    }\n  }\n};\n\n// For std::array\ntemplate <typename T, size_t N>\nstruct JsonSerializer<std::array<T, N>> {\n  static std::optional<std::array<T, N>> TryGet(const Json& json) {\n    if (!json.is_array()) {\n      return std::nullopt;\n    }\n    if (json.size() != N) {\n      return std::nullopt;\n    }\n    std::array<T, N> result;\n    for (size_t i = 0; i < N; i++) {\n      auto maybe = ::nvidia::isaac::serialization::TryGet<T>(json[i]);\n      if (!maybe) {\n        return std::nullopt;\n      }\n      result[i] = *maybe;\n    }\n    return result;\n  }\n  static void Set(Json& json, const std::array<T, N>& value) {\n    json = Json::array();\n    // TODO reserve\n    for (const auto& v : value) {\n      Json vjson;\n      ::nvidia::isaac::serialization::Set(vjson, v);\n      json.push_back(vjson);\n    }\n  }\n};\n\n// For std::pair\ntemplate <typename T1, typename T2>\nstruct JsonSerializer<std::pair<T1, T2>> {\n  static std::optional<std::pair<T1, T2>> TryGet(const Json& json) {\n    if (!json.is_array()) {\n      return std::nullopt;\n    }\n    if (json.size() != 2) {\n      return std::nullopt;\n    }\n    auto maybe1 = ::nvidia::isaac::serialization::TryGet<T1>(json[0]);\n    if (!maybe1) {\n      return std::nullopt;\n    }\n    auto maybe2 = ::nvidia::isaac::serialization::TryGet<T2>(json[1]);\n    if (!maybe2) {\n      return std::nullopt;\n    }\n    return std::pair<T1, T2>{*maybe1, *maybe2};\n  }\n  static void Set(Json& json, const std::pair<T1, T2>& value) {\n    Json value1;\n    ::nvidia::isaac::serialization::Set(value1, value.first);\n    Json value2;\n    ::nvidia::isaac::serialization::Set(value2, value.second);\n    json = Json::array({value1, value2});\n  }\n};\n\n// For std::map<std::string, T>\n// Supporting general std::map<T1, T2> would require array json, which we want to avoid for the time\n// being\ntemplate <typename T>\nstruct JsonSerializer<std::map<std::string, T>> {\n  static std::optional<std::map<std::string, T>> TryGet(const Json& json) {\n    if (!json.is_object()) {\n      return std::nullopt;\n    }\n    std::map<std::string, T> result;\n    for (const auto& val : json.items()) {\n      auto maybe = ::nvidia::isaac::serialization::TryGet<T>(val.value());\n      if (!maybe) {\n        return std::nullopt;\n      }\n      result.emplace(val.key(), *maybe);\n    }\n    return result;\n  }\n  static void Set(Json& json, const std::map<std::string, T>& value) {\n    json = Json::object();\n    for (const auto& v : value) {\n      json.emplace(v.first, v.second);\n    }\n  }\n};\n\n// Parameter support for SO3. It uses doubles to store elements in the json.\ntemplate <typename K>\nstruct JsonSerializer<SO3<K>> {\n  // Tries to get pose in two ways. The first method is kept for backward compatibility.\n  static std::optional<SO3<K>> TryGet(const Json& json) {\n    // First method for backward compatibility.\n    // Expected four elements are (w, x, y, z) values for the quaternion.\n    // Example: [0.0, 0.0, 0.0, 1.0]\n    if (json.is_array()) {\n      auto maybe = JsonSerializer<Vector<K, 4>>::TryGet(json);\n      if (!maybe) {\n        return std::nullopt;\n      }\n      const auto& array = *maybe;\n      return SO3<K>::FromQuaternion(Quaternion<K>{array[0], array[1], array[2], array[3]});\n    }\n\n    // Second method for convenience.\n    // Rotation can set by one of:\n    // { \"axis\": [x, y, z], \"angle_radians\": a }\n    // { \"axis\": [x, y, z], \"angle_degrees\": a }\n    // { \"roll_radians\": r, \"pitch_radians\": p, \"yaw_radians\": y }\n    // { \"roll_degrees\": r, \"pitch_degrees\": p, \"yaw_degrees\": y }\n    // { \"qx\": x, \"qy\": y, \"qz\": z, \"qw\": w }\n\n    // We'll demand one and only one way to be used.\n    size_t num_rotation_ways_set = 0;\n    // If error message gets populated, this function will return null.\n    // This variable will help us prioritize num_rotation_ways_set check when printing an error.\n    std::string error_message;\n    SO3<K> rotation = SO3<K>::Identity();\n\n    // { \"axis\": [x, y, z], \"angle_radians\": a }\n    // { \"axis\": [x, y, z], \"angle_degrees\": a }\n    auto maybe_axis = TryGetFromMap<Vector3<K>>(json, \"axis\");\n    auto maybe_angle = TryGetAngleInRadiansFromMap<K>(json, \"angle\");\n    if (maybe_axis || maybe_angle) {\n      num_rotation_ways_set++;\n      if (maybe_angle && maybe_axis) {\n        rotation = SO3<K>::FromAngleAxis(*maybe_angle, *maybe_axis);\n      } else {\n        error_message = \"Both \\\"axis\\\" and \\\"angle\\\" need to be set.\";\n      }\n    }\n\n    // { \"roll_radians\": r, \"pitch_radians\": p, \"yaw_radians\": y }\n    // { \"roll_degrees\": r, \"pitch_degrees\": p, \"yaw_degrees\": y }\n    auto maybe_roll = TryGetAngleInRadiansFromMap<K>(json, \"roll\");\n    auto maybe_pitch = TryGetAngleInRadiansFromMap<K>(json, \"pitch\");\n    auto maybe_yaw = TryGetAngleInRadiansFromMap<K>(json, \"yaw\");\n    if (maybe_roll || maybe_pitch || maybe_yaw) {\n      num_rotation_ways_set++;\n      if (!maybe_roll) maybe_roll = 0.0;\n      if (!maybe_pitch) maybe_pitch = 0.0;\n      if (!maybe_yaw) maybe_yaw = 0.0;\n      rotation = SO3<K>::FromEulerAnglesRPY(*maybe_roll, *maybe_pitch, *maybe_yaw);\n    }\n\n    // { \"qx\": x, \"qy\": y, \"qz\": z, \"qw\": w }\n    auto maybe_qx = TryGetFromMap<K>(json, \"qx\");\n    auto maybe_qy = TryGetFromMap<K>(json, \"qy\");\n    auto maybe_qz = TryGetFromMap<K>(json, \"qz\");\n    auto maybe_qw = TryGetFromMap<K>(json, \"qw\");\n    if (maybe_qx || maybe_qy || maybe_qz || maybe_qw) {\n      num_rotation_ways_set++;\n      if (maybe_qx && maybe_qy && maybe_qz && maybe_qw) {\n        rotation =\n            SO3<K>::FromQuaternion(Quaternion<K>{*maybe_qw, *maybe_qx, *maybe_qy, *maybe_qz});\n      } else {\n        error_message = \"\\\"qx\\\", \\\"qy\\\", \\\"qz\\\" and \\\"qw\\\" all need to be set.\";\n      }\n    }\n\n    // check that only one way is used.\n    if (num_rotation_ways_set != 1) {\n      LOG_ERROR(\"Rotation can be one of:\");\n      LOG_ERROR(\"{ \\\"axis\\\": [x, y, z], \\\"angle\\\": a }\");\n      LOG_ERROR(\"{ \\\"roll\\\": r, \\\"pitch\\\": p, \\\"yaw\\\": y }\");\n      LOG_ERROR(\"{ \\\"qx\\\": x, \\\"qy\\\": y, \\\"qz\\\": z, \\\"qw\\\": w }\");\n      LOG_ERROR(\"Please set one and only one of them.\");\n\n      return std::nullopt;\n    }\n\n    // If we had an issue, print the error and return null\n    if (!error_message.empty()) {\n      LOG_ERROR(error_message.c_str());\n      return std::nullopt;\n    }\n\n    return rotation;\n  }\n  static void Set(Json& json, const SO3<K>& value) {\n    json = Json(std::array<K, 4>{\n        {value.quaternion().w(), value.quaternion().x(), value.quaternion().y(),\n         value.quaternion().z()}});\n  }\n};\n\n// Parameter support for NSphere (where N is the dimension of the sphere)\n// Input serialization formats\n//   In order of priority TryGet() will look in \"json\" for:\n//     (1) A std::pair for which the first component contains an array of N numbers which represent\n//         the center coordinates of the NSphere and the second component contains a single scalar\n//         value representing the radius of the NSphere.\n//         Ex. Json json = {{1.0, 2.0}, 3.0};\n//             represents a circle with center (1.0, 2.0) and radius 3.0\n//     (2) A Json object named \"center\" containing an array of N numbers which represent the center\n//         coordinates of the NSphere AND a Json object named \"radius\" containing a single scalar\n//         value representing the radius of the NSphere\n//         Ex. Json json;\n//             json[\"center\"] = {1.0, 2.0, 3.0};\n//             json[\"radius\"] = 4.0;\n//             represents a sphere with center (1.0, 2.0, 3.0) and radius 4.0\n//   If neither format is available, std::nullopt will be returned.\n// Output serialization format:\n//   A std::pair for which the first component contains an array of N numbers which represent the\n//   center coordinates of the NSphere and the second component contains a single scalar value\n//   representing the radius of the NSphere.\ntemplate <typename K, int N>\nstruct JsonSerializer<geometry::NSphere<K, N>> {\n  using Sphere_t = geometry::NSphere<K, N>;\n  using Vector_t = typename geometry::NSphere<K, N>::Vector_t;\n  using Scalar_t = typename geometry::NSphere<K, N>::Scalar;\n  static std::optional<Sphere_t> TryGet(const Json& json) {\n    // Try to get NSphere from format: {center_vector, radius_scalar}\n    auto maybe = JsonSerializer<std::pair<Vector_t, Scalar_t>>::TryGet(json);\n    if (maybe) {\n      return Sphere_t{maybe->first, maybe->second};\n    }\n\n    // Try to get NSphere from named attributes for center and radius\n    auto maybe_center = TryGetFromMap<Vector_t>(json, \"center\");\n    auto maybe_radius = TryGetFromMap<Scalar_t>(json, \"radius\");\n    if (maybe_center && maybe_radius) {\n      return Sphere_t{*maybe_center, *maybe_radius};\n    }\n\n    return std::nullopt;\n  }\n  static void Set(Json& json, const Sphere_t& value) {\n    ::nvidia::isaac::serialization::Set(\n        json, std::pair<Vector_t, Scalar_t>{value.center, value.radius});\n  }\n};\n\n// Parameter support for NCuboid (where N is the dimension of the cuboid)\n// Input serialization formats\n//   In order of priority TryGet() will look in \"json\" for:\n//     (1) A std::array of length N, for which each component contains an the minimum and maximum\n//         bound for the Nth dimension.\n//         Ex. Json json = {{-1.0, 1.0}, {-2.0, 2.0}};\n//             represents a rectangle with the x dimension spanning -1.0 to 1.0 and the y dimension\n//             spanning -2.0 to 2.0.\n//     (2) A Json object named \"bounds\" containing an array of length the N, for which each\n//         component contains an the minimum and maximum bound for that dimension.\n//         Ex. Json json;\n//             json[\"bounds\"] = {{-1.0, 1.0}, {-2.0, 2.0}};\n//             represents a rectangle with the x dimension spanning -1.0 to 1.0 and the y dimension\n//             spanning -2.0 to 2.0.\n//     (3) A Json object named \"corner_1\" containing an array of length the N representing the\n//         coordinates of one corner AND another Json object named \"corner_2\" containing an array of\n//         length the N representing the coordinates of the opposite corner.\n//         Ex. Json json;\n//             json[\"corner_1\"] = {-1.0, -2.0};\n//             json[\"corner_2\"] = { 1.0,  2.0};\n//             represents a rectangle with the x dimension spanning -1.0 to 1.0 and the y dimension\n//             spanning -2.0 to 2.0.\n//     (4) A Json object named \"center\" containing an array of length the N representing the\n//         coordinates of the center of the NCuboid AND another Json object named \"dimensions\"\n//         containing an array of length the N representing the length of each dimension.\n//         Ex. Json json;\n//             json[\"center\"] = {0.0, 0.0};\n//             json[\"dimension\"] = {2.0, 4.0};\n//             represents a rectangle with the x dimension spanning -1.0 to 1.0 and the y dimension\n//             spanning -2.0 to 2.0.\n//   If none of these formats are available, std::nullopt will be returned.\n// Output serialization format:\n//   A std::array of length N, for which each component contains an the minimum and maximum\n//   bound for the Nth dimension.\ntemplate <typename K, int N>\nstruct JsonSerializer<geometry::NCuboid<K, N>> {\n  using Cube_t = geometry::NCuboid<K, N>;\n  using Vector_t = typename geometry::NCuboid<K, N>::Vector_t;\n  using Vector2K = Vector<K, 2>;\n  using ArrayNVector2K = std::array<Vector2K, N>;\n  static std::optional<Cube_t> TryGet(const Json& json) {\n    // Try to get NCuboid from bounding cuboid format:\n    // {{min_dim_0, max_dim_0}, {min_dim_1, max_dim_1}, ... {min_dim_N, max_dim_N}}\n    auto maybe = JsonSerializer<std::array<Vector2K, N>>::TryGet(json);\n    if (maybe) {\n      return Cube_t::FromBoundingCuboid(*maybe);\n    }\n\n    // Try to get NCuboid from named bounds\n    auto maybe_bounds = TryGetFromMap<ArrayNVector2K>(json, \"bounds\");\n    if (maybe_bounds) {\n      return Cube_t::FromBoundingCuboid(*maybe_bounds);\n    }\n\n    // Try to get NCuboid from named opposite corners\n    auto maybe_corner_1 = TryGetFromMap<Vector_t>(json, \"corner_1\");\n    auto maybe_corner_2 = TryGetFromMap<Vector_t>(json, \"corner_2\");\n    if (maybe_corner_1 && maybe_corner_2) {\n      return Cube_t::FromOppositeCorners(*maybe_corner_1, *maybe_corner_2);\n    }\n\n    // Try to get NCuboid from named center and dimensions\n    auto maybe_center = TryGetFromMap<Vector_t>(json, \"center\");\n    auto maybe_dimensions = TryGetFromMap<Vector_t>(json, \"dimensions\");\n    if (maybe_center && maybe_dimensions) {\n      return Cube_t::FromSizes(*maybe_center, *maybe_dimensions);\n    }\n\n    return std::nullopt;\n  }\n  static void Set(Json& json, const Cube_t& value) {\n    ArrayNVector2K bounds;\n    for (size_t dim = 0; dim < N; dim++) {\n      bounds[dim] = Vector2K(value.min()[dim], value.max()[dim]);\n    }\n    ::nvidia::isaac::serialization::Set(json, ArrayNVector2K{bounds});\n  }\n};\n\n// Parameter support for LineSegment\n// Input serialization formats\n//   In order of priority TryGet() will look in \"json\" for:\n//     (1) A std::pair for which the first component contains an array of N numbers which represent\n//         the coordinates of the first endpoint and the second component contains an array of N\n//         numbers which represent the coordinates of the second endpoint.\n//         Ex. Json json = {{1.0, 2.0}, {3.0, 4.0}};\n//             represents a 2D line segment spanning from (1.0, 2.0) to (3.0, 4.0).\n//     (2) A Json object named \"point_1\" containing an array of N numbers which represent the\n//         coordinates of the first endpoint AND a Json object named \"point_2\" containing an array\n//         of N numbers which represent the coordinates of the second endpoint.\n//         Ex. Json json;\n//             json[\"point_1\"] = {1.0, 2.0};\n//             json[\"point_2\"] = {3.0, 4.0};\n//             represents a 2D line segment spanning from (1.0, 2.0) to (3.0, 4.0).\n//   If neither format is available, std::nullopt will be returned.\n// Output serialization format:\n//   A std::pair for which the first component contains an array of N numbers which represent the\n//   coordinates of the first endpoint and the second component contains an array of N numbers which\n//   represent the coordinates of the second endpoint.\ntemplate <typename K, int N>\nstruct JsonSerializer<geometry::LineSegment<K, N>> {\n  using LineSegment_t = geometry::LineSegment<K, N>;\n  using Vector_t = typename geometry::LineSegment<K, N>::Vector_t;\n  static std::optional<LineSegment_t> TryGet(const Json& json) {\n    // Try to get LineSegment from format: {point_1_vector, point_2_vector}\n    auto maybe = JsonSerializer<std::pair<Vector_t, Vector_t>>::TryGet(json);\n    if (maybe) {\n      return LineSegment_t{maybe->first, maybe->second};\n    }\n\n    // Try to get LineSegment from named attributes for point_1 and point_2\n    auto maybe_point_1 = TryGetFromMap<Vector_t>(json, \"point_1\");\n    auto maybe_point_2 = TryGetFromMap<Vector_t>(json, \"point_2\");\n    if (maybe_point_1 && maybe_point_2) {\n      return LineSegment_t{*maybe_point_1, *maybe_point_2};\n    }\n\n    return std::nullopt;\n  }\n  static void Set(Json& json, const LineSegment_t& value) {\n    ::nvidia::isaac::serialization::Set(json, std::pair<Vector_t, Vector_t>{value.a(), value.b()});\n  }\n};\n\n// Parameter support for Pose3. It uses doubles to store elements in the json.\ntemplate <typename K>\nstruct JsonSerializer<Pose3<K>> {\n  // Tries to get pose in two ways. The first method is kept for backward compatibility.\n  static std::optional<Pose3<K>> TryGet(const Json& json) {\n    // First method for backward compatibility.\n    // First four elements are (w, x, y, z) values for the quaternion.\n    // Last three elements are (x, y, z) locations.\n    // Example: [0.0, 0.0, 0.0, 1.0, 25.0, 20.0, 0.0]\n    if (json.is_array()) {\n      auto maybe = JsonSerializer<Vector<K, 7>>::TryGet(json);\n      if (!maybe) {\n        return std::nullopt;\n      }\n      const auto& array = *maybe;\n      return Pose3<K>{\n          SO3<K>::FromQuaternion(Quaternion<K>{array[0], array[1], array[2], array[3]}),\n          Vector3<K>{array[4], array[5], array[6]}};\n    }\n\n    // Second method for convenience.\n    // User can supply a translation. If not supplied, zero translation will be used.\n    // User can also supply a rotation. If not supplied, identity matrix (zero rotation) will be\n    // used.\n    // The expected format is:\n    // {\n    //    \"translation\": [x, y, z],\n    //    \"rotation\": R\n    // }\n    // where acceptable formats for R are listed in SO3 serialization.\n    Vector3<K> translation =\n        GetFromMapOrDefault<Vector3<K>>(json, \"translation\", Vector3<K>::Zero());\n    SO3<K> rotation = GetFromMapOrDefault<SO3<K>>(json, \"rotation\", SO3<K>::Identity());\n    return Pose3<K>{rotation, translation};\n  }\n  static void Set(Json& json, const Pose3<K>& value) {\n    json = Json(std::array<K, 7>{\n        {value.rotation.quaternion().w(), value.rotation.quaternion().x(),\n         value.rotation.quaternion().y(), value.rotation.quaternion().z(), value.translation.x(),\n         value.translation.y(), value.translation.z()}});\n  }\n};\n\n// Parameter support for Pose2. It uses doubles to store elements in the json.\ntemplate <typename K>\nstruct JsonSerializer<Pose2<K>> {\n  static std::optional<Pose2<K>> TryGet(const Json& json) {\n    auto maybe = JsonSerializer<Vector<K, 3>>::TryGet(json);\n    if (!maybe) {\n      return std::nullopt;\n    }\n    const auto& array = *maybe;\n    return Pose2<K>{SO2<K>::FromAngle(array[0]), Vector2<K>{array[1], array[2]}};\n  }\n  static void Set(Json& json, const Pose2<K>& value) {\n    json = Json(\n        std::array<K, 3>{{value.rotation.angle(), value.translation.x(), value.translation.y()}});\n  }\n};\n\n// Helper function to validate json color inputs for Pixel3ub and Pixel4ub\ninline bool ValidateColorInput(const std::vector<int>& color_array) {\n  if (!(color_array.size() == 3 || color_array.size() == 4)) {\n    LOG_WARNING(\"Color must have three (RGB) or four (RGBA) components\");\n    return false;\n  }\n  for (int component : color_array) {\n    if (!(component >= 0 && component <= 255)) {\n      LOG_WARNING(\"Color component must be in range 0-255\");\n      return false;\n    }\n  }\n  return true;\n}\n\n// Parameter support for Pixel3ub. It uses uint8_t to store elements in the json.\ntemplate <>\nstruct JsonSerializer<Pixel3ub> {\n  static std::optional<Pixel3ub> TryGet(const Json& json) {\n    auto maybe = JsonSerializer<std::vector<int>>::TryGet(json);\n    if (!maybe) {\n      return std::nullopt;\n    }\n    const auto& array = *maybe;\n    if (ValidateColorInput(array)) {\n      return Pixel3ub{\n          static_cast<uint8_t>(array[0]),\n          static_cast<uint8_t>(array[1]),\n          static_cast<uint8_t>(array[2]),\n      };\n    } else {\n      LOG_WARNING(\"Invalid color input\");\n      return std::nullopt;\n    }\n  }\n  static void Set(Json& json, const Pixel3ub& value) {\n    json = Json(std::array<uint8_t, 3>{{value[0], value[1], value[2]}});\n  }\n};\n\n// Parameter support for Pixel4ub. It uses uint8_t to store elements in the json.\ntemplate <>\nstruct JsonSerializer<Pixel4ub> {\n  static std::optional<Pixel4ub> TryGet(const Json& json) {\n    auto maybe = JsonSerializer<std::vector<int>>::TryGet(json);\n    if (!maybe) {\n      return std::nullopt;\n    }\n    auto& array = *maybe;\n    if (ValidateColorInput(array)) {\n      // Add opaque alpha channel if not supplied in Json file\n      if (array.size() == 3) {\n        array.push_back(255);\n      }\n\n      return Pixel4ub{\n          static_cast<uint8_t>(array[0]), static_cast<uint8_t>(array[1]),\n          static_cast<uint8_t>(array[2]), static_cast<uint8_t>(array[3])};\n    } else {\n      LOG_WARNING(\"Invalid color input\");\n      return std::nullopt;\n    }\n  }\n  static void Set(Json& json, const Pixel4ub& value) {\n    json = Json(std::array<uint8_t, 4>{{value[0], value[1], value[2], value[3]}});\n  }\n};\n\n}  // namespace json_formatter_details\n\ntemplate <typename T>\nstd::optional<T> TryGet(const Json& json) {\n  return json_formatter_details::JsonSerializer<T>::TryGet(json);\n}\n\ntemplate <typename T>\nvoid Set(Json& json, T value) {\n  json_formatter_details::JsonSerializer<T>::Set(json, std::move(value));\n}\n\n}  // namespace serialization\n}  // namespace isaac\n}  // namespace nvidia\n\nnamespace Eigen {\n\n// Conversion from Eigen types like Vector2d to Json\ntemplate <typename T>\nvoid to_json(nlohmann::json& json, const T& value) {\n  ::nvidia::isaac::serialization::Set(json, value);\n}\n\n// Conversion from Json to Eigen types like Vector2d\ntemplate <typename T>\nvoid from_json(const nlohmann::json& json, T& value) {\n  if (auto maybe = ::nvidia::isaac::serialization::TryGet<T>(json)) {\n    value = *maybe;\n  }\n}\n\n}  // namespace Eigen\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/sight/sop.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <memory>\n#include <optional>\n#include <type_traits>\n#include <utility>\n#include <vector>\n\n#include \"gems/sight/sop_geometry.hpp\"\n#include \"gems/sight/sop_serializer.hpp\"\n#include \"gems/sight/sop_style.hpp\"\n#include \"gems/sight/sop_transform.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace sight {\n\n// Contains sight operation as a Tree structure:\n//  - Each internal node contains a potential style (default for its children), a transformation to\n//    apply before rendering the children\n//  - Each leaf contains a simple primitive (or SopImage) to be rendered using the current default\n//    style and using the composition of all the transformation above.\nclass Sop : public SopSerializer {\n public:\n  // Simple constructor\n  Sop() = default;\n  Sop(Sop&&) = default;\n  Sop(const Sop&) = delete;\n\n  // Add a SopSerializer\n  Sop(SopSerializer* sop) {\n    list_.emplace_back(sop);\n  }\n\n  // Add a primitive without a transform or style\n  template <class Primitive, std::enable_if_t<std::is_base_of_v<SopSerializer, Primitive>, int> = 0>\n  void add(Primitive primitive) {\n    list_.emplace_back(new Primitive(std::move(primitive)));\n  }\n\n  // Add a primitive with a given transform\n  template <class Primitive, std::enable_if_t<std::is_base_of_v<SopSerializer, Primitive>, int> = 0>\n  void add(Primitive primitive, const SopTransform& transform) {\n    Sop* sop = new Sop(new Primitive(std::move(primitive)));\n    sop->transform = transform;\n    list_.emplace_back(static_cast<SopSerializer*>(sop));\n  }\n\n  // Add a primitive with a given style\n  template <class Primitive, std::enable_if_t<std::is_base_of_v<SopSerializer, Primitive>, int> = 0>\n  void add(Primitive primitive, const SopStyle& style) {\n    Sop* sop = new Sop(new Primitive(std::move(primitive)));\n    sop->style = style;\n    list_.emplace_back(static_cast<SopSerializer*>(sop));\n  }\n\n  // Add a primitive with a given transform and style\n  template <class Primitive, std::enable_if_t<std::is_base_of_v<SopSerializer, Primitive>, int> = 0>\n  void add(Primitive primitive, const SopStyle& style, const SopTransform& transform) {\n    Sop* sop = new Sop(new Primitive(std::move(primitive)));\n    sop->style = style;\n    sop->transform = transform;\n    list_.emplace_back(static_cast<SopSerializer*>(sop));\n  }\n\n  // Add a primitive with a given transform\n  template <class Primitive, typename... Args>\n  void add(const Args&... args) {\n    list_.emplace_back(new Primitive(args...));\n  }\n\n  // Add a primitive with a given transform\n  template <class Primitive, typename... Args>\n  void add(const SopTransform& transform, const Args&... args) {\n    Sop* sop = new Sop(new Primitive(args...));\n    sop->transform = transform;\n    list_.emplace_back(static_cast<SopSerializer*>(sop));\n  }\n\n  // Add a primitive with a given style\n  template <class Primitive, typename... Args>\n  void add(const SopStyle& style, const Args&... args) {\n    Sop* sop = new Sop(new Primitive(args...));\n    sop->style = style;\n    list_.emplace_back(static_cast<SopSerializer*>(sop));\n  }\n\n  // Add a primitive with a given transform and style\n  template <class Primitive, typename... Args>\n  void add(const SopStyle& style, const SopTransform& transform, const Args&... args) {\n    Sop* sop = new Sop(new Primitive(args...));\n    sop->style = style;\n    sop->transform = transform;\n    list_.emplace_back(static_cast<SopSerializer*>(sop));\n  }\n\n  template <typename F, std::enable_if_t<std::is_invocable_v<F, Sop&>, int> = 0>\n  void add(F f) {\n    Sop* sop = new Sop();\n    f(*sop);\n    list_.emplace_back(static_cast<SopSerializer*>(sop));\n  }\n\n  // Make a sop static, it will be rendered at the latest time from the PoseTree.\n  void makeStatic();\n\n  uint8_t key() const;\n\n  bool toBinary(BufferSerialization& buffer) const override;\n\n  bool fromBinary(BufferSerialization& buffer) override;\n\n  // Optional style to be apply to the children\n  std::optional<SopStyle> style;\n  // Optional transformation to apply to the the subtree\n  std::optional<SopTransform> transform;\n\n private:\n  bool static_ = false;\n  std::vector<std::unique_ptr<SopSerializer>> list_;\n};\n\n}  // namespace sight\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/sight/sop_asset.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n\n#include \"gems/sight/sop_serializer.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace sight {\n\n// Sight Operation Asset.\n// TODO(bbutin): Right it now it contains only a name to an asset, later we need to add information\n// about the asset as well.\nclass SopAsset : public SopSerializer {\n public:\n  // Creates empty SopAsset\n  SopAsset() = default;\n  SopAsset(SopAsset&&) = default;\n  ~SopAsset() override = default;\n\n  // Creates an asset with a name\n  SopAsset(std::string text);\n\n  // Construct a sop by calling fromBinary\n  SopAsset(BufferSerialization& buffer);\n\n  bool toBinary(BufferSerialization& buffer) const override;\n  bool fromBinary(BufferSerialization& buffer) override;\n\n private:\n  std::string name_ = \"\";\n};\n\n}  // namespace sight\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/sight/sop_image.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <memory>\n#include <utility>\n#include <vector>\n\n#include \"gems/core/image/image.hpp\"\n#include \"gems/core/math/types.hpp\"\n#include \"gems/image/io.hpp\"\n#include \"gems/sight/sop_serializer.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace sight {\n\n// Sight Operation Image.\n// Helper to serialize an image in JPG (default) or PNG format\nclass SopImage : public SopSerializer {\n public:\n  // Creates empty image\n  SopImage() = default;\n  SopImage(SopImage&&) = default;\n  ~SopImage() override = default;\n\n  // Create an image in png or jpg format and whether or not you flip it.\n  template <typename Image>\n  SopImage(const Image& img, bool png, bool flip) {\n    if (!img.empty()) {\n      flip_ = flip;\n      if (png) {\n        ::nvidia::isaac::EncodePng(img, image_);\n      } else {\n        ::nvidia::isaac::EncodeJpeg(img, /* quality = */ 50, image_);\n      }\n    }\n  }\n\n  // Create an image in png or jpg format.\n  template <typename Image>\n  SopImage(const Image& img, bool png) {\n    if (!img.empty()) {\n      flip_ = false;\n      if (png) {\n        ::nvidia::isaac::EncodePng(img, image_);\n      } else {\n        ::nvidia::isaac::EncodeJpeg(img, /* quality = */ 50, image_);\n      }\n    }\n  }\n\n  // Create an image in jpg format with a custom quality.\n  template <typename Image>\n  static SopImage Jpg(const Image& img, int quality) {\n    SopImage sop;\n    if (!img.empty()) {\n      sop.flip_ = false;\n      ::nvidia::isaac::EncodeJpeg(img, quality, sop.image_);\n    }\n    return sop;\n  }\n\n  // Create an image in jpg format with a custom quality  and whether or not you flip it.\n  template <typename Image>\n  static SopImage Jpg(const Image& img, int quality, bool flip) {\n    SopImage sop;\n    if (!img.empty()) {\n      sop.flip_ = flip;\n      ::nvidia::isaac::EncodeJpeg(img, quality, sop.image_);\n    }\n    return sop;\n  }\n\n  // Create an image in jpg format.\n  template <typename Image>\n  SopImage(const Image& img) {\n    if (!img.empty()) {\n      flip_ = false;\n      ::nvidia::isaac::EncodeJpeg(img, 50, image_);\n    }\n  }\n\n  // Construct a sop by calling fromBinary\n  SopImage(BufferSerialization& buffer);\n\n  bool toBinary(BufferSerialization& buffer) const override;\n  bool fromBinary(BufferSerialization& buffer) override;\n\n private:\n  bool flip_;\n  std::vector<uint8_t> image_;\n};\n\n}  // namespace sight\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/sight/sop_mesh.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <list>\n#include <string>\n#include <vector>\n\n#include \"gems/core/math/types.hpp\"\n#include \"gems/sight/sop_serializer.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace sight {\n\n// Sight Operation for a mesh.\nclass SopMesh : public SopSerializer {\n public:\n  SopMesh() = default;\n  SopMesh(SopMesh&&) = default;\n  ~SopMesh() override = default;\n\n  // Creates a new SopMesh, `append` indicates whether or not it's an update or if we should restart\n  // from scratch.\n  SopMesh(bool append);\n\n  // Adds a new block to the mesh.\n  // Parameters:\n  // - name: the name of the block.\n  // - num_points: the number of points (N)\n  // - points: a vector of coordinates, size 3N, contains [x0,y0,z0, x1,y1,z1, ...,]\n  // - num_triangles: the number of triangles, (M)\n  // - triangles: indices of points that form a triangle, size 3M, the first three indices\n  //              correspond to the first triangle, the next three to the second triangle, and so on\n  // - colors: if not null, it contains the color of each point in order. The format is RGB.\n  // - normals: if not null, it contains the normal vector of each point,  [nx0,ny0,nz0, ...]\n  void add(std::string name, int num_points, const float* points, int num_triangles,\n           const uint16_t* triangles, const uint8_t* colors = nullptr,\n           const float* normals = nullptr);\n  void add(std::string name, std::vector<float> points, std::vector<uint16_t> triangles,\n           std::vector<uint8_t> colors, std::vector<float> normals);\n\n  // Construct a sop by calling fromBinary\n  SopMesh(BufferSerialization& buffer);\n\n  bool toBinary(BufferSerialization& buffer) const override;\n  bool fromBinary(BufferSerialization& buffer) override;\n\n private:\n  // Hold the information about a single block.\n  struct Block {\n    std::string name;\n    std::vector<float> points;\n    std::vector<float> normals;\n    std::vector<uint8_t> colors;\n    std::vector<uint16_t> triangles;\n  };\n\n  bool append_ = true;\n  std::list<Block> blocks_;\n};\n\n}  // namespace sight\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/sight/sop_point_cloud.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <vector>\n\n#include \"gems/core/math/types.hpp\"\n#include \"gems/core/tensor/sample_cloud.hpp\"\n#include \"gems/sight/sop_serializer.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace sight {\n\n// Sight show operation for point clouds.\nclass SopPointCloud : public SopSerializer {\n public:\n  SopPointCloud() = default;\n  SopPointCloud(SopPointCloud&&) = default;\n  ~SopPointCloud() override = default;\n\n  // Creates a SopPointCloud with 2d points\n  SopPointCloud(::nvidia::isaac::SampleCloudConstView2f points, int downsample_stride = 1);\n  SopPointCloud(::nvidia::isaac::SampleCloudConstView2f points,\n                ::nvidia::isaac::SampleCloudConstView3ub colors,\n                int downsample_stride = 1);\n\n  // Creates a SopPointCloud with 3d points\n  SopPointCloud(::nvidia::isaac::SampleCloudConstView3f points, int downsample_stride = 1);\n  SopPointCloud(::nvidia::isaac::SampleCloudConstView3f points,\n                ::nvidia::isaac::SampleCloudConstView3ub colors,\n                int downsample_stride = 1);\n\n  // Construct a sop by calling fromBinary\n  SopPointCloud(BufferSerialization& buffer);\n\n  bool toBinary(BufferSerialization& buffer) const override;\n  bool fromBinary(BufferSerialization& buffer) override;\n\n private:\n  int dim_ = 0;\n  std::vector<float> pts_;\n  std::vector<uint8_t> colors_;\n};\n\n}  // namespace sight\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/sight/sop_style.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <optional>\n#include <string>\n#include <utility>\n\n#include \"gems/core/image/image.hpp\"\n#include \"gems/sight/sop_serializer.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace sight {\n\n// Sight Operation Style\n// Contains optional information to apply to the sight operation\nstruct SopStyle : public SopSerializer {\n  // Creates empty style\n  SopStyle() = default;\n  ~SopStyle() override = default;\n\n  // Creates a style with a color (valid javascript format: e.g. '#abc' '#123456' 'rgb(0, 1, 2)',\n  // 'white', etc...)\n  SopStyle(std::string color);\n  SopStyle(const char* color);\n  SopStyle(const ::nvidia::isaac::Pixel3ub& col);\n  SopStyle(const ::nvidia::isaac::Pixel4ub& col);\n\n  // Helper == operator for testing deserialize.\n  bool operator==(const SopStyle& style) const;\n\n  // Creates a style with a color and whether object should be filled or not.\n  template <typename C>\n  SopStyle(const C& color, bool fill) : SopStyle(color) {\n    fill_ = fill;\n  }\n\n  // Creates a style with a color, whether object should be filled or not, and a default size.\n  template <typename C>\n  SopStyle(const C& color, bool fill, double size) : SopStyle(color, fill) {\n    size_ = size;\n  }\n\n  // Creates a style with a color, whether object should be filled or not, a default size, and an\n  // alpha value to control transparency. If a Pixel4ub is provided for color, it's alpha component\n  // will be overwritten by the alpha value provided as a double.\n  template <typename C>\n  SopStyle(const C& color, bool fill, double size, double alpha) : SopStyle(color, fill, size) {\n    alpha_ = static_cast<uint8_t>(std::clamp(256.0 * alpha, 0.0, 255.0));\n  }\n\n  bool toBinary(BufferSerialization& buffer) const override;\n\n  bool fromBinary(BufferSerialization& buffer) override;\n\n private:\n  std::optional<std::string> color_str_;\n  std::optional<::nvidia::isaac::Pixel3ub> color_;\n  std::optional<uint8_t> alpha_;\n  std::optional<bool> fill_;\n  std::optional<float> size_;\n};\n\n}  // namespace sight\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/sight/sop_text.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <optional>\n#include <string>\n#include <utility>\n\n#include \"gems/core/math/types.hpp\"\n#include \"gems/sight/sop_serializer.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace sight {\n\n// Sight Operation Text\n// Can be used to display text at a given position\nclass SopText : public SopSerializer {\n public:\n  // Creates empty SopText\n  SopText() = default;\n  SopText(SopText&&) = default;\n  ~SopText() override = default;\n\n  // Creates a text with not other information (will be rendered at (0, 0))\n  SopText(std::string text);\n\n  // Creates a style with a color and wether object should be filled or not.\n  template <typename K>\n  SopText(std::string text, const ::nvidia::isaac::Vector2<K>& pos) : SopText(std::move(text)) {\n    position_ = pos.template cast<float>();\n  }\n\n  // Creates a SopText setup to center the text.\n  template <typename K>\n  SopText(std::string text, const ::nvidia::isaac::Vector2<K>& pos, bool center)\n      : SopText(std::move(text), pos) {\n    center_ = center;\n  }\n\n  // Construct a sop by calling fromBinary\n  SopText(BufferSerialization& buffer);\n\n  bool toBinary(BufferSerialization& buffer) const override;\n  bool fromBinary(BufferSerialization& buffer) override;\n\n private:\n  std::string text_ = \"\";\n  bool center_ = false;\n  std::optional<::nvidia::isaac::Vector2f> position_;\n};\n\n}  // namespace sight\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/sight/sop_transform.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <optional>\n#include <string>\n#include <utility>\n\n#include \"gems/core/math/pose2.hpp\"\n#include \"gems/core/math/pose3.hpp\"\n#include \"gems/geometry/pinhole.hpp\"\n#include \"gems/sight/sop_serializer.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\nnamespace sight {\n\n// Sight Operation Transform\n// Contains the Pose2 or Pose3 transform or frame as well as a scale factor and a pinhole model.\nstruct SopTransform : public SopSerializer {\n  // Creates empty transform\n  SopTransform() = default;\n  ~SopTransform() override = default;\n\n  // Creates a SopTransform using the canvas pixel frame (allow you to draw directly at a given\n  // position on a canvas).\n  // Note: this only works for 2D and the Sop will not be rendered in 3D.\n  static SopTransform CanvasFrame();\n\n  // Creates a SopTransform using a reference frame from uid\n  SopTransform(uint64_t uid);\n\n  // Creates a SopTransform using a reference frame\n  SopTransform(const std::string& reference_frame);\n\n  // Creates a SopTransform using a Pose2\n  template <typename K>\n  SopTransform(const ::nvidia::isaac::Pose2<K>& pose) {\n    pose2_ = pose.template cast<float>();\n  }\n\n  // Creates a SopTransform using a Pose3\n  template <typename K>\n  SopTransform(const ::nvidia::isaac::Pose3<K>& pose) {\n    pose3_ = pose.template cast<float>();\n  }\n\n  // Creates a transform with a Pose and a scale\n  template <typename Pose>\n  SopTransform(const Pose& pose, double scale) : SopTransform(pose) {\n    scale_ = scale;\n  }\n\n  // Creates a transform with a Pose and a pinhole projection\n  template <typename Pose, typename K>\n  SopTransform(const Pose& pose, const ::nvidia::isaac::geometry::Pinhole<K>& pinhole)\n  : SopTransform(pose) {\n    pinhole_ = pinhole.template cast<float>();\n  }\n\n  // Creates a transform with a Pose and a scale\n  template <typename Pose, typename K>\n  SopTransform(const Pose& pose, double scale, const ::nvidia::isaac::geometry::Pinhole<K>& pinhole)\n  : SopTransform(pose, pinhole) {\n    scale_ = scale;\n  }\n\n  // Helper == operator for testing deserialize.\n  bool operator==(const SopTransform& style) const;\n\n  bool toBinary(BufferSerialization& buffer) const override;\n\n  bool fromBinary(BufferSerialization& buffer) override;\n\n private:\n  bool canvas_ = false;\n  std::optional<uint64_t> uid_;\n  std::optional<std::string> frame_;\n  std::optional<::nvidia::isaac::Pose2f> pose2_;\n  std::optional<::nvidia::isaac::Pose3f> pose3_;\n  std::optional<float> scale_;\n  std::optional<::nvidia::isaac::geometry::Pinhole<float>> pinhole_;\n};\n\n}  // namespace sight\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/uuid/uuid.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <cstdint>\n#include <string>\n\nnamespace nvidia {\nnamespace isaac {\n\n// A universally unique identifier (UUID)\nclass Uuid {\n public:\n  static constexpr int kNumBytes = 16;\n\n  // Generates a new UUID\n  static Uuid Generate();\n  // Creates a UUID from a string. If the string has \"unparsed\" UUID format it will be parsed as a\n  // UUID string. Otherwise it will be treated as an ASCII string. An unparsed UUID string has the\n  // format \"XXXXXXXX-XXXX-XXXX-XXXX-XXXXXXXXXXXX\" with XXX alpha-numeric characters. May assert\n  // if neither form of parsing succeeds, e.g. if the string is too long.\n  static Uuid FromString(const std::string& str);\n  // Parses a string as an ASCII string. Will assert if the string is too long.\n  static Uuid FromAsciiString(const std::string& str);\n  // Creates a UUID from a serialized UUID string. This will assert in case the string does not\n  // have the right format.\n  static Uuid FromUuidString(const std::string& str);\n  // Creates UUID from two 64-bit integers\n  static Uuid FromUInt64(uint64_t lower, uint64_t upper) {\n    Uuid uuid;\n    uuid.longs_[0] = lower;\n    uuid.longs_[1] = upper;\n    uuid.is_uuid_ = true;  // FIXME\n    return uuid;\n  }\n\n  // Creates an invalid (whatever that means) UUID with all 0\n  Uuid() { longs_[0] = 0; longs_[1] = 0; is_uuid_ = false; }\n\n  // Iterator interface\n  const unsigned char* begin() const { return bytes_; }\n  unsigned char* begin() { return bytes_; }\n  const unsigned char* end() const { return bytes_ + kNumBytes; }\n  unsigned char* end() { return bytes_ + kNumBytes; }\n\n  uint64_t lower() const { return longs_[0]; }\n  uint64_t upper() const { return longs_[1]; }\n\n  // A hash value so that we can use the UUID as a key\n  size_t hash() const {\n    // Should be way faster than a string-based hash\n    return lower() ^ upper();\n  }\n\n  // Gets an unparsed string representation for the UUID\n  const std::string& str() const {\n    if (str_.empty()) {\n      unparse();\n    }\n    return str_;\n  }\n  // Same as str() but directly returns a null-terminated C-string\n  const char* c_str() const {\n    return str().c_str();\n  }\n\n  // Comparison operators\n  friend bool operator==(const Uuid& lhs, const Uuid& rhs) {\n    return lhs.lower() == rhs.lower() && lhs.upper() == rhs.upper();\n  }\n  friend bool operator!=(const Uuid& lhs, const Uuid& rhs) {\n    return !(lhs == rhs);\n  }\n  friend bool operator<(const Uuid& lhs, const Uuid& rhs) {\n    return lhs.upper() < rhs.upper()\n        || (lhs.upper() == rhs.upper() && lhs.lower() < rhs.lower());\n  }\n\n private:\n  // Use both 8-bit unsigned and bytes representations\n  union {\n    unsigned char bytes_[kNumBytes];\n    uint64_t longs_[2];\n  };\n\n  // True if this is actually a UUID and not a string chosen by the user.\n  bool is_uuid_;\n\n  // Cached string version of uuid used for printing\n  mutable std::string str_;\n\n  // Computes the unparsed string version of the UUID\n  void unparse() const;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n\nnamespace std {\n\n// Overload for std::hash to be compatible with standard containers\ntemplate <>\nstruct hash<nvidia::isaac::Uuid> {\n  size_t operator()(const nvidia::isaac::Uuid& x) const {\n    return x.hash();\n  }\n};\n\n}  // namespace std\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/gems/video_buffer/allocator.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <array>\n#include <vector>\n#include \"gxf/multimedia/video.hpp\"\n#include \"gxf/std/allocator.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\ntemplate <gxf::VideoFormat T>\nstruct NoPaddingColorPlanes {};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB> {\n  explicit NoPaddingColorPlanes(uint32_t width) : planes({gxf::ColorPlane(\"RGB\", 3, width * 3)}) {}\n  std::array<gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"BGR\", 3, width * 3)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_RGBA> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"RGBA\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_BGRA> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"BGRA\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB16> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"RGB\", 6, width * 6)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR16> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"BGR\", 6, width * 6)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB32> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"RGB\", 12, width * 12)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR32> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"BGR\", 12, width * 12)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"gray\", 1, width)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY16> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"gray\", 2, width * 2)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"gray\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32F> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"gray\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n                nvidia::gxf::ColorPlane(\"UV\", 2, width)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12_ER> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n                nvidia::gxf::ColorPlane(\"UV\", 2, width)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n                nvidia::gxf::ColorPlane(\"UV\", 2, width * 2)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24_ER> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n                nvidia::gxf::ColorPlane(\"UV\", 2, width * 2)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes({gxf::ColorPlane(\"R\", 1, width), gxf::ColorPlane(\"G\", 1, width),\n    gxf::ColorPlane(\"B\", 1, width)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_R16_G16_B16> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes({gxf::ColorPlane(\"R\", 2, width * 2), gxf::ColorPlane(\"G\", 2, width * 2),\n    gxf::ColorPlane(\"B\", 2, width * 2)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes(\n        {gxf::ColorPlane(\"R\", 4, width * 4), gxf::ColorPlane(\"G\", 4, width * 4),\n        gxf::ColorPlane(\"B\", 4, width * 4)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_B8_G8_R8> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes({gxf::ColorPlane(\"B\", 1, width), gxf::ColorPlane(\"G\", 1, width),\n    gxf::ColorPlane(\"R\", 1, width)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_B16_G16_R16> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes(\n        {gxf::ColorPlane(\"R\", 2, width * 2), gxf::ColorPlane(\"G\", 2, width * 2),\n        gxf::ColorPlane(\"B\", 2, width * 2)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_B32_G32_R32> {\n  explicit NoPaddingColorPlanes(size_t width)\n    : planes(\n        {gxf::ColorPlane(\"B\", 4, width * 4), gxf::ColorPlane(\"G\", 4, width * 4),\n        gxf::ColorPlane(\"R\", 4, width * 4)}) {}\n  std::array<gxf::ColorPlane, 3> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_D32F> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"D\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate <>\nstruct NoPaddingColorPlanes<gxf::VideoFormat::GXF_VIDEO_FORMAT_D64F> {\n  explicit NoPaddingColorPlanes(uint32_t width)\n      : planes({nvidia::gxf::ColorPlane(\"D\", 8, width * 8)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\n// This includes the list of video buffer formats that supported for the allocator\nconstexpr bool IsSupportedVideoFormat(const gxf::VideoFormat format) {\n  return format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGBA ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGRA ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB16 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR16 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB32 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_BGR32 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY16 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_GRAY32F ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_R8_G8_B8 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_B8_G8_R8 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_R16_G16_B16 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_B16_G16_R16 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_R32_G32_B32 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_B32_G32_R32 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV12_ER ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24 ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_NV24_ER ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_D32F ||\n         format == gxf::VideoFormat::GXF_VIDEO_FORMAT_D64F;\n}\n\ntemplate<gxf::VideoFormat T, typename std::enable_if<!IsSupportedVideoFormat(T)>::type* = nullptr>\ngxf::Expected<void> AllocateUnpaddedVideoBuffer(\n    gxf::Handle<gxf::VideoBuffer> frame, uint32_t width, uint32_t height,\n    gxf::MemoryStorageType storage_type,\n    gxf::Handle<gxf::Allocator> allocator) {\n  GXF_LOG_ERROR(\"Received unsupported video format!\");\n  return gxf::Unexpected{GXF_FAILURE};\n}\n\ntemplate<gxf::VideoFormat T, typename std::enable_if<IsSupportedVideoFormat(T)>::type* = nullptr>\ngxf::Expected<void> AllocateUnpaddedVideoBuffer(\n    gxf::Handle<gxf::VideoBuffer> frame, uint32_t width, uint32_t height,\n    gxf::MemoryStorageType storage_type, gxf::Handle<gxf::Allocator> allocator) {\n  if (width % 2 != 0 || height % 2 != 0) {\n    GXF_LOG_ERROR(\n        \"Error: expected even width and height but received %u width and %u height\",\n        width, height);\n    return gxf::Unexpected{GXF_FAILURE};\n  }\n  NoPaddingColorPlanes<T> nopadding_planes(width);\n  gxf::VideoFormatSize<T> video_format_size;\n  auto size = video_format_size.size(width, height, nopadding_planes.planes);\n  std::vector<gxf::ColorPlane> color_planes{\n      nopadding_planes.planes.begin(), nopadding_planes.planes.end()};\n  gxf::VideoBufferInfo buffer_info{width, height, T, color_planes,\n      gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR};\n  return frame->resizeCustom(buffer_info, size, storage_type, allocator);\n}\n\ntemplate<gxf::VideoFormat T>\ngxf::Expected<void> AllocateVideoBuffer(\n    gxf::Handle<gxf::VideoBuffer> frame, uint32_t width, uint32_t height,\n    gxf::SurfaceLayout layout, gxf::MemoryStorageType storage_type,\n    gxf::Handle<gxf::Allocator> allocator) {\n    return frame->resize<T>(width, height, layout, storage_type, allocator);\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/gxf/third_party/nlohmann/json.hpp",
    "content": "//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n/****************************************************************************\\\n * Note on documentation: The source files contain links to the online      *\n * documentation of the public API at https://json.nlohmann.me. This URL    *\n * contains the most recent documentation and should also be applicable to  *\n * previous versions; documentation for deprecated functions is not         *\n * removed, but marked deprecated. See \"Generate documentation\" section in  *\n * file docs/README.md.                                                     *\n\\****************************************************************************/\n\n#ifndef INCLUDE_NLOHMANN_JSON_HPP_\n#define INCLUDE_NLOHMANN_JSON_HPP_\n\n#include <algorithm> // all_of, find, for_each\n#include <cstddef> // nullptr_t, ptrdiff_t, size_t\n#include <functional> // hash, less\n#include <initializer_list> // initializer_list\n#ifndef JSON_NO_IO\n    #include <iosfwd> // istream, ostream\n#endif  // JSON_NO_IO\n#include <iterator> // random_access_iterator_tag\n#include <memory> // unique_ptr\n#include <numeric> // accumulate\n#include <string> // string, stoi, to_string\n#include <utility> // declval, forward, move, pair, swap\n#include <vector> // vector\n\n// #include <nlohmann/adl_serializer.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <utility>\n\n// #include <nlohmann/detail/abi_macros.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n// This file contains all macro definitions affecting or depending on the ABI\n\n#ifndef JSON_SKIP_LIBRARY_VERSION_CHECK\n    #if defined(NLOHMANN_JSON_VERSION_MAJOR) && defined(NLOHMANN_JSON_VERSION_MINOR) && defined(NLOHMANN_JSON_VERSION_PATCH)\n        #if NLOHMANN_JSON_VERSION_MAJOR != 3 || NLOHMANN_JSON_VERSION_MINOR != 11 || NLOHMANN_JSON_VERSION_PATCH != 2\n            #warning \"Already included a different version of the library!\"\n        #endif\n    #endif\n#endif\n\n#define NLOHMANN_JSON_VERSION_MAJOR 3   // NOLINT(modernize-macro-to-enum)\n#define NLOHMANN_JSON_VERSION_MINOR 11  // NOLINT(modernize-macro-to-enum)\n#define NLOHMANN_JSON_VERSION_PATCH 2   // NOLINT(modernize-macro-to-enum)\n\n#ifndef JSON_DIAGNOSTICS\n    #define JSON_DIAGNOSTICS 0\n#endif\n\n#ifndef JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON\n    #define JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON 0\n#endif\n\n#if JSON_DIAGNOSTICS\n    #define NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS _diag\n#else\n    #define NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS\n#endif\n\n#if JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON\n    #define NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON _ldvcmp\n#else\n    #define NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON\n#endif\n\n#ifndef NLOHMANN_JSON_NAMESPACE_NO_VERSION\n    #define NLOHMANN_JSON_NAMESPACE_NO_VERSION 0\n#endif\n\n// Construct the namespace ABI tags component\n#define NLOHMANN_JSON_ABI_TAGS_CONCAT_EX(a, b) json_abi ## a ## b\n#define NLOHMANN_JSON_ABI_TAGS_CONCAT(a, b) \\\n    NLOHMANN_JSON_ABI_TAGS_CONCAT_EX(a, b)\n\n#define NLOHMANN_JSON_ABI_TAGS                                       \\\n    NLOHMANN_JSON_ABI_TAGS_CONCAT(                                   \\\n            NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS,                       \\\n            NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON)\n\n// Construct the namespace version component\n#define NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT_EX(major, minor, patch) \\\n    _v ## major ## _ ## minor ## _ ## patch\n#define NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT(major, minor, patch) \\\n    NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT_EX(major, minor, patch)\n\n#if NLOHMANN_JSON_NAMESPACE_NO_VERSION\n#define NLOHMANN_JSON_NAMESPACE_VERSION\n#else\n#define NLOHMANN_JSON_NAMESPACE_VERSION                                 \\\n    NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT(NLOHMANN_JSON_VERSION_MAJOR, \\\n                                           NLOHMANN_JSON_VERSION_MINOR, \\\n                                           NLOHMANN_JSON_VERSION_PATCH)\n#endif\n\n// Combine namespace components\n#define NLOHMANN_JSON_NAMESPACE_CONCAT_EX(a, b) a ## b\n#define NLOHMANN_JSON_NAMESPACE_CONCAT(a, b) \\\n    NLOHMANN_JSON_NAMESPACE_CONCAT_EX(a, b)\n\n#ifndef NLOHMANN_JSON_NAMESPACE\n#define NLOHMANN_JSON_NAMESPACE               \\\n    nlohmann::NLOHMANN_JSON_NAMESPACE_CONCAT( \\\n            NLOHMANN_JSON_ABI_TAGS,           \\\n            NLOHMANN_JSON_NAMESPACE_VERSION)\n#endif\n\n#ifndef NLOHMANN_JSON_NAMESPACE_BEGIN\n#define NLOHMANN_JSON_NAMESPACE_BEGIN                \\\n    namespace nlohmann                               \\\n    {                                                \\\n    inline namespace NLOHMANN_JSON_NAMESPACE_CONCAT( \\\n                NLOHMANN_JSON_ABI_TAGS,              \\\n                NLOHMANN_JSON_NAMESPACE_VERSION)     \\\n    {\n#endif\n\n#ifndef NLOHMANN_JSON_NAMESPACE_END\n#define NLOHMANN_JSON_NAMESPACE_END                                     \\\n    }  /* namespace (inline namespace) NOLINT(readability/namespace) */ \\\n    }  // namespace nlohmann\n#endif\n\n// #include <nlohmann/detail/conversions/from_json.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <algorithm> // transform\n#include <array> // array\n#include <forward_list> // forward_list\n#include <iterator> // inserter, front_inserter, end\n#include <map> // map\n#include <string> // string\n#include <tuple> // tuple, make_tuple\n#include <type_traits> // is_arithmetic, is_same, is_enum, underlying_type, is_convertible\n#include <unordered_map> // unordered_map\n#include <utility> // pair, declval\n#include <valarray> // valarray\n\n// #include <nlohmann/detail/exceptions.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <cstddef> // nullptr_t\n#include <exception> // exception\n#include <stdexcept> // runtime_error\n#include <string> // to_string\n#include <vector> // vector\n\n// #include <nlohmann/detail/value_t.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <array> // array\n#include <cstddef> // size_t\n#include <cstdint> // uint8_t\n#include <string> // string\n\n// #include <nlohmann/detail/macro_scope.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <utility> // declval, pair\n// #include <nlohmann/detail/meta/detected.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <type_traits>\n\n// #include <nlohmann/detail/meta/void_t.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\ntemplate<typename ...Ts> struct make_void\n{\n    using type = void;\n};\ntemplate<typename ...Ts> using void_t = typename make_void<Ts...>::type;\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n// https://en.cppreference.com/w/cpp/experimental/is_detected\nstruct nonesuch\n{\n    nonesuch() = delete;\n    ~nonesuch() = delete;\n    nonesuch(nonesuch const&) = delete;\n    nonesuch(nonesuch const&&) = delete;\n    void operator=(nonesuch const&) = delete;\n    void operator=(nonesuch&&) = delete;\n};\n\ntemplate<class Default,\n         class AlwaysVoid,\n         template<class...> class Op,\n         class... Args>\nstruct detector\n{\n    using value_t = std::false_type;\n    using type = Default;\n};\n\ntemplate<class Default, template<class...> class Op, class... Args>\nstruct detector<Default, void_t<Op<Args...>>, Op, Args...>\n{\n    using value_t = std::true_type;\n    using type = Op<Args...>;\n};\n\ntemplate<template<class...> class Op, class... Args>\nusing is_detected = typename detector<nonesuch, void, Op, Args...>::value_t;\n\ntemplate<template<class...> class Op, class... Args>\nstruct is_detected_lazy : is_detected<Op, Args...> { };\n\ntemplate<template<class...> class Op, class... Args>\nusing detected_t = typename detector<nonesuch, void, Op, Args...>::type;\n\ntemplate<class Default, template<class...> class Op, class... Args>\nusing detected_or = detector<Default, void, Op, Args...>;\n\ntemplate<class Default, template<class...> class Op, class... Args>\nusing detected_or_t = typename detected_or<Default, Op, Args...>::type;\n\ntemplate<class Expected, template<class...> class Op, class... Args>\nusing is_detected_exact = std::is_same<Expected, detected_t<Op, Args...>>;\n\ntemplate<class To, template<class...> class Op, class... Args>\nusing is_detected_convertible =\n    std::is_convertible<detected_t<Op, Args...>, To>;\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/thirdparty/hedley/hedley.hpp>\n\n\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-FileCopyrightText: 2016-2021 Evan Nemerson <evan@nemerson.com>\n// SPDX-License-Identifier: MIT\n\n/* Hedley - https://nemequ.github.io/hedley\n * Created by Evan Nemerson <evan@nemerson.com>\n */\n\n#if !defined(JSON_HEDLEY_VERSION) || (JSON_HEDLEY_VERSION < 15)\n#if defined(JSON_HEDLEY_VERSION)\n    #undef JSON_HEDLEY_VERSION\n#endif\n#define JSON_HEDLEY_VERSION 15\n\n#if defined(JSON_HEDLEY_STRINGIFY_EX)\n    #undef JSON_HEDLEY_STRINGIFY_EX\n#endif\n#define JSON_HEDLEY_STRINGIFY_EX(x) #x\n\n#if defined(JSON_HEDLEY_STRINGIFY)\n    #undef JSON_HEDLEY_STRINGIFY\n#endif\n#define JSON_HEDLEY_STRINGIFY(x) JSON_HEDLEY_STRINGIFY_EX(x)\n\n#if defined(JSON_HEDLEY_CONCAT_EX)\n    #undef JSON_HEDLEY_CONCAT_EX\n#endif\n#define JSON_HEDLEY_CONCAT_EX(a,b) a##b\n\n#if defined(JSON_HEDLEY_CONCAT)\n    #undef JSON_HEDLEY_CONCAT\n#endif\n#define JSON_HEDLEY_CONCAT(a,b) JSON_HEDLEY_CONCAT_EX(a,b)\n\n#if defined(JSON_HEDLEY_CONCAT3_EX)\n    #undef JSON_HEDLEY_CONCAT3_EX\n#endif\n#define JSON_HEDLEY_CONCAT3_EX(a,b,c) a##b##c\n\n#if defined(JSON_HEDLEY_CONCAT3)\n    #undef JSON_HEDLEY_CONCAT3\n#endif\n#define JSON_HEDLEY_CONCAT3(a,b,c) JSON_HEDLEY_CONCAT3_EX(a,b,c)\n\n#if defined(JSON_HEDLEY_VERSION_ENCODE)\n    #undef JSON_HEDLEY_VERSION_ENCODE\n#endif\n#define JSON_HEDLEY_VERSION_ENCODE(major,minor,revision) (((major) * 1000000) + ((minor) * 1000) + (revision))\n\n#if defined(JSON_HEDLEY_VERSION_DECODE_MAJOR)\n    #undef JSON_HEDLEY_VERSION_DECODE_MAJOR\n#endif\n#define JSON_HEDLEY_VERSION_DECODE_MAJOR(version) ((version) / 1000000)\n\n#if defined(JSON_HEDLEY_VERSION_DECODE_MINOR)\n    #undef JSON_HEDLEY_VERSION_DECODE_MINOR\n#endif\n#define JSON_HEDLEY_VERSION_DECODE_MINOR(version) (((version) % 1000000) / 1000)\n\n#if defined(JSON_HEDLEY_VERSION_DECODE_REVISION)\n    #undef JSON_HEDLEY_VERSION_DECODE_REVISION\n#endif\n#define JSON_HEDLEY_VERSION_DECODE_REVISION(version) ((version) % 1000)\n\n#if defined(JSON_HEDLEY_GNUC_VERSION)\n    #undef JSON_HEDLEY_GNUC_VERSION\n#endif\n#if defined(__GNUC__) && defined(__GNUC_PATCHLEVEL__)\n    #define JSON_HEDLEY_GNUC_VERSION JSON_HEDLEY_VERSION_ENCODE(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__)\n#elif defined(__GNUC__)\n    #define JSON_HEDLEY_GNUC_VERSION JSON_HEDLEY_VERSION_ENCODE(__GNUC__, __GNUC_MINOR__, 0)\n#endif\n\n#if defined(JSON_HEDLEY_GNUC_VERSION_CHECK)\n    #undef JSON_HEDLEY_GNUC_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_GNUC_VERSION)\n    #define JSON_HEDLEY_GNUC_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_GNUC_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_GNUC_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_MSVC_VERSION)\n    #undef JSON_HEDLEY_MSVC_VERSION\n#endif\n#if defined(_MSC_FULL_VER) && (_MSC_FULL_VER >= 140000000) && !defined(__ICL)\n    #define JSON_HEDLEY_MSVC_VERSION JSON_HEDLEY_VERSION_ENCODE(_MSC_FULL_VER / 10000000, (_MSC_FULL_VER % 10000000) / 100000, (_MSC_FULL_VER % 100000) / 100)\n#elif defined(_MSC_FULL_VER) && !defined(__ICL)\n    #define JSON_HEDLEY_MSVC_VERSION JSON_HEDLEY_VERSION_ENCODE(_MSC_FULL_VER / 1000000, (_MSC_FULL_VER % 1000000) / 10000, (_MSC_FULL_VER % 10000) / 10)\n#elif defined(_MSC_VER) && !defined(__ICL)\n    #define JSON_HEDLEY_MSVC_VERSION JSON_HEDLEY_VERSION_ENCODE(_MSC_VER / 100, _MSC_VER % 100, 0)\n#endif\n\n#if defined(JSON_HEDLEY_MSVC_VERSION_CHECK)\n    #undef JSON_HEDLEY_MSVC_VERSION_CHECK\n#endif\n#if !defined(JSON_HEDLEY_MSVC_VERSION)\n    #define JSON_HEDLEY_MSVC_VERSION_CHECK(major,minor,patch) (0)\n#elif defined(_MSC_VER) && (_MSC_VER >= 1400)\n    #define JSON_HEDLEY_MSVC_VERSION_CHECK(major,minor,patch) (_MSC_FULL_VER >= ((major * 10000000) + (minor * 100000) + (patch)))\n#elif defined(_MSC_VER) && (_MSC_VER >= 1200)\n    #define JSON_HEDLEY_MSVC_VERSION_CHECK(major,minor,patch) (_MSC_FULL_VER >= ((major * 1000000) + (minor * 10000) + (patch)))\n#else\n    #define JSON_HEDLEY_MSVC_VERSION_CHECK(major,minor,patch) (_MSC_VER >= ((major * 100) + (minor)))\n#endif\n\n#if defined(JSON_HEDLEY_INTEL_VERSION)\n    #undef JSON_HEDLEY_INTEL_VERSION\n#endif\n#if defined(__INTEL_COMPILER) && defined(__INTEL_COMPILER_UPDATE) && !defined(__ICL)\n    #define JSON_HEDLEY_INTEL_VERSION JSON_HEDLEY_VERSION_ENCODE(__INTEL_COMPILER / 100, __INTEL_COMPILER % 100, __INTEL_COMPILER_UPDATE)\n#elif defined(__INTEL_COMPILER) && !defined(__ICL)\n    #define JSON_HEDLEY_INTEL_VERSION JSON_HEDLEY_VERSION_ENCODE(__INTEL_COMPILER / 100, __INTEL_COMPILER % 100, 0)\n#endif\n\n#if defined(JSON_HEDLEY_INTEL_VERSION_CHECK)\n    #undef JSON_HEDLEY_INTEL_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_INTEL_VERSION)\n    #define JSON_HEDLEY_INTEL_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_INTEL_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_INTEL_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_INTEL_CL_VERSION)\n    #undef JSON_HEDLEY_INTEL_CL_VERSION\n#endif\n#if defined(__INTEL_COMPILER) && defined(__INTEL_COMPILER_UPDATE) && defined(__ICL)\n    #define JSON_HEDLEY_INTEL_CL_VERSION JSON_HEDLEY_VERSION_ENCODE(__INTEL_COMPILER, __INTEL_COMPILER_UPDATE, 0)\n#endif\n\n#if defined(JSON_HEDLEY_INTEL_CL_VERSION_CHECK)\n    #undef JSON_HEDLEY_INTEL_CL_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_INTEL_CL_VERSION)\n    #define JSON_HEDLEY_INTEL_CL_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_INTEL_CL_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_INTEL_CL_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_PGI_VERSION)\n    #undef JSON_HEDLEY_PGI_VERSION\n#endif\n#if defined(__PGI) && defined(__PGIC__) && defined(__PGIC_MINOR__) && defined(__PGIC_PATCHLEVEL__)\n    #define JSON_HEDLEY_PGI_VERSION JSON_HEDLEY_VERSION_ENCODE(__PGIC__, __PGIC_MINOR__, __PGIC_PATCHLEVEL__)\n#endif\n\n#if defined(JSON_HEDLEY_PGI_VERSION_CHECK)\n    #undef JSON_HEDLEY_PGI_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_PGI_VERSION)\n    #define JSON_HEDLEY_PGI_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_PGI_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_PGI_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_SUNPRO_VERSION)\n    #undef JSON_HEDLEY_SUNPRO_VERSION\n#endif\n#if defined(__SUNPRO_C) && (__SUNPRO_C > 0x1000)\n    #define JSON_HEDLEY_SUNPRO_VERSION JSON_HEDLEY_VERSION_ENCODE((((__SUNPRO_C >> 16) & 0xf) * 10) + ((__SUNPRO_C >> 12) & 0xf), (((__SUNPRO_C >> 8) & 0xf) * 10) + ((__SUNPRO_C >> 4) & 0xf), (__SUNPRO_C & 0xf) * 10)\n#elif defined(__SUNPRO_C)\n    #define JSON_HEDLEY_SUNPRO_VERSION JSON_HEDLEY_VERSION_ENCODE((__SUNPRO_C >> 8) & 0xf, (__SUNPRO_C >> 4) & 0xf, (__SUNPRO_C) & 0xf)\n#elif defined(__SUNPRO_CC) && (__SUNPRO_CC > 0x1000)\n    #define JSON_HEDLEY_SUNPRO_VERSION JSON_HEDLEY_VERSION_ENCODE((((__SUNPRO_CC >> 16) & 0xf) * 10) + ((__SUNPRO_CC >> 12) & 0xf), (((__SUNPRO_CC >> 8) & 0xf) * 10) + ((__SUNPRO_CC >> 4) & 0xf), (__SUNPRO_CC & 0xf) * 10)\n#elif defined(__SUNPRO_CC)\n    #define JSON_HEDLEY_SUNPRO_VERSION JSON_HEDLEY_VERSION_ENCODE((__SUNPRO_CC >> 8) & 0xf, (__SUNPRO_CC >> 4) & 0xf, (__SUNPRO_CC) & 0xf)\n#endif\n\n#if defined(JSON_HEDLEY_SUNPRO_VERSION_CHECK)\n    #undef JSON_HEDLEY_SUNPRO_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_SUNPRO_VERSION)\n    #define JSON_HEDLEY_SUNPRO_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_SUNPRO_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_SUNPRO_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_EMSCRIPTEN_VERSION)\n    #undef JSON_HEDLEY_EMSCRIPTEN_VERSION\n#endif\n#if defined(__EMSCRIPTEN__)\n    #define JSON_HEDLEY_EMSCRIPTEN_VERSION JSON_HEDLEY_VERSION_ENCODE(__EMSCRIPTEN_major__, __EMSCRIPTEN_minor__, __EMSCRIPTEN_tiny__)\n#endif\n\n#if defined(JSON_HEDLEY_EMSCRIPTEN_VERSION_CHECK)\n    #undef JSON_HEDLEY_EMSCRIPTEN_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_EMSCRIPTEN_VERSION)\n    #define JSON_HEDLEY_EMSCRIPTEN_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_EMSCRIPTEN_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_EMSCRIPTEN_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_ARM_VERSION)\n    #undef JSON_HEDLEY_ARM_VERSION\n#endif\n#if defined(__CC_ARM) && defined(__ARMCOMPILER_VERSION)\n    #define JSON_HEDLEY_ARM_VERSION JSON_HEDLEY_VERSION_ENCODE(__ARMCOMPILER_VERSION / 1000000, (__ARMCOMPILER_VERSION % 1000000) / 10000, (__ARMCOMPILER_VERSION % 10000) / 100)\n#elif defined(__CC_ARM) && defined(__ARMCC_VERSION)\n    #define JSON_HEDLEY_ARM_VERSION JSON_HEDLEY_VERSION_ENCODE(__ARMCC_VERSION / 1000000, (__ARMCC_VERSION % 1000000) / 10000, (__ARMCC_VERSION % 10000) / 100)\n#endif\n\n#if defined(JSON_HEDLEY_ARM_VERSION_CHECK)\n    #undef JSON_HEDLEY_ARM_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_ARM_VERSION)\n    #define JSON_HEDLEY_ARM_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_ARM_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_ARM_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_IBM_VERSION)\n    #undef JSON_HEDLEY_IBM_VERSION\n#endif\n#if defined(__ibmxl__)\n    #define JSON_HEDLEY_IBM_VERSION JSON_HEDLEY_VERSION_ENCODE(__ibmxl_version__, __ibmxl_release__, __ibmxl_modification__)\n#elif defined(__xlC__) && defined(__xlC_ver__)\n    #define JSON_HEDLEY_IBM_VERSION JSON_HEDLEY_VERSION_ENCODE(__xlC__ >> 8, __xlC__ & 0xff, (__xlC_ver__ >> 8) & 0xff)\n#elif defined(__xlC__)\n    #define JSON_HEDLEY_IBM_VERSION JSON_HEDLEY_VERSION_ENCODE(__xlC__ >> 8, __xlC__ & 0xff, 0)\n#endif\n\n#if defined(JSON_HEDLEY_IBM_VERSION_CHECK)\n    #undef JSON_HEDLEY_IBM_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_IBM_VERSION)\n    #define JSON_HEDLEY_IBM_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_IBM_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_IBM_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_TI_VERSION)\n    #undef JSON_HEDLEY_TI_VERSION\n#endif\n#if \\\n    defined(__TI_COMPILER_VERSION__) && \\\n    ( \\\n      defined(__TMS470__) || defined(__TI_ARM__) || \\\n      defined(__MSP430__) || \\\n      defined(__TMS320C2000__) \\\n    )\n#if (__TI_COMPILER_VERSION__ >= 16000000)\n    #define JSON_HEDLEY_TI_VERSION JSON_HEDLEY_VERSION_ENCODE(__TI_COMPILER_VERSION__ / 1000000, (__TI_COMPILER_VERSION__ % 1000000) / 1000, (__TI_COMPILER_VERSION__ % 1000))\n#endif\n#endif\n\n#if defined(JSON_HEDLEY_TI_VERSION_CHECK)\n    #undef JSON_HEDLEY_TI_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_TI_VERSION)\n    #define JSON_HEDLEY_TI_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_TI_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_TI_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_TI_CL2000_VERSION)\n    #undef JSON_HEDLEY_TI_CL2000_VERSION\n#endif\n#if defined(__TI_COMPILER_VERSION__) && defined(__TMS320C2000__)\n    #define JSON_HEDLEY_TI_CL2000_VERSION JSON_HEDLEY_VERSION_ENCODE(__TI_COMPILER_VERSION__ / 1000000, (__TI_COMPILER_VERSION__ % 1000000) / 1000, (__TI_COMPILER_VERSION__ % 1000))\n#endif\n\n#if defined(JSON_HEDLEY_TI_CL2000_VERSION_CHECK)\n    #undef JSON_HEDLEY_TI_CL2000_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_TI_CL2000_VERSION)\n    #define JSON_HEDLEY_TI_CL2000_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_TI_CL2000_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_TI_CL2000_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_TI_CL430_VERSION)\n    #undef JSON_HEDLEY_TI_CL430_VERSION\n#endif\n#if defined(__TI_COMPILER_VERSION__) && defined(__MSP430__)\n    #define JSON_HEDLEY_TI_CL430_VERSION JSON_HEDLEY_VERSION_ENCODE(__TI_COMPILER_VERSION__ / 1000000, (__TI_COMPILER_VERSION__ % 1000000) / 1000, (__TI_COMPILER_VERSION__ % 1000))\n#endif\n\n#if defined(JSON_HEDLEY_TI_CL430_VERSION_CHECK)\n    #undef JSON_HEDLEY_TI_CL430_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_TI_CL430_VERSION)\n    #define JSON_HEDLEY_TI_CL430_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_TI_CL430_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_TI_CL430_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_TI_ARMCL_VERSION)\n    #undef JSON_HEDLEY_TI_ARMCL_VERSION\n#endif\n#if defined(__TI_COMPILER_VERSION__) && (defined(__TMS470__) || defined(__TI_ARM__))\n    #define JSON_HEDLEY_TI_ARMCL_VERSION JSON_HEDLEY_VERSION_ENCODE(__TI_COMPILER_VERSION__ / 1000000, (__TI_COMPILER_VERSION__ % 1000000) / 1000, (__TI_COMPILER_VERSION__ % 1000))\n#endif\n\n#if defined(JSON_HEDLEY_TI_ARMCL_VERSION_CHECK)\n    #undef JSON_HEDLEY_TI_ARMCL_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_TI_ARMCL_VERSION)\n    #define JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_TI_ARMCL_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_TI_CL6X_VERSION)\n    #undef JSON_HEDLEY_TI_CL6X_VERSION\n#endif\n#if defined(__TI_COMPILER_VERSION__) && defined(__TMS320C6X__)\n    #define JSON_HEDLEY_TI_CL6X_VERSION JSON_HEDLEY_VERSION_ENCODE(__TI_COMPILER_VERSION__ / 1000000, (__TI_COMPILER_VERSION__ % 1000000) / 1000, (__TI_COMPILER_VERSION__ % 1000))\n#endif\n\n#if defined(JSON_HEDLEY_TI_CL6X_VERSION_CHECK)\n    #undef JSON_HEDLEY_TI_CL6X_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_TI_CL6X_VERSION)\n    #define JSON_HEDLEY_TI_CL6X_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_TI_CL6X_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_TI_CL6X_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_TI_CL7X_VERSION)\n    #undef JSON_HEDLEY_TI_CL7X_VERSION\n#endif\n#if defined(__TI_COMPILER_VERSION__) && defined(__C7000__)\n    #define JSON_HEDLEY_TI_CL7X_VERSION JSON_HEDLEY_VERSION_ENCODE(__TI_COMPILER_VERSION__ / 1000000, (__TI_COMPILER_VERSION__ % 1000000) / 1000, (__TI_COMPILER_VERSION__ % 1000))\n#endif\n\n#if defined(JSON_HEDLEY_TI_CL7X_VERSION_CHECK)\n    #undef JSON_HEDLEY_TI_CL7X_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_TI_CL7X_VERSION)\n    #define JSON_HEDLEY_TI_CL7X_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_TI_CL7X_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_TI_CL7X_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_TI_CLPRU_VERSION)\n    #undef JSON_HEDLEY_TI_CLPRU_VERSION\n#endif\n#if defined(__TI_COMPILER_VERSION__) && defined(__PRU__)\n    #define JSON_HEDLEY_TI_CLPRU_VERSION JSON_HEDLEY_VERSION_ENCODE(__TI_COMPILER_VERSION__ / 1000000, (__TI_COMPILER_VERSION__ % 1000000) / 1000, (__TI_COMPILER_VERSION__ % 1000))\n#endif\n\n#if defined(JSON_HEDLEY_TI_CLPRU_VERSION_CHECK)\n    #undef JSON_HEDLEY_TI_CLPRU_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_TI_CLPRU_VERSION)\n    #define JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_TI_CLPRU_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_CRAY_VERSION)\n    #undef JSON_HEDLEY_CRAY_VERSION\n#endif\n#if defined(_CRAYC)\n    #if defined(_RELEASE_PATCHLEVEL)\n        #define JSON_HEDLEY_CRAY_VERSION JSON_HEDLEY_VERSION_ENCODE(_RELEASE_MAJOR, _RELEASE_MINOR, _RELEASE_PATCHLEVEL)\n    #else\n        #define JSON_HEDLEY_CRAY_VERSION JSON_HEDLEY_VERSION_ENCODE(_RELEASE_MAJOR, _RELEASE_MINOR, 0)\n    #endif\n#endif\n\n#if defined(JSON_HEDLEY_CRAY_VERSION_CHECK)\n    #undef JSON_HEDLEY_CRAY_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_CRAY_VERSION)\n    #define JSON_HEDLEY_CRAY_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_CRAY_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_CRAY_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_IAR_VERSION)\n    #undef JSON_HEDLEY_IAR_VERSION\n#endif\n#if defined(__IAR_SYSTEMS_ICC__)\n    #if __VER__ > 1000\n        #define JSON_HEDLEY_IAR_VERSION JSON_HEDLEY_VERSION_ENCODE((__VER__ / 1000000), ((__VER__ / 1000) % 1000), (__VER__ % 1000))\n    #else\n        #define JSON_HEDLEY_IAR_VERSION JSON_HEDLEY_VERSION_ENCODE(__VER__ / 100, __VER__ % 100, 0)\n    #endif\n#endif\n\n#if defined(JSON_HEDLEY_IAR_VERSION_CHECK)\n    #undef JSON_HEDLEY_IAR_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_IAR_VERSION)\n    #define JSON_HEDLEY_IAR_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_IAR_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_IAR_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_TINYC_VERSION)\n    #undef JSON_HEDLEY_TINYC_VERSION\n#endif\n#if defined(__TINYC__)\n    #define JSON_HEDLEY_TINYC_VERSION JSON_HEDLEY_VERSION_ENCODE(__TINYC__ / 1000, (__TINYC__ / 100) % 10, __TINYC__ % 100)\n#endif\n\n#if defined(JSON_HEDLEY_TINYC_VERSION_CHECK)\n    #undef JSON_HEDLEY_TINYC_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_TINYC_VERSION)\n    #define JSON_HEDLEY_TINYC_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_TINYC_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_TINYC_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_DMC_VERSION)\n    #undef JSON_HEDLEY_DMC_VERSION\n#endif\n#if defined(__DMC__)\n    #define JSON_HEDLEY_DMC_VERSION JSON_HEDLEY_VERSION_ENCODE(__DMC__ >> 8, (__DMC__ >> 4) & 0xf, __DMC__ & 0xf)\n#endif\n\n#if defined(JSON_HEDLEY_DMC_VERSION_CHECK)\n    #undef JSON_HEDLEY_DMC_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_DMC_VERSION)\n    #define JSON_HEDLEY_DMC_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_DMC_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_DMC_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_COMPCERT_VERSION)\n    #undef JSON_HEDLEY_COMPCERT_VERSION\n#endif\n#if defined(__COMPCERT_VERSION__)\n    #define JSON_HEDLEY_COMPCERT_VERSION JSON_HEDLEY_VERSION_ENCODE(__COMPCERT_VERSION__ / 10000, (__COMPCERT_VERSION__ / 100) % 100, __COMPCERT_VERSION__ % 100)\n#endif\n\n#if defined(JSON_HEDLEY_COMPCERT_VERSION_CHECK)\n    #undef JSON_HEDLEY_COMPCERT_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_COMPCERT_VERSION)\n    #define JSON_HEDLEY_COMPCERT_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_COMPCERT_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_COMPCERT_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_PELLES_VERSION)\n    #undef JSON_HEDLEY_PELLES_VERSION\n#endif\n#if defined(__POCC__)\n    #define JSON_HEDLEY_PELLES_VERSION JSON_HEDLEY_VERSION_ENCODE(__POCC__ / 100, __POCC__ % 100, 0)\n#endif\n\n#if defined(JSON_HEDLEY_PELLES_VERSION_CHECK)\n    #undef JSON_HEDLEY_PELLES_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_PELLES_VERSION)\n    #define JSON_HEDLEY_PELLES_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_PELLES_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_PELLES_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_MCST_LCC_VERSION)\n    #undef JSON_HEDLEY_MCST_LCC_VERSION\n#endif\n#if defined(__LCC__) && defined(__LCC_MINOR__)\n    #define JSON_HEDLEY_MCST_LCC_VERSION JSON_HEDLEY_VERSION_ENCODE(__LCC__ / 100, __LCC__ % 100, __LCC_MINOR__)\n#endif\n\n#if defined(JSON_HEDLEY_MCST_LCC_VERSION_CHECK)\n    #undef JSON_HEDLEY_MCST_LCC_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_MCST_LCC_VERSION)\n    #define JSON_HEDLEY_MCST_LCC_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_MCST_LCC_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_MCST_LCC_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_GCC_VERSION)\n    #undef JSON_HEDLEY_GCC_VERSION\n#endif\n#if \\\n    defined(JSON_HEDLEY_GNUC_VERSION) && \\\n    !defined(__clang__) && \\\n    !defined(JSON_HEDLEY_INTEL_VERSION) && \\\n    !defined(JSON_HEDLEY_PGI_VERSION) && \\\n    !defined(JSON_HEDLEY_ARM_VERSION) && \\\n    !defined(JSON_HEDLEY_CRAY_VERSION) && \\\n    !defined(JSON_HEDLEY_TI_VERSION) && \\\n    !defined(JSON_HEDLEY_TI_ARMCL_VERSION) && \\\n    !defined(JSON_HEDLEY_TI_CL430_VERSION) && \\\n    !defined(JSON_HEDLEY_TI_CL2000_VERSION) && \\\n    !defined(JSON_HEDLEY_TI_CL6X_VERSION) && \\\n    !defined(JSON_HEDLEY_TI_CL7X_VERSION) && \\\n    !defined(JSON_HEDLEY_TI_CLPRU_VERSION) && \\\n    !defined(__COMPCERT__) && \\\n    !defined(JSON_HEDLEY_MCST_LCC_VERSION)\n    #define JSON_HEDLEY_GCC_VERSION JSON_HEDLEY_GNUC_VERSION\n#endif\n\n#if defined(JSON_HEDLEY_GCC_VERSION_CHECK)\n    #undef JSON_HEDLEY_GCC_VERSION_CHECK\n#endif\n#if defined(JSON_HEDLEY_GCC_VERSION)\n    #define JSON_HEDLEY_GCC_VERSION_CHECK(major,minor,patch) (JSON_HEDLEY_GCC_VERSION >= JSON_HEDLEY_VERSION_ENCODE(major, minor, patch))\n#else\n    #define JSON_HEDLEY_GCC_VERSION_CHECK(major,minor,patch) (0)\n#endif\n\n#if defined(JSON_HEDLEY_HAS_ATTRIBUTE)\n    #undef JSON_HEDLEY_HAS_ATTRIBUTE\n#endif\n#if \\\n  defined(__has_attribute) && \\\n  ( \\\n    (!defined(JSON_HEDLEY_IAR_VERSION) || JSON_HEDLEY_IAR_VERSION_CHECK(8,5,9)) \\\n  )\n#  define JSON_HEDLEY_HAS_ATTRIBUTE(attribute) __has_attribute(attribute)\n#else\n#  define JSON_HEDLEY_HAS_ATTRIBUTE(attribute) (0)\n#endif\n\n#if defined(JSON_HEDLEY_GNUC_HAS_ATTRIBUTE)\n    #undef JSON_HEDLEY_GNUC_HAS_ATTRIBUTE\n#endif\n#if defined(__has_attribute)\n    #define JSON_HEDLEY_GNUC_HAS_ATTRIBUTE(attribute,major,minor,patch) JSON_HEDLEY_HAS_ATTRIBUTE(attribute)\n#else\n    #define JSON_HEDLEY_GNUC_HAS_ATTRIBUTE(attribute,major,minor,patch) JSON_HEDLEY_GNUC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_GCC_HAS_ATTRIBUTE)\n    #undef JSON_HEDLEY_GCC_HAS_ATTRIBUTE\n#endif\n#if defined(__has_attribute)\n    #define JSON_HEDLEY_GCC_HAS_ATTRIBUTE(attribute,major,minor,patch) JSON_HEDLEY_HAS_ATTRIBUTE(attribute)\n#else\n    #define JSON_HEDLEY_GCC_HAS_ATTRIBUTE(attribute,major,minor,patch) JSON_HEDLEY_GCC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_HAS_CPP_ATTRIBUTE)\n    #undef JSON_HEDLEY_HAS_CPP_ATTRIBUTE\n#endif\n#if \\\n    defined(__has_cpp_attribute) && \\\n    defined(__cplusplus) && \\\n    (!defined(JSON_HEDLEY_SUNPRO_VERSION) || JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,15,0))\n    #define JSON_HEDLEY_HAS_CPP_ATTRIBUTE(attribute) __has_cpp_attribute(attribute)\n#else\n    #define JSON_HEDLEY_HAS_CPP_ATTRIBUTE(attribute) (0)\n#endif\n\n#if defined(JSON_HEDLEY_HAS_CPP_ATTRIBUTE_NS)\n    #undef JSON_HEDLEY_HAS_CPP_ATTRIBUTE_NS\n#endif\n#if !defined(__cplusplus) || !defined(__has_cpp_attribute)\n    #define JSON_HEDLEY_HAS_CPP_ATTRIBUTE_NS(ns,attribute) (0)\n#elif \\\n    !defined(JSON_HEDLEY_PGI_VERSION) && \\\n    !defined(JSON_HEDLEY_IAR_VERSION) && \\\n    (!defined(JSON_HEDLEY_SUNPRO_VERSION) || JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,15,0)) && \\\n    (!defined(JSON_HEDLEY_MSVC_VERSION) || JSON_HEDLEY_MSVC_VERSION_CHECK(19,20,0))\n    #define JSON_HEDLEY_HAS_CPP_ATTRIBUTE_NS(ns,attribute) JSON_HEDLEY_HAS_CPP_ATTRIBUTE(ns::attribute)\n#else\n    #define JSON_HEDLEY_HAS_CPP_ATTRIBUTE_NS(ns,attribute) (0)\n#endif\n\n#if defined(JSON_HEDLEY_GNUC_HAS_CPP_ATTRIBUTE)\n    #undef JSON_HEDLEY_GNUC_HAS_CPP_ATTRIBUTE\n#endif\n#if defined(__has_cpp_attribute) && defined(__cplusplus)\n    #define JSON_HEDLEY_GNUC_HAS_CPP_ATTRIBUTE(attribute,major,minor,patch) __has_cpp_attribute(attribute)\n#else\n    #define JSON_HEDLEY_GNUC_HAS_CPP_ATTRIBUTE(attribute,major,minor,patch) JSON_HEDLEY_GNUC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_GCC_HAS_CPP_ATTRIBUTE)\n    #undef JSON_HEDLEY_GCC_HAS_CPP_ATTRIBUTE\n#endif\n#if defined(__has_cpp_attribute) && defined(__cplusplus)\n    #define JSON_HEDLEY_GCC_HAS_CPP_ATTRIBUTE(attribute,major,minor,patch) __has_cpp_attribute(attribute)\n#else\n    #define JSON_HEDLEY_GCC_HAS_CPP_ATTRIBUTE(attribute,major,minor,patch) JSON_HEDLEY_GCC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_HAS_BUILTIN)\n    #undef JSON_HEDLEY_HAS_BUILTIN\n#endif\n#if defined(__has_builtin)\n    #define JSON_HEDLEY_HAS_BUILTIN(builtin) __has_builtin(builtin)\n#else\n    #define JSON_HEDLEY_HAS_BUILTIN(builtin) (0)\n#endif\n\n#if defined(JSON_HEDLEY_GNUC_HAS_BUILTIN)\n    #undef JSON_HEDLEY_GNUC_HAS_BUILTIN\n#endif\n#if defined(__has_builtin)\n    #define JSON_HEDLEY_GNUC_HAS_BUILTIN(builtin,major,minor,patch) __has_builtin(builtin)\n#else\n    #define JSON_HEDLEY_GNUC_HAS_BUILTIN(builtin,major,minor,patch) JSON_HEDLEY_GNUC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_GCC_HAS_BUILTIN)\n    #undef JSON_HEDLEY_GCC_HAS_BUILTIN\n#endif\n#if defined(__has_builtin)\n    #define JSON_HEDLEY_GCC_HAS_BUILTIN(builtin,major,minor,patch) __has_builtin(builtin)\n#else\n    #define JSON_HEDLEY_GCC_HAS_BUILTIN(builtin,major,minor,patch) JSON_HEDLEY_GCC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_HAS_FEATURE)\n    #undef JSON_HEDLEY_HAS_FEATURE\n#endif\n#if defined(__has_feature)\n    #define JSON_HEDLEY_HAS_FEATURE(feature) __has_feature(feature)\n#else\n    #define JSON_HEDLEY_HAS_FEATURE(feature) (0)\n#endif\n\n#if defined(JSON_HEDLEY_GNUC_HAS_FEATURE)\n    #undef JSON_HEDLEY_GNUC_HAS_FEATURE\n#endif\n#if defined(__has_feature)\n    #define JSON_HEDLEY_GNUC_HAS_FEATURE(feature,major,minor,patch) __has_feature(feature)\n#else\n    #define JSON_HEDLEY_GNUC_HAS_FEATURE(feature,major,minor,patch) JSON_HEDLEY_GNUC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_GCC_HAS_FEATURE)\n    #undef JSON_HEDLEY_GCC_HAS_FEATURE\n#endif\n#if defined(__has_feature)\n    #define JSON_HEDLEY_GCC_HAS_FEATURE(feature,major,minor,patch) __has_feature(feature)\n#else\n    #define JSON_HEDLEY_GCC_HAS_FEATURE(feature,major,minor,patch) JSON_HEDLEY_GCC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_HAS_EXTENSION)\n    #undef JSON_HEDLEY_HAS_EXTENSION\n#endif\n#if defined(__has_extension)\n    #define JSON_HEDLEY_HAS_EXTENSION(extension) __has_extension(extension)\n#else\n    #define JSON_HEDLEY_HAS_EXTENSION(extension) (0)\n#endif\n\n#if defined(JSON_HEDLEY_GNUC_HAS_EXTENSION)\n    #undef JSON_HEDLEY_GNUC_HAS_EXTENSION\n#endif\n#if defined(__has_extension)\n    #define JSON_HEDLEY_GNUC_HAS_EXTENSION(extension,major,minor,patch) __has_extension(extension)\n#else\n    #define JSON_HEDLEY_GNUC_HAS_EXTENSION(extension,major,minor,patch) JSON_HEDLEY_GNUC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_GCC_HAS_EXTENSION)\n    #undef JSON_HEDLEY_GCC_HAS_EXTENSION\n#endif\n#if defined(__has_extension)\n    #define JSON_HEDLEY_GCC_HAS_EXTENSION(extension,major,minor,patch) __has_extension(extension)\n#else\n    #define JSON_HEDLEY_GCC_HAS_EXTENSION(extension,major,minor,patch) JSON_HEDLEY_GCC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_HAS_DECLSPEC_ATTRIBUTE)\n    #undef JSON_HEDLEY_HAS_DECLSPEC_ATTRIBUTE\n#endif\n#if defined(__has_declspec_attribute)\n    #define JSON_HEDLEY_HAS_DECLSPEC_ATTRIBUTE(attribute) __has_declspec_attribute(attribute)\n#else\n    #define JSON_HEDLEY_HAS_DECLSPEC_ATTRIBUTE(attribute) (0)\n#endif\n\n#if defined(JSON_HEDLEY_GNUC_HAS_DECLSPEC_ATTRIBUTE)\n    #undef JSON_HEDLEY_GNUC_HAS_DECLSPEC_ATTRIBUTE\n#endif\n#if defined(__has_declspec_attribute)\n    #define JSON_HEDLEY_GNUC_HAS_DECLSPEC_ATTRIBUTE(attribute,major,minor,patch) __has_declspec_attribute(attribute)\n#else\n    #define JSON_HEDLEY_GNUC_HAS_DECLSPEC_ATTRIBUTE(attribute,major,minor,patch) JSON_HEDLEY_GNUC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_GCC_HAS_DECLSPEC_ATTRIBUTE)\n    #undef JSON_HEDLEY_GCC_HAS_DECLSPEC_ATTRIBUTE\n#endif\n#if defined(__has_declspec_attribute)\n    #define JSON_HEDLEY_GCC_HAS_DECLSPEC_ATTRIBUTE(attribute,major,minor,patch) __has_declspec_attribute(attribute)\n#else\n    #define JSON_HEDLEY_GCC_HAS_DECLSPEC_ATTRIBUTE(attribute,major,minor,patch) JSON_HEDLEY_GCC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_HAS_WARNING)\n    #undef JSON_HEDLEY_HAS_WARNING\n#endif\n#if defined(__has_warning)\n    #define JSON_HEDLEY_HAS_WARNING(warning) __has_warning(warning)\n#else\n    #define JSON_HEDLEY_HAS_WARNING(warning) (0)\n#endif\n\n#if defined(JSON_HEDLEY_GNUC_HAS_WARNING)\n    #undef JSON_HEDLEY_GNUC_HAS_WARNING\n#endif\n#if defined(__has_warning)\n    #define JSON_HEDLEY_GNUC_HAS_WARNING(warning,major,minor,patch) __has_warning(warning)\n#else\n    #define JSON_HEDLEY_GNUC_HAS_WARNING(warning,major,minor,patch) JSON_HEDLEY_GNUC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_GCC_HAS_WARNING)\n    #undef JSON_HEDLEY_GCC_HAS_WARNING\n#endif\n#if defined(__has_warning)\n    #define JSON_HEDLEY_GCC_HAS_WARNING(warning,major,minor,patch) __has_warning(warning)\n#else\n    #define JSON_HEDLEY_GCC_HAS_WARNING(warning,major,minor,patch) JSON_HEDLEY_GCC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if \\\n    (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)) || \\\n    defined(__clang__) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(3,0,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_IAR_VERSION_CHECK(8,0,0) || \\\n    JSON_HEDLEY_PGI_VERSION_CHECK(18,4,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n    JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,7,0) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(2,0,1) || \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,1,0) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,0,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n    JSON_HEDLEY_CRAY_VERSION_CHECK(5,0,0) || \\\n    JSON_HEDLEY_TINYC_VERSION_CHECK(0,9,17) || \\\n    JSON_HEDLEY_SUNPRO_VERSION_CHECK(8,0,0) || \\\n    (JSON_HEDLEY_IBM_VERSION_CHECK(10,1,0) && defined(__C99_PRAGMA_OPERATOR))\n    #define JSON_HEDLEY_PRAGMA(value) _Pragma(#value)\n#elif JSON_HEDLEY_MSVC_VERSION_CHECK(15,0,0)\n    #define JSON_HEDLEY_PRAGMA(value) __pragma(value)\n#else\n    #define JSON_HEDLEY_PRAGMA(value)\n#endif\n\n#if defined(JSON_HEDLEY_DIAGNOSTIC_PUSH)\n    #undef JSON_HEDLEY_DIAGNOSTIC_PUSH\n#endif\n#if defined(JSON_HEDLEY_DIAGNOSTIC_POP)\n    #undef JSON_HEDLEY_DIAGNOSTIC_POP\n#endif\n#if defined(__clang__)\n    #define JSON_HEDLEY_DIAGNOSTIC_PUSH _Pragma(\"clang diagnostic push\")\n    #define JSON_HEDLEY_DIAGNOSTIC_POP _Pragma(\"clang diagnostic pop\")\n#elif JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_PUSH _Pragma(\"warning(push)\")\n    #define JSON_HEDLEY_DIAGNOSTIC_POP _Pragma(\"warning(pop)\")\n#elif JSON_HEDLEY_GCC_VERSION_CHECK(4,6,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_PUSH _Pragma(\"GCC diagnostic push\")\n    #define JSON_HEDLEY_DIAGNOSTIC_POP _Pragma(\"GCC diagnostic pop\")\n#elif \\\n    JSON_HEDLEY_MSVC_VERSION_CHECK(15,0,0) || \\\n    JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_PUSH __pragma(warning(push))\n    #define JSON_HEDLEY_DIAGNOSTIC_POP __pragma(warning(pop))\n#elif JSON_HEDLEY_ARM_VERSION_CHECK(5,6,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_PUSH _Pragma(\"push\")\n    #define JSON_HEDLEY_DIAGNOSTIC_POP _Pragma(\"pop\")\n#elif \\\n    JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,4,0) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(8,1,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_PUSH _Pragma(\"diag_push\")\n    #define JSON_HEDLEY_DIAGNOSTIC_POP _Pragma(\"diag_pop\")\n#elif JSON_HEDLEY_PELLES_VERSION_CHECK(2,90,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_PUSH _Pragma(\"warning(push)\")\n    #define JSON_HEDLEY_DIAGNOSTIC_POP _Pragma(\"warning(pop)\")\n#else\n    #define JSON_HEDLEY_DIAGNOSTIC_PUSH\n    #define JSON_HEDLEY_DIAGNOSTIC_POP\n#endif\n\n/* JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_ is for\n   HEDLEY INTERNAL USE ONLY.  API subject to change without notice. */\n#if defined(JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_)\n    #undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_\n#endif\n#if defined(__cplusplus)\n#  if JSON_HEDLEY_HAS_WARNING(\"-Wc++98-compat\")\n#    if JSON_HEDLEY_HAS_WARNING(\"-Wc++17-extensions\")\n#      if JSON_HEDLEY_HAS_WARNING(\"-Wc++1z-extensions\")\n#        define JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_(xpr) \\\n    JSON_HEDLEY_DIAGNOSTIC_PUSH \\\n    _Pragma(\"clang diagnostic ignored \\\"-Wc++98-compat\\\"\") \\\n    _Pragma(\"clang diagnostic ignored \\\"-Wc++17-extensions\\\"\") \\\n    _Pragma(\"clang diagnostic ignored \\\"-Wc++1z-extensions\\\"\") \\\n    xpr \\\n    JSON_HEDLEY_DIAGNOSTIC_POP\n#      else\n#        define JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_(xpr) \\\n    JSON_HEDLEY_DIAGNOSTIC_PUSH \\\n    _Pragma(\"clang diagnostic ignored \\\"-Wc++98-compat\\\"\") \\\n    _Pragma(\"clang diagnostic ignored \\\"-Wc++17-extensions\\\"\") \\\n    xpr \\\n    JSON_HEDLEY_DIAGNOSTIC_POP\n#      endif\n#    else\n#      define JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_(xpr) \\\n    JSON_HEDLEY_DIAGNOSTIC_PUSH \\\n    _Pragma(\"clang diagnostic ignored \\\"-Wc++98-compat\\\"\") \\\n    xpr \\\n    JSON_HEDLEY_DIAGNOSTIC_POP\n#    endif\n#  endif\n#endif\n#if !defined(JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_(x) x\n#endif\n\n#if defined(JSON_HEDLEY_CONST_CAST)\n    #undef JSON_HEDLEY_CONST_CAST\n#endif\n#if defined(__cplusplus)\n#  define JSON_HEDLEY_CONST_CAST(T, expr) (const_cast<T>(expr))\n#elif \\\n  JSON_HEDLEY_HAS_WARNING(\"-Wcast-qual\") || \\\n  JSON_HEDLEY_GCC_VERSION_CHECK(4,6,0) || \\\n  JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0)\n#  define JSON_HEDLEY_CONST_CAST(T, expr) (__extension__ ({ \\\n        JSON_HEDLEY_DIAGNOSTIC_PUSH \\\n        JSON_HEDLEY_DIAGNOSTIC_DISABLE_CAST_QUAL \\\n        ((T) (expr)); \\\n        JSON_HEDLEY_DIAGNOSTIC_POP \\\n    }))\n#else\n#  define JSON_HEDLEY_CONST_CAST(T, expr) ((T) (expr))\n#endif\n\n#if defined(JSON_HEDLEY_REINTERPRET_CAST)\n    #undef JSON_HEDLEY_REINTERPRET_CAST\n#endif\n#if defined(__cplusplus)\n    #define JSON_HEDLEY_REINTERPRET_CAST(T, expr) (reinterpret_cast<T>(expr))\n#else\n    #define JSON_HEDLEY_REINTERPRET_CAST(T, expr) ((T) (expr))\n#endif\n\n#if defined(JSON_HEDLEY_STATIC_CAST)\n    #undef JSON_HEDLEY_STATIC_CAST\n#endif\n#if defined(__cplusplus)\n    #define JSON_HEDLEY_STATIC_CAST(T, expr) (static_cast<T>(expr))\n#else\n    #define JSON_HEDLEY_STATIC_CAST(T, expr) ((T) (expr))\n#endif\n\n#if defined(JSON_HEDLEY_CPP_CAST)\n    #undef JSON_HEDLEY_CPP_CAST\n#endif\n#if defined(__cplusplus)\n#  if JSON_HEDLEY_HAS_WARNING(\"-Wold-style-cast\")\n#    define JSON_HEDLEY_CPP_CAST(T, expr) \\\n    JSON_HEDLEY_DIAGNOSTIC_PUSH \\\n    _Pragma(\"clang diagnostic ignored \\\"-Wold-style-cast\\\"\") \\\n    ((T) (expr)) \\\n    JSON_HEDLEY_DIAGNOSTIC_POP\n#  elif JSON_HEDLEY_IAR_VERSION_CHECK(8,3,0)\n#    define JSON_HEDLEY_CPP_CAST(T, expr) \\\n    JSON_HEDLEY_DIAGNOSTIC_PUSH \\\n    _Pragma(\"diag_suppress=Pe137\") \\\n    JSON_HEDLEY_DIAGNOSTIC_POP\n#  else\n#    define JSON_HEDLEY_CPP_CAST(T, expr) ((T) (expr))\n#  endif\n#else\n#  define JSON_HEDLEY_CPP_CAST(T, expr) (expr)\n#endif\n\n#if defined(JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED)\n    #undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED\n#endif\n#if JSON_HEDLEY_HAS_WARNING(\"-Wdeprecated-declarations\")\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED _Pragma(\"clang diagnostic ignored \\\"-Wdeprecated-declarations\\\"\")\n#elif JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED _Pragma(\"warning(disable:1478 1786)\")\n#elif JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED __pragma(warning(disable:1478 1786))\n#elif JSON_HEDLEY_PGI_VERSION_CHECK(20,7,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED _Pragma(\"diag_suppress 1215,1216,1444,1445\")\n#elif JSON_HEDLEY_PGI_VERSION_CHECK(17,10,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED _Pragma(\"diag_suppress 1215,1444\")\n#elif JSON_HEDLEY_GCC_VERSION_CHECK(4,3,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED _Pragma(\"GCC diagnostic ignored \\\"-Wdeprecated-declarations\\\"\")\n#elif JSON_HEDLEY_MSVC_VERSION_CHECK(15,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED __pragma(warning(disable:4996))\n#elif JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED _Pragma(\"diag_suppress 1215,1444\")\n#elif \\\n    JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n    (JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,8,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n    (JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,4,0) || \\\n    (JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n    (JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,2,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,5,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED _Pragma(\"diag_suppress 1291,1718\")\n#elif JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,13,0) && !defined(__cplusplus)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED _Pragma(\"error_messages(off,E_DEPRECATED_ATT,E_DEPRECATED_ATT_MESS)\")\n#elif JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,13,0) && defined(__cplusplus)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED _Pragma(\"error_messages(off,symdeprecated,symdeprecated2)\")\n#elif JSON_HEDLEY_IAR_VERSION_CHECK(8,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED _Pragma(\"diag_suppress=Pe1444,Pe1215\")\n#elif JSON_HEDLEY_PELLES_VERSION_CHECK(2,90,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED _Pragma(\"warn(disable:2241)\")\n#else\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED\n#endif\n\n#if defined(JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS)\n    #undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS\n#endif\n#if JSON_HEDLEY_HAS_WARNING(\"-Wunknown-pragmas\")\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS _Pragma(\"clang diagnostic ignored \\\"-Wunknown-pragmas\\\"\")\n#elif JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS _Pragma(\"warning(disable:161)\")\n#elif JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS __pragma(warning(disable:161))\n#elif JSON_HEDLEY_PGI_VERSION_CHECK(17,10,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS _Pragma(\"diag_suppress 1675\")\n#elif JSON_HEDLEY_GCC_VERSION_CHECK(4,3,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS _Pragma(\"GCC diagnostic ignored \\\"-Wunknown-pragmas\\\"\")\n#elif JSON_HEDLEY_MSVC_VERSION_CHECK(15,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS __pragma(warning(disable:4068))\n#elif \\\n    JSON_HEDLEY_TI_VERSION_CHECK(16,9,0) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(8,0,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,3,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS _Pragma(\"diag_suppress 163\")\n#elif JSON_HEDLEY_TI_CL6X_VERSION_CHECK(8,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS _Pragma(\"diag_suppress 163\")\n#elif JSON_HEDLEY_IAR_VERSION_CHECK(8,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS _Pragma(\"diag_suppress=Pe161\")\n#elif JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS _Pragma(\"diag_suppress 161\")\n#else\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS\n#endif\n\n#if defined(JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES)\n    #undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES\n#endif\n#if JSON_HEDLEY_HAS_WARNING(\"-Wunknown-attributes\")\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES _Pragma(\"clang diagnostic ignored \\\"-Wunknown-attributes\\\"\")\n#elif JSON_HEDLEY_GCC_VERSION_CHECK(4,6,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES _Pragma(\"GCC diagnostic ignored \\\"-Wdeprecated-declarations\\\"\")\n#elif JSON_HEDLEY_INTEL_VERSION_CHECK(17,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES _Pragma(\"warning(disable:1292)\")\n#elif JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES __pragma(warning(disable:1292))\n#elif JSON_HEDLEY_MSVC_VERSION_CHECK(19,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES __pragma(warning(disable:5030))\n#elif JSON_HEDLEY_PGI_VERSION_CHECK(20,7,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES _Pragma(\"diag_suppress 1097,1098\")\n#elif JSON_HEDLEY_PGI_VERSION_CHECK(17,10,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES _Pragma(\"diag_suppress 1097\")\n#elif JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,14,0) && defined(__cplusplus)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES _Pragma(\"error_messages(off,attrskipunsup)\")\n#elif \\\n    JSON_HEDLEY_TI_VERSION_CHECK(18,1,0) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(8,3,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES _Pragma(\"diag_suppress 1173\")\n#elif JSON_HEDLEY_IAR_VERSION_CHECK(8,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES _Pragma(\"diag_suppress=Pe1097\")\n#elif JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES _Pragma(\"diag_suppress 1097\")\n#else\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES\n#endif\n\n#if defined(JSON_HEDLEY_DIAGNOSTIC_DISABLE_CAST_QUAL)\n    #undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_CAST_QUAL\n#endif\n#if JSON_HEDLEY_HAS_WARNING(\"-Wcast-qual\")\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_CAST_QUAL _Pragma(\"clang diagnostic ignored \\\"-Wcast-qual\\\"\")\n#elif JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_CAST_QUAL _Pragma(\"warning(disable:2203 2331)\")\n#elif JSON_HEDLEY_GCC_VERSION_CHECK(3,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_CAST_QUAL _Pragma(\"GCC diagnostic ignored \\\"-Wcast-qual\\\"\")\n#else\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_CAST_QUAL\n#endif\n\n#if defined(JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNUSED_FUNCTION)\n    #undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNUSED_FUNCTION\n#endif\n#if JSON_HEDLEY_HAS_WARNING(\"-Wunused-function\")\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNUSED_FUNCTION _Pragma(\"clang diagnostic ignored \\\"-Wunused-function\\\"\")\n#elif JSON_HEDLEY_GCC_VERSION_CHECK(3,4,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNUSED_FUNCTION _Pragma(\"GCC diagnostic ignored \\\"-Wunused-function\\\"\")\n#elif JSON_HEDLEY_MSVC_VERSION_CHECK(1,0,0)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNUSED_FUNCTION __pragma(warning(disable:4505))\n#elif JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNUSED_FUNCTION _Pragma(\"diag_suppress 3142\")\n#else\n    #define JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNUSED_FUNCTION\n#endif\n\n#if defined(JSON_HEDLEY_DEPRECATED)\n    #undef JSON_HEDLEY_DEPRECATED\n#endif\n#if defined(JSON_HEDLEY_DEPRECATED_FOR)\n    #undef JSON_HEDLEY_DEPRECATED_FOR\n#endif\n#if \\\n    JSON_HEDLEY_MSVC_VERSION_CHECK(14,0,0) || \\\n    JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n    #define JSON_HEDLEY_DEPRECATED(since) __declspec(deprecated(\"Since \" # since))\n    #define JSON_HEDLEY_DEPRECATED_FOR(since, replacement) __declspec(deprecated(\"Since \" #since \"; use \" #replacement))\n#elif \\\n    (JSON_HEDLEY_HAS_EXTENSION(attribute_deprecated_with_message) && !defined(JSON_HEDLEY_IAR_VERSION)) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(4,5,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(5,6,0) || \\\n    JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,13,0) || \\\n    JSON_HEDLEY_PGI_VERSION_CHECK(17,10,0) || \\\n    JSON_HEDLEY_TI_VERSION_CHECK(18,1,0) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(18,1,0) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(8,3,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,3,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_DEPRECATED(since) __attribute__((__deprecated__(\"Since \" #since)))\n    #define JSON_HEDLEY_DEPRECATED_FOR(since, replacement) __attribute__((__deprecated__(\"Since \" #since \"; use \" #replacement)))\n#elif defined(__cplusplus) && (__cplusplus >= 201402L)\n    #define JSON_HEDLEY_DEPRECATED(since) JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_([[deprecated(\"Since \" #since)]])\n    #define JSON_HEDLEY_DEPRECATED_FOR(since, replacement) JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_([[deprecated(\"Since \" #since \"; use \" #replacement)]])\n#elif \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(deprecated) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(3,1,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n    JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n    (JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,8,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n    (JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,4,0) || \\\n    (JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n    (JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,2,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,5,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10) || \\\n    JSON_HEDLEY_IAR_VERSION_CHECK(8,10,0)\n    #define JSON_HEDLEY_DEPRECATED(since) __attribute__((__deprecated__))\n    #define JSON_HEDLEY_DEPRECATED_FOR(since, replacement) __attribute__((__deprecated__))\n#elif \\\n    JSON_HEDLEY_MSVC_VERSION_CHECK(13,10,0) || \\\n    JSON_HEDLEY_PELLES_VERSION_CHECK(6,50,0) || \\\n    JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n    #define JSON_HEDLEY_DEPRECATED(since) __declspec(deprecated)\n    #define JSON_HEDLEY_DEPRECATED_FOR(since, replacement) __declspec(deprecated)\n#elif JSON_HEDLEY_IAR_VERSION_CHECK(8,0,0)\n    #define JSON_HEDLEY_DEPRECATED(since) _Pragma(\"deprecated\")\n    #define JSON_HEDLEY_DEPRECATED_FOR(since, replacement) _Pragma(\"deprecated\")\n#else\n    #define JSON_HEDLEY_DEPRECATED(since)\n    #define JSON_HEDLEY_DEPRECATED_FOR(since, replacement)\n#endif\n\n#if defined(JSON_HEDLEY_UNAVAILABLE)\n    #undef JSON_HEDLEY_UNAVAILABLE\n#endif\n#if \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(warning) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(4,3,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_UNAVAILABLE(available_since) __attribute__((__warning__(\"Not available until \" #available_since)))\n#else\n    #define JSON_HEDLEY_UNAVAILABLE(available_since)\n#endif\n\n#if defined(JSON_HEDLEY_WARN_UNUSED_RESULT)\n    #undef JSON_HEDLEY_WARN_UNUSED_RESULT\n#endif\n#if defined(JSON_HEDLEY_WARN_UNUSED_RESULT_MSG)\n    #undef JSON_HEDLEY_WARN_UNUSED_RESULT_MSG\n#endif\n#if \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(warn_unused_result) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(3,4,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n    (JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,8,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n    (JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,4,0) || \\\n    (JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n    (JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,2,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,5,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n    (JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,15,0) && defined(__cplusplus)) || \\\n    JSON_HEDLEY_PGI_VERSION_CHECK(17,10,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_WARN_UNUSED_RESULT __attribute__((__warn_unused_result__))\n    #define JSON_HEDLEY_WARN_UNUSED_RESULT_MSG(msg) __attribute__((__warn_unused_result__))\n#elif (JSON_HEDLEY_HAS_CPP_ATTRIBUTE(nodiscard) >= 201907L)\n    #define JSON_HEDLEY_WARN_UNUSED_RESULT JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_([[nodiscard]])\n    #define JSON_HEDLEY_WARN_UNUSED_RESULT_MSG(msg) JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_([[nodiscard(msg)]])\n#elif JSON_HEDLEY_HAS_CPP_ATTRIBUTE(nodiscard)\n    #define JSON_HEDLEY_WARN_UNUSED_RESULT JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_([[nodiscard]])\n    #define JSON_HEDLEY_WARN_UNUSED_RESULT_MSG(msg) JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_([[nodiscard]])\n#elif defined(_Check_return_) /* SAL */\n    #define JSON_HEDLEY_WARN_UNUSED_RESULT _Check_return_\n    #define JSON_HEDLEY_WARN_UNUSED_RESULT_MSG(msg) _Check_return_\n#else\n    #define JSON_HEDLEY_WARN_UNUSED_RESULT\n    #define JSON_HEDLEY_WARN_UNUSED_RESULT_MSG(msg)\n#endif\n\n#if defined(JSON_HEDLEY_SENTINEL)\n    #undef JSON_HEDLEY_SENTINEL\n#endif\n#if \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(sentinel) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(4,0,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(5,4,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_SENTINEL(position) __attribute__((__sentinel__(position)))\n#else\n    #define JSON_HEDLEY_SENTINEL(position)\n#endif\n\n#if defined(JSON_HEDLEY_NO_RETURN)\n    #undef JSON_HEDLEY_NO_RETURN\n#endif\n#if JSON_HEDLEY_IAR_VERSION_CHECK(8,0,0)\n    #define JSON_HEDLEY_NO_RETURN __noreturn\n#elif \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_NO_RETURN __attribute__((__noreturn__))\n#elif defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L\n    #define JSON_HEDLEY_NO_RETURN _Noreturn\n#elif defined(__cplusplus) && (__cplusplus >= 201103L)\n    #define JSON_HEDLEY_NO_RETURN JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_([[noreturn]])\n#elif \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(noreturn) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(3,2,0) || \\\n    JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,11,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n    JSON_HEDLEY_IBM_VERSION_CHECK(10,1,0) || \\\n    JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n    (JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,8,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n    (JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,4,0) || \\\n    (JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n    (JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,2,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,5,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n    JSON_HEDLEY_IAR_VERSION_CHECK(8,10,0)\n    #define JSON_HEDLEY_NO_RETURN __attribute__((__noreturn__))\n#elif JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,10,0)\n    #define JSON_HEDLEY_NO_RETURN _Pragma(\"does_not_return\")\n#elif \\\n    JSON_HEDLEY_MSVC_VERSION_CHECK(13,10,0) || \\\n    JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n    #define JSON_HEDLEY_NO_RETURN __declspec(noreturn)\n#elif JSON_HEDLEY_TI_CL6X_VERSION_CHECK(6,0,0) && defined(__cplusplus)\n    #define JSON_HEDLEY_NO_RETURN _Pragma(\"FUNC_NEVER_RETURNS;\")\n#elif JSON_HEDLEY_COMPCERT_VERSION_CHECK(3,2,0)\n    #define JSON_HEDLEY_NO_RETURN __attribute((noreturn))\n#elif JSON_HEDLEY_PELLES_VERSION_CHECK(9,0,0)\n    #define JSON_HEDLEY_NO_RETURN __declspec(noreturn)\n#else\n    #define JSON_HEDLEY_NO_RETURN\n#endif\n\n#if defined(JSON_HEDLEY_NO_ESCAPE)\n    #undef JSON_HEDLEY_NO_ESCAPE\n#endif\n#if JSON_HEDLEY_HAS_ATTRIBUTE(noescape)\n    #define JSON_HEDLEY_NO_ESCAPE __attribute__((__noescape__))\n#else\n    #define JSON_HEDLEY_NO_ESCAPE\n#endif\n\n#if defined(JSON_HEDLEY_UNREACHABLE)\n    #undef JSON_HEDLEY_UNREACHABLE\n#endif\n#if defined(JSON_HEDLEY_UNREACHABLE_RETURN)\n    #undef JSON_HEDLEY_UNREACHABLE_RETURN\n#endif\n#if defined(JSON_HEDLEY_ASSUME)\n    #undef JSON_HEDLEY_ASSUME\n#endif\n#if \\\n    JSON_HEDLEY_MSVC_VERSION_CHECK(13,10,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n    #define JSON_HEDLEY_ASSUME(expr) __assume(expr)\n#elif JSON_HEDLEY_HAS_BUILTIN(__builtin_assume)\n    #define JSON_HEDLEY_ASSUME(expr) __builtin_assume(expr)\n#elif \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,2,0) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(4,0,0)\n    #if defined(__cplusplus)\n        #define JSON_HEDLEY_ASSUME(expr) std::_nassert(expr)\n    #else\n        #define JSON_HEDLEY_ASSUME(expr) _nassert(expr)\n    #endif\n#endif\n#if \\\n    (JSON_HEDLEY_HAS_BUILTIN(__builtin_unreachable) && (!defined(JSON_HEDLEY_ARM_VERSION))) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(4,5,0) || \\\n    JSON_HEDLEY_PGI_VERSION_CHECK(18,10,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_IBM_VERSION_CHECK(13,1,5) || \\\n    JSON_HEDLEY_CRAY_VERSION_CHECK(10,0,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_UNREACHABLE() __builtin_unreachable()\n#elif defined(JSON_HEDLEY_ASSUME)\n    #define JSON_HEDLEY_UNREACHABLE() JSON_HEDLEY_ASSUME(0)\n#endif\n#if !defined(JSON_HEDLEY_ASSUME)\n    #if defined(JSON_HEDLEY_UNREACHABLE)\n        #define JSON_HEDLEY_ASSUME(expr) JSON_HEDLEY_STATIC_CAST(void, ((expr) ? 1 : (JSON_HEDLEY_UNREACHABLE(), 1)))\n    #else\n        #define JSON_HEDLEY_ASSUME(expr) JSON_HEDLEY_STATIC_CAST(void, expr)\n    #endif\n#endif\n#if defined(JSON_HEDLEY_UNREACHABLE)\n    #if  \\\n        JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,2,0) || \\\n        JSON_HEDLEY_TI_CL6X_VERSION_CHECK(4,0,0)\n        #define JSON_HEDLEY_UNREACHABLE_RETURN(value) return (JSON_HEDLEY_STATIC_CAST(void, JSON_HEDLEY_ASSUME(0)), (value))\n    #else\n        #define JSON_HEDLEY_UNREACHABLE_RETURN(value) JSON_HEDLEY_UNREACHABLE()\n    #endif\n#else\n    #define JSON_HEDLEY_UNREACHABLE_RETURN(value) return (value)\n#endif\n#if !defined(JSON_HEDLEY_UNREACHABLE)\n    #define JSON_HEDLEY_UNREACHABLE() JSON_HEDLEY_ASSUME(0)\n#endif\n\nJSON_HEDLEY_DIAGNOSTIC_PUSH\n#if JSON_HEDLEY_HAS_WARNING(\"-Wpedantic\")\n    #pragma clang diagnostic ignored \"-Wpedantic\"\n#endif\n#if JSON_HEDLEY_HAS_WARNING(\"-Wc++98-compat-pedantic\") && defined(__cplusplus)\n    #pragma clang diagnostic ignored \"-Wc++98-compat-pedantic\"\n#endif\n#if JSON_HEDLEY_GCC_HAS_WARNING(\"-Wvariadic-macros\",4,0,0)\n    #if defined(__clang__)\n        #pragma clang diagnostic ignored \"-Wvariadic-macros\"\n    #elif defined(JSON_HEDLEY_GCC_VERSION)\n        #pragma GCC diagnostic ignored \"-Wvariadic-macros\"\n    #endif\n#endif\n#if defined(JSON_HEDLEY_NON_NULL)\n    #undef JSON_HEDLEY_NON_NULL\n#endif\n#if \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(nonnull) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(3,3,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0)\n    #define JSON_HEDLEY_NON_NULL(...) __attribute__((__nonnull__(__VA_ARGS__)))\n#else\n    #define JSON_HEDLEY_NON_NULL(...)\n#endif\nJSON_HEDLEY_DIAGNOSTIC_POP\n\n#if defined(JSON_HEDLEY_PRINTF_FORMAT)\n    #undef JSON_HEDLEY_PRINTF_FORMAT\n#endif\n#if defined(__MINGW32__) && JSON_HEDLEY_GCC_HAS_ATTRIBUTE(format,4,4,0) && !defined(__USE_MINGW_ANSI_STDIO)\n    #define JSON_HEDLEY_PRINTF_FORMAT(string_idx,first_to_check) __attribute__((__format__(ms_printf, string_idx, first_to_check)))\n#elif defined(__MINGW32__) && JSON_HEDLEY_GCC_HAS_ATTRIBUTE(format,4,4,0) && defined(__USE_MINGW_ANSI_STDIO)\n    #define JSON_HEDLEY_PRINTF_FORMAT(string_idx,first_to_check) __attribute__((__format__(gnu_printf, string_idx, first_to_check)))\n#elif \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(format) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(3,1,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(5,6,0) || \\\n    JSON_HEDLEY_IBM_VERSION_CHECK(10,1,0) || \\\n    JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n    (JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,8,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n    (JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,4,0) || \\\n    (JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n    (JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,2,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,5,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_PRINTF_FORMAT(string_idx,first_to_check) __attribute__((__format__(__printf__, string_idx, first_to_check)))\n#elif JSON_HEDLEY_PELLES_VERSION_CHECK(6,0,0)\n    #define JSON_HEDLEY_PRINTF_FORMAT(string_idx,first_to_check) __declspec(vaformat(printf,string_idx,first_to_check))\n#else\n    #define JSON_HEDLEY_PRINTF_FORMAT(string_idx,first_to_check)\n#endif\n\n#if defined(JSON_HEDLEY_CONSTEXPR)\n    #undef JSON_HEDLEY_CONSTEXPR\n#endif\n#if defined(__cplusplus)\n    #if __cplusplus >= 201103L\n        #define JSON_HEDLEY_CONSTEXPR JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_(constexpr)\n    #endif\n#endif\n#if !defined(JSON_HEDLEY_CONSTEXPR)\n    #define JSON_HEDLEY_CONSTEXPR\n#endif\n\n#if defined(JSON_HEDLEY_PREDICT)\n    #undef JSON_HEDLEY_PREDICT\n#endif\n#if defined(JSON_HEDLEY_LIKELY)\n    #undef JSON_HEDLEY_LIKELY\n#endif\n#if defined(JSON_HEDLEY_UNLIKELY)\n    #undef JSON_HEDLEY_UNLIKELY\n#endif\n#if defined(JSON_HEDLEY_UNPREDICTABLE)\n    #undef JSON_HEDLEY_UNPREDICTABLE\n#endif\n#if JSON_HEDLEY_HAS_BUILTIN(__builtin_unpredictable)\n    #define JSON_HEDLEY_UNPREDICTABLE(expr) __builtin_unpredictable((expr))\n#endif\n#if \\\n  (JSON_HEDLEY_HAS_BUILTIN(__builtin_expect_with_probability) && !defined(JSON_HEDLEY_PGI_VERSION)) || \\\n  JSON_HEDLEY_GCC_VERSION_CHECK(9,0,0) || \\\n  JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n#  define JSON_HEDLEY_PREDICT(expr, value, probability) __builtin_expect_with_probability(  (expr), (value), (probability))\n#  define JSON_HEDLEY_PREDICT_TRUE(expr, probability)   __builtin_expect_with_probability(!!(expr),    1   , (probability))\n#  define JSON_HEDLEY_PREDICT_FALSE(expr, probability)  __builtin_expect_with_probability(!!(expr),    0   , (probability))\n#  define JSON_HEDLEY_LIKELY(expr)                      __builtin_expect                 (!!(expr),    1                  )\n#  define JSON_HEDLEY_UNLIKELY(expr)                    __builtin_expect                 (!!(expr),    0                  )\n#elif \\\n  (JSON_HEDLEY_HAS_BUILTIN(__builtin_expect) && !defined(JSON_HEDLEY_INTEL_CL_VERSION)) || \\\n  JSON_HEDLEY_GCC_VERSION_CHECK(3,0,0) || \\\n  JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n  (JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,15,0) && defined(__cplusplus)) || \\\n  JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n  JSON_HEDLEY_IBM_VERSION_CHECK(10,1,0) || \\\n  JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n  JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,7,0) || \\\n  JSON_HEDLEY_TI_CL430_VERSION_CHECK(3,1,0) || \\\n  JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,1,0) || \\\n  JSON_HEDLEY_TI_CL6X_VERSION_CHECK(6,1,0) || \\\n  JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n  JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n  JSON_HEDLEY_TINYC_VERSION_CHECK(0,9,27) || \\\n  JSON_HEDLEY_CRAY_VERSION_CHECK(8,1,0) || \\\n  JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n#  define JSON_HEDLEY_PREDICT(expr, expected, probability) \\\n    (((probability) >= 0.9) ? __builtin_expect((expr), (expected)) : (JSON_HEDLEY_STATIC_CAST(void, expected), (expr)))\n#  define JSON_HEDLEY_PREDICT_TRUE(expr, probability) \\\n    (__extension__ ({ \\\n        double hedley_probability_ = (probability); \\\n        ((hedley_probability_ >= 0.9) ? __builtin_expect(!!(expr), 1) : ((hedley_probability_ <= 0.1) ? __builtin_expect(!!(expr), 0) : !!(expr))); \\\n    }))\n#  define JSON_HEDLEY_PREDICT_FALSE(expr, probability) \\\n    (__extension__ ({ \\\n        double hedley_probability_ = (probability); \\\n        ((hedley_probability_ >= 0.9) ? __builtin_expect(!!(expr), 0) : ((hedley_probability_ <= 0.1) ? __builtin_expect(!!(expr), 1) : !!(expr))); \\\n    }))\n#  define JSON_HEDLEY_LIKELY(expr)   __builtin_expect(!!(expr), 1)\n#  define JSON_HEDLEY_UNLIKELY(expr) __builtin_expect(!!(expr), 0)\n#else\n#  define JSON_HEDLEY_PREDICT(expr, expected, probability) (JSON_HEDLEY_STATIC_CAST(void, expected), (expr))\n#  define JSON_HEDLEY_PREDICT_TRUE(expr, probability) (!!(expr))\n#  define JSON_HEDLEY_PREDICT_FALSE(expr, probability) (!!(expr))\n#  define JSON_HEDLEY_LIKELY(expr) (!!(expr))\n#  define JSON_HEDLEY_UNLIKELY(expr) (!!(expr))\n#endif\n#if !defined(JSON_HEDLEY_UNPREDICTABLE)\n    #define JSON_HEDLEY_UNPREDICTABLE(expr) JSON_HEDLEY_PREDICT(expr, 1, 0.5)\n#endif\n\n#if defined(JSON_HEDLEY_MALLOC)\n    #undef JSON_HEDLEY_MALLOC\n#endif\n#if \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(malloc) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(3,1,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,11,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n    JSON_HEDLEY_IBM_VERSION_CHECK(12,1,0) || \\\n    JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n    (JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,8,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n    (JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,4,0) || \\\n    (JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n    (JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,2,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,5,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_MALLOC __attribute__((__malloc__))\n#elif JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,10,0)\n    #define JSON_HEDLEY_MALLOC _Pragma(\"returns_new_memory\")\n#elif \\\n    JSON_HEDLEY_MSVC_VERSION_CHECK(14,0,0) || \\\n    JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n    #define JSON_HEDLEY_MALLOC __declspec(restrict)\n#else\n    #define JSON_HEDLEY_MALLOC\n#endif\n\n#if defined(JSON_HEDLEY_PURE)\n    #undef JSON_HEDLEY_PURE\n#endif\n#if \\\n  JSON_HEDLEY_HAS_ATTRIBUTE(pure) || \\\n  JSON_HEDLEY_GCC_VERSION_CHECK(2,96,0) || \\\n  JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n  JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,11,0) || \\\n  JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n  JSON_HEDLEY_IBM_VERSION_CHECK(10,1,0) || \\\n  JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n  (JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,8,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n  JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n  (JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n  JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,4,0) || \\\n  (JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n  JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n  (JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,2,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n  JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,5,0) || \\\n  JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n  JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n  JSON_HEDLEY_PGI_VERSION_CHECK(17,10,0) || \\\n  JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n#  define JSON_HEDLEY_PURE __attribute__((__pure__))\n#elif JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,10,0)\n#  define JSON_HEDLEY_PURE _Pragma(\"does_not_write_global_data\")\n#elif defined(__cplusplus) && \\\n    ( \\\n      JSON_HEDLEY_TI_CL430_VERSION_CHECK(2,0,1) || \\\n      JSON_HEDLEY_TI_CL6X_VERSION_CHECK(4,0,0) || \\\n      JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) \\\n    )\n#  define JSON_HEDLEY_PURE _Pragma(\"FUNC_IS_PURE;\")\n#else\n#  define JSON_HEDLEY_PURE\n#endif\n\n#if defined(JSON_HEDLEY_CONST)\n    #undef JSON_HEDLEY_CONST\n#endif\n#if \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(const) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(2,5,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,11,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n    JSON_HEDLEY_IBM_VERSION_CHECK(10,1,0) || \\\n    JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n    (JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,8,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n    (JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,4,0) || \\\n    (JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n    (JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,2,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,5,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n    JSON_HEDLEY_PGI_VERSION_CHECK(17,10,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_CONST __attribute__((__const__))\n#elif \\\n    JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,10,0)\n    #define JSON_HEDLEY_CONST _Pragma(\"no_side_effect\")\n#else\n    #define JSON_HEDLEY_CONST JSON_HEDLEY_PURE\n#endif\n\n#if defined(JSON_HEDLEY_RESTRICT)\n    #undef JSON_HEDLEY_RESTRICT\n#endif\n#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L) && !defined(__cplusplus)\n    #define JSON_HEDLEY_RESTRICT restrict\n#elif \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(3,1,0) || \\\n    JSON_HEDLEY_MSVC_VERSION_CHECK(14,0,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n    JSON_HEDLEY_IBM_VERSION_CHECK(10,1,0) || \\\n    JSON_HEDLEY_PGI_VERSION_CHECK(17,10,0) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,2,4) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(8,1,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    (JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,14,0) && defined(__cplusplus)) || \\\n    JSON_HEDLEY_IAR_VERSION_CHECK(8,0,0) || \\\n    defined(__clang__) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_RESTRICT __restrict\n#elif JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,3,0) && !defined(__cplusplus)\n    #define JSON_HEDLEY_RESTRICT _Restrict\n#else\n    #define JSON_HEDLEY_RESTRICT\n#endif\n\n#if defined(JSON_HEDLEY_INLINE)\n    #undef JSON_HEDLEY_INLINE\n#endif\n#if \\\n    (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)) || \\\n    (defined(__cplusplus) && (__cplusplus >= 199711L))\n    #define JSON_HEDLEY_INLINE inline\n#elif \\\n    defined(JSON_HEDLEY_GCC_VERSION) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(6,2,0)\n    #define JSON_HEDLEY_INLINE __inline__\n#elif \\\n    JSON_HEDLEY_MSVC_VERSION_CHECK(12,0,0) || \\\n    JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,1,0) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(3,1,0) || \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,2,0) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(8,0,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_INLINE __inline\n#else\n    #define JSON_HEDLEY_INLINE\n#endif\n\n#if defined(JSON_HEDLEY_ALWAYS_INLINE)\n    #undef JSON_HEDLEY_ALWAYS_INLINE\n#endif\n#if \\\n  JSON_HEDLEY_HAS_ATTRIBUTE(always_inline) || \\\n  JSON_HEDLEY_GCC_VERSION_CHECK(4,0,0) || \\\n  JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n  JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,11,0) || \\\n  JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n  JSON_HEDLEY_IBM_VERSION_CHECK(10,1,0) || \\\n  JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n  (JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,8,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n  JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n  (JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n  JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,4,0) || \\\n  (JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n  JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n  (JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,2,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n  JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,5,0) || \\\n  JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n  JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n  JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10) || \\\n  JSON_HEDLEY_IAR_VERSION_CHECK(8,10,0)\n#  define JSON_HEDLEY_ALWAYS_INLINE __attribute__((__always_inline__)) JSON_HEDLEY_INLINE\n#elif \\\n  JSON_HEDLEY_MSVC_VERSION_CHECK(12,0,0) || \\\n  JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n#  define JSON_HEDLEY_ALWAYS_INLINE __forceinline\n#elif defined(__cplusplus) && \\\n    ( \\\n      JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n      JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n      JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,4,0) || \\\n      JSON_HEDLEY_TI_CL6X_VERSION_CHECK(6,1,0) || \\\n      JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n      JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) \\\n    )\n#  define JSON_HEDLEY_ALWAYS_INLINE _Pragma(\"FUNC_ALWAYS_INLINE;\")\n#elif JSON_HEDLEY_IAR_VERSION_CHECK(8,0,0)\n#  define JSON_HEDLEY_ALWAYS_INLINE _Pragma(\"inline=forced\")\n#else\n#  define JSON_HEDLEY_ALWAYS_INLINE JSON_HEDLEY_INLINE\n#endif\n\n#if defined(JSON_HEDLEY_NEVER_INLINE)\n    #undef JSON_HEDLEY_NEVER_INLINE\n#endif\n#if \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(noinline) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(4,0,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,11,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n    JSON_HEDLEY_IBM_VERSION_CHECK(10,1,0) || \\\n    JSON_HEDLEY_TI_VERSION_CHECK(15,12,0) || \\\n    (JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(4,8,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_ARMCL_VERSION_CHECK(5,2,0) || \\\n    (JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL2000_VERSION_CHECK(6,4,0) || \\\n    (JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,0,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL430_VERSION_CHECK(4,3,0) || \\\n    (JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,2,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,5,0) || \\\n    JSON_HEDLEY_TI_CL7X_VERSION_CHECK(1,2,0) || \\\n    JSON_HEDLEY_TI_CLPRU_VERSION_CHECK(2,1,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10) || \\\n    JSON_HEDLEY_IAR_VERSION_CHECK(8,10,0)\n    #define JSON_HEDLEY_NEVER_INLINE __attribute__((__noinline__))\n#elif \\\n    JSON_HEDLEY_MSVC_VERSION_CHECK(13,10,0) || \\\n    JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n    #define JSON_HEDLEY_NEVER_INLINE __declspec(noinline)\n#elif JSON_HEDLEY_PGI_VERSION_CHECK(10,2,0)\n    #define JSON_HEDLEY_NEVER_INLINE _Pragma(\"noinline\")\n#elif JSON_HEDLEY_TI_CL6X_VERSION_CHECK(6,0,0) && defined(__cplusplus)\n    #define JSON_HEDLEY_NEVER_INLINE _Pragma(\"FUNC_CANNOT_INLINE;\")\n#elif JSON_HEDLEY_IAR_VERSION_CHECK(8,0,0)\n    #define JSON_HEDLEY_NEVER_INLINE _Pragma(\"inline=never\")\n#elif JSON_HEDLEY_COMPCERT_VERSION_CHECK(3,2,0)\n    #define JSON_HEDLEY_NEVER_INLINE __attribute((noinline))\n#elif JSON_HEDLEY_PELLES_VERSION_CHECK(9,0,0)\n    #define JSON_HEDLEY_NEVER_INLINE __declspec(noinline)\n#else\n    #define JSON_HEDLEY_NEVER_INLINE\n#endif\n\n#if defined(JSON_HEDLEY_PRIVATE)\n    #undef JSON_HEDLEY_PRIVATE\n#endif\n#if defined(JSON_HEDLEY_PUBLIC)\n    #undef JSON_HEDLEY_PUBLIC\n#endif\n#if defined(JSON_HEDLEY_IMPORT)\n    #undef JSON_HEDLEY_IMPORT\n#endif\n#if defined(_WIN32) || defined(__CYGWIN__)\n#  define JSON_HEDLEY_PRIVATE\n#  define JSON_HEDLEY_PUBLIC   __declspec(dllexport)\n#  define JSON_HEDLEY_IMPORT   __declspec(dllimport)\n#else\n#  if \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(visibility) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(3,3,0) || \\\n    JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,11,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n    JSON_HEDLEY_IBM_VERSION_CHECK(13,1,0) || \\\n    ( \\\n      defined(__TI_EABI__) && \\\n      ( \\\n        (JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,2,0) && defined(__TI_GNU_ATTRIBUTE_SUPPORT__)) || \\\n        JSON_HEDLEY_TI_CL6X_VERSION_CHECK(7,5,0) \\\n      ) \\\n    ) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n#    define JSON_HEDLEY_PRIVATE __attribute__((__visibility__(\"hidden\")))\n#    define JSON_HEDLEY_PUBLIC  __attribute__((__visibility__(\"default\")))\n#  else\n#    define JSON_HEDLEY_PRIVATE\n#    define JSON_HEDLEY_PUBLIC\n#  endif\n#  define JSON_HEDLEY_IMPORT    extern\n#endif\n\n#if defined(JSON_HEDLEY_NO_THROW)\n    #undef JSON_HEDLEY_NO_THROW\n#endif\n#if \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(nothrow) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(3,3,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_NO_THROW __attribute__((__nothrow__))\n#elif \\\n    JSON_HEDLEY_MSVC_VERSION_CHECK(13,1,0) || \\\n    JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0)\n    #define JSON_HEDLEY_NO_THROW __declspec(nothrow)\n#else\n    #define JSON_HEDLEY_NO_THROW\n#endif\n\n#if defined(JSON_HEDLEY_FALL_THROUGH)\n    #undef JSON_HEDLEY_FALL_THROUGH\n#endif\n#if \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(fallthrough) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(7,0,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_FALL_THROUGH __attribute__((__fallthrough__))\n#elif JSON_HEDLEY_HAS_CPP_ATTRIBUTE_NS(clang,fallthrough)\n    #define JSON_HEDLEY_FALL_THROUGH JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_([[clang::fallthrough]])\n#elif JSON_HEDLEY_HAS_CPP_ATTRIBUTE(fallthrough)\n    #define JSON_HEDLEY_FALL_THROUGH JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_([[fallthrough]])\n#elif defined(__fallthrough) /* SAL */\n    #define JSON_HEDLEY_FALL_THROUGH __fallthrough\n#else\n    #define JSON_HEDLEY_FALL_THROUGH\n#endif\n\n#if defined(JSON_HEDLEY_RETURNS_NON_NULL)\n    #undef JSON_HEDLEY_RETURNS_NON_NULL\n#endif\n#if \\\n    JSON_HEDLEY_HAS_ATTRIBUTE(returns_nonnull) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(4,9,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_RETURNS_NON_NULL __attribute__((__returns_nonnull__))\n#elif defined(_Ret_notnull_) /* SAL */\n    #define JSON_HEDLEY_RETURNS_NON_NULL _Ret_notnull_\n#else\n    #define JSON_HEDLEY_RETURNS_NON_NULL\n#endif\n\n#if defined(JSON_HEDLEY_ARRAY_PARAM)\n    #undef JSON_HEDLEY_ARRAY_PARAM\n#endif\n#if \\\n    defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L) && \\\n    !defined(__STDC_NO_VLA__) && \\\n    !defined(__cplusplus) && \\\n    !defined(JSON_HEDLEY_PGI_VERSION) && \\\n    !defined(JSON_HEDLEY_TINYC_VERSION)\n    #define JSON_HEDLEY_ARRAY_PARAM(name) (name)\n#else\n    #define JSON_HEDLEY_ARRAY_PARAM(name)\n#endif\n\n#if defined(JSON_HEDLEY_IS_CONSTANT)\n    #undef JSON_HEDLEY_IS_CONSTANT\n#endif\n#if defined(JSON_HEDLEY_REQUIRE_CONSTEXPR)\n    #undef JSON_HEDLEY_REQUIRE_CONSTEXPR\n#endif\n/* JSON_HEDLEY_IS_CONSTEXPR_ is for\n   HEDLEY INTERNAL USE ONLY.  API subject to change without notice. */\n#if defined(JSON_HEDLEY_IS_CONSTEXPR_)\n    #undef JSON_HEDLEY_IS_CONSTEXPR_\n#endif\n#if \\\n    JSON_HEDLEY_HAS_BUILTIN(__builtin_constant_p) || \\\n    JSON_HEDLEY_GCC_VERSION_CHECK(3,4,0) || \\\n    JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n    JSON_HEDLEY_TINYC_VERSION_CHECK(0,9,19) || \\\n    JSON_HEDLEY_ARM_VERSION_CHECK(4,1,0) || \\\n    JSON_HEDLEY_IBM_VERSION_CHECK(13,1,0) || \\\n    JSON_HEDLEY_TI_CL6X_VERSION_CHECK(6,1,0) || \\\n    (JSON_HEDLEY_SUNPRO_VERSION_CHECK(5,10,0) && !defined(__cplusplus)) || \\\n    JSON_HEDLEY_CRAY_VERSION_CHECK(8,1,0) || \\\n    JSON_HEDLEY_MCST_LCC_VERSION_CHECK(1,25,10)\n    #define JSON_HEDLEY_IS_CONSTANT(expr) __builtin_constant_p(expr)\n#endif\n#if !defined(__cplusplus)\n#  if \\\n       JSON_HEDLEY_HAS_BUILTIN(__builtin_types_compatible_p) || \\\n       JSON_HEDLEY_GCC_VERSION_CHECK(3,4,0) || \\\n       JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n       JSON_HEDLEY_IBM_VERSION_CHECK(13,1,0) || \\\n       JSON_HEDLEY_CRAY_VERSION_CHECK(8,1,0) || \\\n       JSON_HEDLEY_ARM_VERSION_CHECK(5,4,0) || \\\n       JSON_HEDLEY_TINYC_VERSION_CHECK(0,9,24)\n#if defined(__INTPTR_TYPE__)\n    #define JSON_HEDLEY_IS_CONSTEXPR_(expr) __builtin_types_compatible_p(__typeof__((1 ? (void*) ((__INTPTR_TYPE__) ((expr) * 0)) : (int*) 0)), int*)\n#else\n    #include <stdint.h>\n    #define JSON_HEDLEY_IS_CONSTEXPR_(expr) __builtin_types_compatible_p(__typeof__((1 ? (void*) ((intptr_t) ((expr) * 0)) : (int*) 0)), int*)\n#endif\n#  elif \\\n       ( \\\n          defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201112L) && \\\n          !defined(JSON_HEDLEY_SUNPRO_VERSION) && \\\n          !defined(JSON_HEDLEY_PGI_VERSION) && \\\n          !defined(JSON_HEDLEY_IAR_VERSION)) || \\\n       (JSON_HEDLEY_HAS_EXTENSION(c_generic_selections) && !defined(JSON_HEDLEY_IAR_VERSION)) || \\\n       JSON_HEDLEY_GCC_VERSION_CHECK(4,9,0) || \\\n       JSON_HEDLEY_INTEL_VERSION_CHECK(17,0,0) || \\\n       JSON_HEDLEY_IBM_VERSION_CHECK(12,1,0) || \\\n       JSON_HEDLEY_ARM_VERSION_CHECK(5,3,0)\n#if defined(__INTPTR_TYPE__)\n    #define JSON_HEDLEY_IS_CONSTEXPR_(expr) _Generic((1 ? (void*) ((__INTPTR_TYPE__) ((expr) * 0)) : (int*) 0), int*: 1, void*: 0)\n#else\n    #include <stdint.h>\n    #define JSON_HEDLEY_IS_CONSTEXPR_(expr) _Generic((1 ? (void*) ((intptr_t) * 0) : (int*) 0), int*: 1, void*: 0)\n#endif\n#  elif \\\n       defined(JSON_HEDLEY_GCC_VERSION) || \\\n       defined(JSON_HEDLEY_INTEL_VERSION) || \\\n       defined(JSON_HEDLEY_TINYC_VERSION) || \\\n       defined(JSON_HEDLEY_TI_ARMCL_VERSION) || \\\n       JSON_HEDLEY_TI_CL430_VERSION_CHECK(18,12,0) || \\\n       defined(JSON_HEDLEY_TI_CL2000_VERSION) || \\\n       defined(JSON_HEDLEY_TI_CL6X_VERSION) || \\\n       defined(JSON_HEDLEY_TI_CL7X_VERSION) || \\\n       defined(JSON_HEDLEY_TI_CLPRU_VERSION) || \\\n       defined(__clang__)\n#    define JSON_HEDLEY_IS_CONSTEXPR_(expr) ( \\\n        sizeof(void) != \\\n        sizeof(*( \\\n                  1 ? \\\n                  ((void*) ((expr) * 0L) ) : \\\n((struct { char v[sizeof(void) * 2]; } *) 1) \\\n                ) \\\n              ) \\\n                                            )\n#  endif\n#endif\n#if defined(JSON_HEDLEY_IS_CONSTEXPR_)\n    #if !defined(JSON_HEDLEY_IS_CONSTANT)\n        #define JSON_HEDLEY_IS_CONSTANT(expr) JSON_HEDLEY_IS_CONSTEXPR_(expr)\n    #endif\n    #define JSON_HEDLEY_REQUIRE_CONSTEXPR(expr) (JSON_HEDLEY_IS_CONSTEXPR_(expr) ? (expr) : (-1))\n#else\n    #if !defined(JSON_HEDLEY_IS_CONSTANT)\n        #define JSON_HEDLEY_IS_CONSTANT(expr) (0)\n    #endif\n    #define JSON_HEDLEY_REQUIRE_CONSTEXPR(expr) (expr)\n#endif\n\n#if defined(JSON_HEDLEY_BEGIN_C_DECLS)\n    #undef JSON_HEDLEY_BEGIN_C_DECLS\n#endif\n#if defined(JSON_HEDLEY_END_C_DECLS)\n    #undef JSON_HEDLEY_END_C_DECLS\n#endif\n#if defined(JSON_HEDLEY_C_DECL)\n    #undef JSON_HEDLEY_C_DECL\n#endif\n#if defined(__cplusplus)\n    #define JSON_HEDLEY_BEGIN_C_DECLS extern \"C\" {\n    #define JSON_HEDLEY_END_C_DECLS }\n    #define JSON_HEDLEY_C_DECL extern \"C\"\n#else\n    #define JSON_HEDLEY_BEGIN_C_DECLS\n    #define JSON_HEDLEY_END_C_DECLS\n    #define JSON_HEDLEY_C_DECL\n#endif\n\n#if defined(JSON_HEDLEY_STATIC_ASSERT)\n    #undef JSON_HEDLEY_STATIC_ASSERT\n#endif\n#if \\\n  !defined(__cplusplus) && ( \\\n      (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201112L)) || \\\n      (JSON_HEDLEY_HAS_FEATURE(c_static_assert) && !defined(JSON_HEDLEY_INTEL_CL_VERSION)) || \\\n      JSON_HEDLEY_GCC_VERSION_CHECK(6,0,0) || \\\n      JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0) || \\\n      defined(_Static_assert) \\\n    )\n#  define JSON_HEDLEY_STATIC_ASSERT(expr, message) _Static_assert(expr, message)\n#elif \\\n  (defined(__cplusplus) && (__cplusplus >= 201103L)) || \\\n  JSON_HEDLEY_MSVC_VERSION_CHECK(16,0,0) || \\\n  JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n#  define JSON_HEDLEY_STATIC_ASSERT(expr, message) JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_(static_assert(expr, message))\n#else\n#  define JSON_HEDLEY_STATIC_ASSERT(expr, message)\n#endif\n\n#if defined(JSON_HEDLEY_NULL)\n    #undef JSON_HEDLEY_NULL\n#endif\n#if defined(__cplusplus)\n    #if __cplusplus >= 201103L\n        #define JSON_HEDLEY_NULL JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_(nullptr)\n    #elif defined(NULL)\n        #define JSON_HEDLEY_NULL NULL\n    #else\n        #define JSON_HEDLEY_NULL JSON_HEDLEY_STATIC_CAST(void*, 0)\n    #endif\n#elif defined(NULL)\n    #define JSON_HEDLEY_NULL NULL\n#else\n    #define JSON_HEDLEY_NULL ((void*) 0)\n#endif\n\n#if defined(JSON_HEDLEY_MESSAGE)\n    #undef JSON_HEDLEY_MESSAGE\n#endif\n#if JSON_HEDLEY_HAS_WARNING(\"-Wunknown-pragmas\")\n#  define JSON_HEDLEY_MESSAGE(msg) \\\n    JSON_HEDLEY_DIAGNOSTIC_PUSH \\\n    JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS \\\n    JSON_HEDLEY_PRAGMA(message msg) \\\n    JSON_HEDLEY_DIAGNOSTIC_POP\n#elif \\\n  JSON_HEDLEY_GCC_VERSION_CHECK(4,4,0) || \\\n  JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0)\n#  define JSON_HEDLEY_MESSAGE(msg) JSON_HEDLEY_PRAGMA(message msg)\n#elif JSON_HEDLEY_CRAY_VERSION_CHECK(5,0,0)\n#  define JSON_HEDLEY_MESSAGE(msg) JSON_HEDLEY_PRAGMA(_CRI message msg)\n#elif JSON_HEDLEY_IAR_VERSION_CHECK(8,0,0)\n#  define JSON_HEDLEY_MESSAGE(msg) JSON_HEDLEY_PRAGMA(message(msg))\n#elif JSON_HEDLEY_PELLES_VERSION_CHECK(2,0,0)\n#  define JSON_HEDLEY_MESSAGE(msg) JSON_HEDLEY_PRAGMA(message(msg))\n#else\n#  define JSON_HEDLEY_MESSAGE(msg)\n#endif\n\n#if defined(JSON_HEDLEY_WARNING)\n    #undef JSON_HEDLEY_WARNING\n#endif\n#if JSON_HEDLEY_HAS_WARNING(\"-Wunknown-pragmas\")\n#  define JSON_HEDLEY_WARNING(msg) \\\n    JSON_HEDLEY_DIAGNOSTIC_PUSH \\\n    JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS \\\n    JSON_HEDLEY_PRAGMA(clang warning msg) \\\n    JSON_HEDLEY_DIAGNOSTIC_POP\n#elif \\\n  JSON_HEDLEY_GCC_VERSION_CHECK(4,8,0) || \\\n  JSON_HEDLEY_PGI_VERSION_CHECK(18,4,0) || \\\n  JSON_HEDLEY_INTEL_VERSION_CHECK(13,0,0)\n#  define JSON_HEDLEY_WARNING(msg) JSON_HEDLEY_PRAGMA(GCC warning msg)\n#elif \\\n  JSON_HEDLEY_MSVC_VERSION_CHECK(15,0,0) || \\\n  JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n#  define JSON_HEDLEY_WARNING(msg) JSON_HEDLEY_PRAGMA(message(msg))\n#else\n#  define JSON_HEDLEY_WARNING(msg) JSON_HEDLEY_MESSAGE(msg)\n#endif\n\n#if defined(JSON_HEDLEY_REQUIRE)\n    #undef JSON_HEDLEY_REQUIRE\n#endif\n#if defined(JSON_HEDLEY_REQUIRE_MSG)\n    #undef JSON_HEDLEY_REQUIRE_MSG\n#endif\n#if JSON_HEDLEY_HAS_ATTRIBUTE(diagnose_if)\n#  if JSON_HEDLEY_HAS_WARNING(\"-Wgcc-compat\")\n#    define JSON_HEDLEY_REQUIRE(expr) \\\n    JSON_HEDLEY_DIAGNOSTIC_PUSH \\\n    _Pragma(\"clang diagnostic ignored \\\"-Wgcc-compat\\\"\") \\\n    __attribute__((diagnose_if(!(expr), #expr, \"error\"))) \\\n    JSON_HEDLEY_DIAGNOSTIC_POP\n#    define JSON_HEDLEY_REQUIRE_MSG(expr,msg) \\\n    JSON_HEDLEY_DIAGNOSTIC_PUSH \\\n    _Pragma(\"clang diagnostic ignored \\\"-Wgcc-compat\\\"\") \\\n    __attribute__((diagnose_if(!(expr), msg, \"error\"))) \\\n    JSON_HEDLEY_DIAGNOSTIC_POP\n#  else\n#    define JSON_HEDLEY_REQUIRE(expr) __attribute__((diagnose_if(!(expr), #expr, \"error\")))\n#    define JSON_HEDLEY_REQUIRE_MSG(expr,msg) __attribute__((diagnose_if(!(expr), msg, \"error\")))\n#  endif\n#else\n#  define JSON_HEDLEY_REQUIRE(expr)\n#  define JSON_HEDLEY_REQUIRE_MSG(expr,msg)\n#endif\n\n#if defined(JSON_HEDLEY_FLAGS)\n    #undef JSON_HEDLEY_FLAGS\n#endif\n#if JSON_HEDLEY_HAS_ATTRIBUTE(flag_enum) && (!defined(__cplusplus) || JSON_HEDLEY_HAS_WARNING(\"-Wbitfield-enum-conversion\"))\n    #define JSON_HEDLEY_FLAGS __attribute__((__flag_enum__))\n#else\n    #define JSON_HEDLEY_FLAGS\n#endif\n\n#if defined(JSON_HEDLEY_FLAGS_CAST)\n    #undef JSON_HEDLEY_FLAGS_CAST\n#endif\n#if JSON_HEDLEY_INTEL_VERSION_CHECK(19,0,0)\n#  define JSON_HEDLEY_FLAGS_CAST(T, expr) (__extension__ ({ \\\n        JSON_HEDLEY_DIAGNOSTIC_PUSH \\\n        _Pragma(\"warning(disable:188)\") \\\n        ((T) (expr)); \\\n        JSON_HEDLEY_DIAGNOSTIC_POP \\\n    }))\n#else\n#  define JSON_HEDLEY_FLAGS_CAST(T, expr) JSON_HEDLEY_STATIC_CAST(T, expr)\n#endif\n\n#if defined(JSON_HEDLEY_EMPTY_BASES)\n    #undef JSON_HEDLEY_EMPTY_BASES\n#endif\n#if \\\n    (JSON_HEDLEY_MSVC_VERSION_CHECK(19,0,23918) && !JSON_HEDLEY_MSVC_VERSION_CHECK(20,0,0)) || \\\n    JSON_HEDLEY_INTEL_CL_VERSION_CHECK(2021,1,0)\n    #define JSON_HEDLEY_EMPTY_BASES __declspec(empty_bases)\n#else\n    #define JSON_HEDLEY_EMPTY_BASES\n#endif\n\n/* Remaining macros are deprecated. */\n\n#if defined(JSON_HEDLEY_GCC_NOT_CLANG_VERSION_CHECK)\n    #undef JSON_HEDLEY_GCC_NOT_CLANG_VERSION_CHECK\n#endif\n#if defined(__clang__)\n    #define JSON_HEDLEY_GCC_NOT_CLANG_VERSION_CHECK(major,minor,patch) (0)\n#else\n    #define JSON_HEDLEY_GCC_NOT_CLANG_VERSION_CHECK(major,minor,patch) JSON_HEDLEY_GCC_VERSION_CHECK(major,minor,patch)\n#endif\n\n#if defined(JSON_HEDLEY_CLANG_HAS_ATTRIBUTE)\n    #undef JSON_HEDLEY_CLANG_HAS_ATTRIBUTE\n#endif\n#define JSON_HEDLEY_CLANG_HAS_ATTRIBUTE(attribute) JSON_HEDLEY_HAS_ATTRIBUTE(attribute)\n\n#if defined(JSON_HEDLEY_CLANG_HAS_CPP_ATTRIBUTE)\n    #undef JSON_HEDLEY_CLANG_HAS_CPP_ATTRIBUTE\n#endif\n#define JSON_HEDLEY_CLANG_HAS_CPP_ATTRIBUTE(attribute) JSON_HEDLEY_HAS_CPP_ATTRIBUTE(attribute)\n\n#if defined(JSON_HEDLEY_CLANG_HAS_BUILTIN)\n    #undef JSON_HEDLEY_CLANG_HAS_BUILTIN\n#endif\n#define JSON_HEDLEY_CLANG_HAS_BUILTIN(builtin) JSON_HEDLEY_HAS_BUILTIN(builtin)\n\n#if defined(JSON_HEDLEY_CLANG_HAS_FEATURE)\n    #undef JSON_HEDLEY_CLANG_HAS_FEATURE\n#endif\n#define JSON_HEDLEY_CLANG_HAS_FEATURE(feature) JSON_HEDLEY_HAS_FEATURE(feature)\n\n#if defined(JSON_HEDLEY_CLANG_HAS_EXTENSION)\n    #undef JSON_HEDLEY_CLANG_HAS_EXTENSION\n#endif\n#define JSON_HEDLEY_CLANG_HAS_EXTENSION(extension) JSON_HEDLEY_HAS_EXTENSION(extension)\n\n#if defined(JSON_HEDLEY_CLANG_HAS_DECLSPEC_DECLSPEC_ATTRIBUTE)\n    #undef JSON_HEDLEY_CLANG_HAS_DECLSPEC_DECLSPEC_ATTRIBUTE\n#endif\n#define JSON_HEDLEY_CLANG_HAS_DECLSPEC_ATTRIBUTE(attribute) JSON_HEDLEY_HAS_DECLSPEC_ATTRIBUTE(attribute)\n\n#if defined(JSON_HEDLEY_CLANG_HAS_WARNING)\n    #undef JSON_HEDLEY_CLANG_HAS_WARNING\n#endif\n#define JSON_HEDLEY_CLANG_HAS_WARNING(warning) JSON_HEDLEY_HAS_WARNING(warning)\n\n#endif /* !defined(JSON_HEDLEY_VERSION) || (JSON_HEDLEY_VERSION < X) */\n\n\n// This file contains all internal macro definitions (except those affecting ABI)\n// You MUST include macro_unscope.hpp at the end of json.hpp to undef all of them\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n\n// exclude unsupported compilers\n#if !defined(JSON_SKIP_UNSUPPORTED_COMPILER_CHECK)\n    #if defined(__clang__)\n        #if (__clang_major__ * 10000 + __clang_minor__ * 100 + __clang_patchlevel__) < 30400\n            #error \"unsupported Clang version - see https://github.com/nlohmann/json#supported-compilers\"\n        #endif\n    #elif defined(__GNUC__) && !(defined(__ICC) || defined(__INTEL_COMPILER))\n        #if (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) < 40800\n            #error \"unsupported GCC version - see https://github.com/nlohmann/json#supported-compilers\"\n        #endif\n    #endif\n#endif\n\n// C++ language standard detection\n// if the user manually specified the used c++ version this is skipped\n#if !defined(JSON_HAS_CPP_20) && !defined(JSON_HAS_CPP_17) && !defined(JSON_HAS_CPP_14) && !defined(JSON_HAS_CPP_11)\n    #if (defined(__cplusplus) && __cplusplus >= 202002L) || (defined(_MSVC_LANG) && _MSVC_LANG >= 202002L)\n        #define JSON_HAS_CPP_20\n        #define JSON_HAS_CPP_17\n        #define JSON_HAS_CPP_14\n    #elif (defined(__cplusplus) && __cplusplus >= 201703L) || (defined(_HAS_CXX17) && _HAS_CXX17 == 1) // fix for issue #464\n        #define JSON_HAS_CPP_17\n        #define JSON_HAS_CPP_14\n    #elif (defined(__cplusplus) && __cplusplus >= 201402L) || (defined(_HAS_CXX14) && _HAS_CXX14 == 1)\n        #define JSON_HAS_CPP_14\n    #endif\n    // the cpp 11 flag is always specified because it is the minimal required version\n    #define JSON_HAS_CPP_11\n#endif\n\n#ifdef __has_include\n    #if __has_include(<version>)\n        #include <version>\n    #endif\n#endif\n\n#if !defined(JSON_HAS_FILESYSTEM) && !defined(JSON_HAS_EXPERIMENTAL_FILESYSTEM)\n    #ifdef JSON_HAS_CPP_17\n        #if defined(__cpp_lib_filesystem)\n            #define JSON_HAS_FILESYSTEM 1\n        #elif defined(__cpp_lib_experimental_filesystem)\n            #define JSON_HAS_EXPERIMENTAL_FILESYSTEM 1\n        #elif !defined(__has_include)\n            #define JSON_HAS_EXPERIMENTAL_FILESYSTEM 1\n        #elif __has_include(<filesystem>)\n            #define JSON_HAS_FILESYSTEM 1\n        #elif __has_include(<experimental/filesystem>)\n            #define JSON_HAS_EXPERIMENTAL_FILESYSTEM 1\n        #endif\n\n        // std::filesystem does not work on MinGW GCC 8: https://sourceforge.net/p/mingw-w64/bugs/737/\n        #if defined(__MINGW32__) && defined(__GNUC__) && __GNUC__ == 8\n            #undef JSON_HAS_FILESYSTEM\n            #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM\n        #endif\n\n        // no filesystem support before GCC 8: https://en.cppreference.com/w/cpp/compiler_support\n        #if defined(__GNUC__) && !defined(__clang__) && __GNUC__ < 8\n            #undef JSON_HAS_FILESYSTEM\n            #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM\n        #endif\n\n        // no filesystem support before Clang 7: https://en.cppreference.com/w/cpp/compiler_support\n        #if defined(__clang_major__) && __clang_major__ < 7\n            #undef JSON_HAS_FILESYSTEM\n            #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM\n        #endif\n\n        // no filesystem support before MSVC 19.14: https://en.cppreference.com/w/cpp/compiler_support\n        #if defined(_MSC_VER) && _MSC_VER < 1914\n            #undef JSON_HAS_FILESYSTEM\n            #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM\n        #endif\n\n        // no filesystem support before iOS 13\n        #if defined(__IPHONE_OS_VERSION_MIN_REQUIRED) && __IPHONE_OS_VERSION_MIN_REQUIRED < 130000\n            #undef JSON_HAS_FILESYSTEM\n            #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM\n        #endif\n\n        // no filesystem support before macOS Catalina\n        #if defined(__MAC_OS_X_VERSION_MIN_REQUIRED) && __MAC_OS_X_VERSION_MIN_REQUIRED < 101500\n            #undef JSON_HAS_FILESYSTEM\n            #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM\n        #endif\n    #endif\n#endif\n\n#ifndef JSON_HAS_EXPERIMENTAL_FILESYSTEM\n    #define JSON_HAS_EXPERIMENTAL_FILESYSTEM 0\n#endif\n\n#ifndef JSON_HAS_FILESYSTEM\n    #define JSON_HAS_FILESYSTEM 0\n#endif\n\n#ifndef JSON_HAS_THREE_WAY_COMPARISON\n    #if defined(__cpp_impl_three_way_comparison) && __cpp_impl_three_way_comparison >= 201907L \\\n        && defined(__cpp_lib_three_way_comparison) && __cpp_lib_three_way_comparison >= 201907L\n        #define JSON_HAS_THREE_WAY_COMPARISON 1\n    #else\n        #define JSON_HAS_THREE_WAY_COMPARISON 0\n    #endif\n#endif\n\n#ifndef JSON_HAS_RANGES\n    // ranges header shipping in GCC 11.1.0 (released 2021-04-27) has syntax error\n    #if defined(__GLIBCXX__) && __GLIBCXX__ == 20210427\n        #define JSON_HAS_RANGES 0\n    #elif defined(__cpp_lib_ranges)\n        #define JSON_HAS_RANGES 1\n    #else\n        #define JSON_HAS_RANGES 0\n    #endif\n#endif\n\n#ifdef JSON_HAS_CPP_17\n    #define JSON_INLINE_VARIABLE inline\n#else\n    #define JSON_INLINE_VARIABLE\n#endif\n\n#if JSON_HEDLEY_HAS_ATTRIBUTE(no_unique_address)\n    #define JSON_NO_UNIQUE_ADDRESS [[no_unique_address]]\n#else\n    #define JSON_NO_UNIQUE_ADDRESS\n#endif\n\n// disable documentation warnings on clang\n#if defined(__clang__)\n    #pragma clang diagnostic push\n    #pragma clang diagnostic ignored \"-Wdocumentation\"\n    #pragma clang diagnostic ignored \"-Wdocumentation-unknown-command\"\n#endif\n\n// allow disabling exceptions\n#if (defined(__cpp_exceptions) || defined(__EXCEPTIONS) || defined(_CPPUNWIND)) && !defined(JSON_NOEXCEPTION)\n    #define JSON_THROW(exception) throw exception\n    #define JSON_TRY try\n    #define JSON_CATCH(exception) catch(exception)\n    #define JSON_INTERNAL_CATCH(exception) catch(exception)\n#else\n    #include <cstdlib>\n    #define JSON_THROW(exception) std::abort()\n    #define JSON_TRY if(true)\n    #define JSON_CATCH(exception) if(false)\n    #define JSON_INTERNAL_CATCH(exception) if(false)\n#endif\n\n// override exception macros\n#if defined(JSON_THROW_USER)\n    #undef JSON_THROW\n    #define JSON_THROW JSON_THROW_USER\n#endif\n#if defined(JSON_TRY_USER)\n    #undef JSON_TRY\n    #define JSON_TRY JSON_TRY_USER\n#endif\n#if defined(JSON_CATCH_USER)\n    #undef JSON_CATCH\n    #define JSON_CATCH JSON_CATCH_USER\n    #undef JSON_INTERNAL_CATCH\n    #define JSON_INTERNAL_CATCH JSON_CATCH_USER\n#endif\n#if defined(JSON_INTERNAL_CATCH_USER)\n    #undef JSON_INTERNAL_CATCH\n    #define JSON_INTERNAL_CATCH JSON_INTERNAL_CATCH_USER\n#endif\n\n// allow overriding assert\n#if !defined(JSON_ASSERT)\n    #include <cassert> // assert\n    #define JSON_ASSERT(x) assert(x)\n#endif\n\n// allow to access some private functions (needed by the test suite)\n#if defined(JSON_TESTS_PRIVATE)\n    #define JSON_PRIVATE_UNLESS_TESTED public\n#else\n    #define JSON_PRIVATE_UNLESS_TESTED private\n#endif\n\n/*!\n@brief macro to briefly define a mapping between an enum and JSON\n@def NLOHMANN_JSON_SERIALIZE_ENUM\n@since version 3.4.0\n*/\n#define NLOHMANN_JSON_SERIALIZE_ENUM(ENUM_TYPE, ...)                                            \\\n    template<typename BasicJsonType>                                                            \\\n    inline void to_json(BasicJsonType& j, const ENUM_TYPE& e)                                   \\\n    {                                                                                           \\\n        static_assert(std::is_enum<ENUM_TYPE>::value, #ENUM_TYPE \" must be an enum!\");          \\\n        static const std::pair<ENUM_TYPE, BasicJsonType> m[] = __VA_ARGS__;                     \\\n        auto it = std::find_if(std::begin(m), std::end(m),                                      \\\n                               [e](const std::pair<ENUM_TYPE, BasicJsonType>& ej_pair) -> bool  \\\n        {                                                                                       \\\n            return ej_pair.first == e;                                                          \\\n        });                                                                                     \\\n        j = ((it != std::end(m)) ? it : std::begin(m))->second;                                 \\\n    }                                                                                           \\\n    template<typename BasicJsonType>                                                            \\\n    inline void from_json(const BasicJsonType& j, ENUM_TYPE& e)                                 \\\n    {                                                                                           \\\n        static_assert(std::is_enum<ENUM_TYPE>::value, #ENUM_TYPE \" must be an enum!\");          \\\n        static const std::pair<ENUM_TYPE, BasicJsonType> m[] = __VA_ARGS__;                     \\\n        auto it = std::find_if(std::begin(m), std::end(m),                                      \\\n                               [&j](const std::pair<ENUM_TYPE, BasicJsonType>& ej_pair) -> bool \\\n        {                                                                                       \\\n            return ej_pair.second == j;                                                         \\\n        });                                                                                     \\\n        e = ((it != std::end(m)) ? it : std::begin(m))->first;                                  \\\n    }\n\n// Ugly macros to avoid uglier copy-paste when specializing basic_json. They\n// may be removed in the future once the class is split.\n\n#define NLOHMANN_BASIC_JSON_TPL_DECLARATION                                \\\n    template<template<typename, typename, typename...> class ObjectType,   \\\n             template<typename, typename...> class ArrayType,              \\\n             class StringType, class BooleanType, class NumberIntegerType, \\\n             class NumberUnsignedType, class NumberFloatType,              \\\n             template<typename> class AllocatorType,                       \\\n             template<typename, typename = void> class JSONSerializer,     \\\n             class BinaryType>\n\n#define NLOHMANN_BASIC_JSON_TPL                                            \\\n    basic_json<ObjectType, ArrayType, StringType, BooleanType,             \\\n    NumberIntegerType, NumberUnsignedType, NumberFloatType,                \\\n    AllocatorType, JSONSerializer, BinaryType>\n\n// Macros to simplify conversion from/to types\n\n#define NLOHMANN_JSON_EXPAND( x ) x\n#define NLOHMANN_JSON_GET_MACRO(_1, _2, _3, _4, _5, _6, _7, _8, _9, _10, _11, _12, _13, _14, _15, _16, _17, _18, _19, _20, _21, _22, _23, _24, _25, _26, _27, _28, _29, _30, _31, _32, _33, _34, _35, _36, _37, _38, _39, _40, _41, _42, _43, _44, _45, _46, _47, _48, _49, _50, _51, _52, _53, _54, _55, _56, _57, _58, _59, _60, _61, _62, _63, _64, NAME,...) NAME\n#define NLOHMANN_JSON_PASTE(...) NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_GET_MACRO(__VA_ARGS__, \\\n        NLOHMANN_JSON_PASTE64, \\\n        NLOHMANN_JSON_PASTE63, \\\n        NLOHMANN_JSON_PASTE62, \\\n        NLOHMANN_JSON_PASTE61, \\\n        NLOHMANN_JSON_PASTE60, \\\n        NLOHMANN_JSON_PASTE59, \\\n        NLOHMANN_JSON_PASTE58, \\\n        NLOHMANN_JSON_PASTE57, \\\n        NLOHMANN_JSON_PASTE56, \\\n        NLOHMANN_JSON_PASTE55, \\\n        NLOHMANN_JSON_PASTE54, \\\n        NLOHMANN_JSON_PASTE53, \\\n        NLOHMANN_JSON_PASTE52, \\\n        NLOHMANN_JSON_PASTE51, \\\n        NLOHMANN_JSON_PASTE50, \\\n        NLOHMANN_JSON_PASTE49, \\\n        NLOHMANN_JSON_PASTE48, \\\n        NLOHMANN_JSON_PASTE47, \\\n        NLOHMANN_JSON_PASTE46, \\\n        NLOHMANN_JSON_PASTE45, \\\n        NLOHMANN_JSON_PASTE44, \\\n        NLOHMANN_JSON_PASTE43, \\\n        NLOHMANN_JSON_PASTE42, \\\n        NLOHMANN_JSON_PASTE41, \\\n        NLOHMANN_JSON_PASTE40, \\\n        NLOHMANN_JSON_PASTE39, \\\n        NLOHMANN_JSON_PASTE38, \\\n        NLOHMANN_JSON_PASTE37, \\\n        NLOHMANN_JSON_PASTE36, \\\n        NLOHMANN_JSON_PASTE35, \\\n        NLOHMANN_JSON_PASTE34, \\\n        NLOHMANN_JSON_PASTE33, \\\n        NLOHMANN_JSON_PASTE32, \\\n        NLOHMANN_JSON_PASTE31, \\\n        NLOHMANN_JSON_PASTE30, \\\n        NLOHMANN_JSON_PASTE29, \\\n        NLOHMANN_JSON_PASTE28, \\\n        NLOHMANN_JSON_PASTE27, \\\n        NLOHMANN_JSON_PASTE26, \\\n        NLOHMANN_JSON_PASTE25, \\\n        NLOHMANN_JSON_PASTE24, \\\n        NLOHMANN_JSON_PASTE23, \\\n        NLOHMANN_JSON_PASTE22, \\\n        NLOHMANN_JSON_PASTE21, \\\n        NLOHMANN_JSON_PASTE20, \\\n        NLOHMANN_JSON_PASTE19, \\\n        NLOHMANN_JSON_PASTE18, \\\n        NLOHMANN_JSON_PASTE17, \\\n        NLOHMANN_JSON_PASTE16, \\\n        NLOHMANN_JSON_PASTE15, \\\n        NLOHMANN_JSON_PASTE14, \\\n        NLOHMANN_JSON_PASTE13, \\\n        NLOHMANN_JSON_PASTE12, \\\n        NLOHMANN_JSON_PASTE11, \\\n        NLOHMANN_JSON_PASTE10, \\\n        NLOHMANN_JSON_PASTE9, \\\n        NLOHMANN_JSON_PASTE8, \\\n        NLOHMANN_JSON_PASTE7, \\\n        NLOHMANN_JSON_PASTE6, \\\n        NLOHMANN_JSON_PASTE5, \\\n        NLOHMANN_JSON_PASTE4, \\\n        NLOHMANN_JSON_PASTE3, \\\n        NLOHMANN_JSON_PASTE2, \\\n        NLOHMANN_JSON_PASTE1)(__VA_ARGS__))\n#define NLOHMANN_JSON_PASTE2(func, v1) func(v1)\n#define NLOHMANN_JSON_PASTE3(func, v1, v2) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE2(func, v2)\n#define NLOHMANN_JSON_PASTE4(func, v1, v2, v3) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE3(func, v2, v3)\n#define NLOHMANN_JSON_PASTE5(func, v1, v2, v3, v4) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE4(func, v2, v3, v4)\n#define NLOHMANN_JSON_PASTE6(func, v1, v2, v3, v4, v5) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE5(func, v2, v3, v4, v5)\n#define NLOHMANN_JSON_PASTE7(func, v1, v2, v3, v4, v5, v6) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE6(func, v2, v3, v4, v5, v6)\n#define NLOHMANN_JSON_PASTE8(func, v1, v2, v3, v4, v5, v6, v7) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE7(func, v2, v3, v4, v5, v6, v7)\n#define NLOHMANN_JSON_PASTE9(func, v1, v2, v3, v4, v5, v6, v7, v8) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE8(func, v2, v3, v4, v5, v6, v7, v8)\n#define NLOHMANN_JSON_PASTE10(func, v1, v2, v3, v4, v5, v6, v7, v8, v9) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE9(func, v2, v3, v4, v5, v6, v7, v8, v9)\n#define NLOHMANN_JSON_PASTE11(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE10(func, v2, v3, v4, v5, v6, v7, v8, v9, v10)\n#define NLOHMANN_JSON_PASTE12(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE11(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11)\n#define NLOHMANN_JSON_PASTE13(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE12(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12)\n#define NLOHMANN_JSON_PASTE14(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE13(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13)\n#define NLOHMANN_JSON_PASTE15(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE14(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14)\n#define NLOHMANN_JSON_PASTE16(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE15(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15)\n#define NLOHMANN_JSON_PASTE17(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE16(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16)\n#define NLOHMANN_JSON_PASTE18(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE17(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17)\n#define NLOHMANN_JSON_PASTE19(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE18(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18)\n#define NLOHMANN_JSON_PASTE20(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE19(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19)\n#define NLOHMANN_JSON_PASTE21(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE20(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20)\n#define NLOHMANN_JSON_PASTE22(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE21(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21)\n#define NLOHMANN_JSON_PASTE23(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE22(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22)\n#define NLOHMANN_JSON_PASTE24(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE23(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23)\n#define NLOHMANN_JSON_PASTE25(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE24(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24)\n#define NLOHMANN_JSON_PASTE26(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE25(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25)\n#define NLOHMANN_JSON_PASTE27(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE26(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26)\n#define NLOHMANN_JSON_PASTE28(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE27(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27)\n#define NLOHMANN_JSON_PASTE29(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE28(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28)\n#define NLOHMANN_JSON_PASTE30(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE29(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29)\n#define NLOHMANN_JSON_PASTE31(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE30(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30)\n#define NLOHMANN_JSON_PASTE32(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE31(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31)\n#define NLOHMANN_JSON_PASTE33(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE32(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32)\n#define NLOHMANN_JSON_PASTE34(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE33(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33)\n#define NLOHMANN_JSON_PASTE35(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE34(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34)\n#define NLOHMANN_JSON_PASTE36(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE35(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35)\n#define NLOHMANN_JSON_PASTE37(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE36(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36)\n#define NLOHMANN_JSON_PASTE38(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE37(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37)\n#define NLOHMANN_JSON_PASTE39(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE38(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38)\n#define NLOHMANN_JSON_PASTE40(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE39(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39)\n#define NLOHMANN_JSON_PASTE41(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE40(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40)\n#define NLOHMANN_JSON_PASTE42(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE41(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41)\n#define NLOHMANN_JSON_PASTE43(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE42(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42)\n#define NLOHMANN_JSON_PASTE44(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE43(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43)\n#define NLOHMANN_JSON_PASTE45(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE44(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44)\n#define NLOHMANN_JSON_PASTE46(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE45(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45)\n#define NLOHMANN_JSON_PASTE47(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE46(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46)\n#define NLOHMANN_JSON_PASTE48(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE47(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47)\n#define NLOHMANN_JSON_PASTE49(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE48(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48)\n#define NLOHMANN_JSON_PASTE50(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE49(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49)\n#define NLOHMANN_JSON_PASTE51(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE50(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50)\n#define NLOHMANN_JSON_PASTE52(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE51(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51)\n#define NLOHMANN_JSON_PASTE53(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE52(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52)\n#define NLOHMANN_JSON_PASTE54(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE53(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53)\n#define NLOHMANN_JSON_PASTE55(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE54(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54)\n#define NLOHMANN_JSON_PASTE56(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE55(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55)\n#define NLOHMANN_JSON_PASTE57(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE56(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56)\n#define NLOHMANN_JSON_PASTE58(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE57(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57)\n#define NLOHMANN_JSON_PASTE59(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE58(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58)\n#define NLOHMANN_JSON_PASTE60(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE59(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59)\n#define NLOHMANN_JSON_PASTE61(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE60(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60)\n#define NLOHMANN_JSON_PASTE62(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE61(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61)\n#define NLOHMANN_JSON_PASTE63(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE62(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62)\n#define NLOHMANN_JSON_PASTE64(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62, v63) NLOHMANN_JSON_PASTE2(func, v1) NLOHMANN_JSON_PASTE63(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62, v63)\n\n#define NLOHMANN_JSON_TO(v1) nlohmann_json_j[#v1] = nlohmann_json_t.v1;\n#define NLOHMANN_JSON_FROM(v1) nlohmann_json_j.at(#v1).get_to(nlohmann_json_t.v1);\n#define NLOHMANN_JSON_FROM_WITH_DEFAULT(v1) nlohmann_json_t.v1 = nlohmann_json_j.value(#v1, nlohmann_json_default_obj.v1);\n\n/*!\n@brief macro\n@def NLOHMANN_DEFINE_TYPE_INTRUSIVE\n@since version 3.9.0\n*/\n#define NLOHMANN_DEFINE_TYPE_INTRUSIVE(Type, ...)  \\\n    friend void to_json(nlohmann::json& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \\\n    friend void from_json(const nlohmann::json& nlohmann_json_j, Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM, __VA_ARGS__)) }\n\n#define NLOHMANN_DEFINE_TYPE_INTRUSIVE_WITH_DEFAULT(Type, ...)  \\\n    friend void to_json(nlohmann::json& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \\\n    friend void from_json(const nlohmann::json& nlohmann_json_j, Type& nlohmann_json_t) { Type nlohmann_json_default_obj; NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM_WITH_DEFAULT, __VA_ARGS__)) }\n\n/*!\n@brief macro\n@def NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE\n@since version 3.9.0\n*/\n#define NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(Type, ...)  \\\n    inline void to_json(nlohmann::json& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \\\n    inline void from_json(const nlohmann::json& nlohmann_json_j, Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM, __VA_ARGS__)) }\n\n#define NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE_WITH_DEFAULT(Type, ...)  \\\n    inline void to_json(nlohmann::json& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \\\n    inline void from_json(const nlohmann::json& nlohmann_json_j, Type& nlohmann_json_t) { Type nlohmann_json_default_obj; NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM_WITH_DEFAULT, __VA_ARGS__)) }\n\n\n// inspired from https://stackoverflow.com/a/26745591\n// allows to call any std function as if (e.g. with begin):\n// using std::begin; begin(x);\n//\n// it allows using the detected idiom to retrieve the return type\n// of such an expression\n#define NLOHMANN_CAN_CALL_STD_FUNC_IMPL(std_name)                                 \\\n    namespace detail {                                                            \\\n    using std::std_name;                                                          \\\n    \\\n    template<typename... T>                                                       \\\n    using result_of_##std_name = decltype(std_name(std::declval<T>()...));        \\\n    }                                                                             \\\n    \\\n    namespace detail2 {                                                           \\\n    struct std_name##_tag                                                         \\\n    {                                                                             \\\n    };                                                                            \\\n    \\\n    template<typename... T>                                                       \\\n    std_name##_tag std_name(T&&...);                                              \\\n    \\\n    template<typename... T>                                                       \\\n    using result_of_##std_name = decltype(std_name(std::declval<T>()...));        \\\n    \\\n    template<typename... T>                                                       \\\n    struct would_call_std_##std_name                                              \\\n    {                                                                             \\\n        static constexpr auto const value = ::nlohmann::detail::                  \\\n                                            is_detected_exact<std_name##_tag, result_of_##std_name, T...>::value; \\\n    };                                                                            \\\n    } /* namespace detail2 */ \\\n    \\\n    template<typename... T>                                                       \\\n    struct would_call_std_##std_name : detail2::would_call_std_##std_name<T...>   \\\n    {                                                                             \\\n    }\n\n#ifndef JSON_USE_IMPLICIT_CONVERSIONS\n    #define JSON_USE_IMPLICIT_CONVERSIONS 1\n#endif\n\n#if JSON_USE_IMPLICIT_CONVERSIONS\n    #define JSON_EXPLICIT\n#else\n    #define JSON_EXPLICIT explicit\n#endif\n\n#ifndef JSON_DISABLE_ENUM_SERIALIZATION\n    #define JSON_DISABLE_ENUM_SERIALIZATION 0\n#endif\n\n#ifndef JSON_USE_GLOBAL_UDLS\n    #define JSON_USE_GLOBAL_UDLS 1\n#endif\n\n#if JSON_HAS_THREE_WAY_COMPARISON\n    #include <compare> // partial_ordering\n#endif\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n///////////////////////////\n// JSON type enumeration //\n///////////////////////////\n\n/*!\n@brief the JSON type enumeration\n\nThis enumeration collects the different JSON types. It is internally used to\ndistinguish the stored values, and the functions @ref basic_json::is_null(),\n@ref basic_json::is_object(), @ref basic_json::is_array(),\n@ref basic_json::is_string(), @ref basic_json::is_boolean(),\n@ref basic_json::is_number() (with @ref basic_json::is_number_integer(),\n@ref basic_json::is_number_unsigned(), and @ref basic_json::is_number_float()),\n@ref basic_json::is_discarded(), @ref basic_json::is_primitive(), and\n@ref basic_json::is_structured() rely on it.\n\n@note There are three enumeration entries (number_integer, number_unsigned, and\nnumber_float), because the library distinguishes these three types for numbers:\n@ref basic_json::number_unsigned_t is used for unsigned integers,\n@ref basic_json::number_integer_t is used for signed integers, and\n@ref basic_json::number_float_t is used for floating-point numbers or to\napproximate integers which do not fit in the limits of their respective type.\n\n@sa see @ref basic_json::basic_json(const value_t value_type) -- create a JSON\nvalue with the default value for a given type\n\n@since version 1.0.0\n*/\nenum class value_t : std::uint8_t\n{\n    null,             ///< null value\n    object,           ///< object (unordered set of name/value pairs)\n    array,            ///< array (ordered collection of values)\n    string,           ///< string value\n    boolean,          ///< boolean value\n    number_integer,   ///< number value (signed integer)\n    number_unsigned,  ///< number value (unsigned integer)\n    number_float,     ///< number value (floating-point)\n    binary,           ///< binary array (ordered collection of bytes)\n    discarded         ///< discarded by the parser callback function\n};\n\n/*!\n@brief comparison operator for JSON types\n\nReturns an ordering that is similar to Python:\n- order: null < boolean < number < object < array < string < binary\n- furthermore, each type is not smaller than itself\n- discarded values are not comparable\n- binary is represented as a b\"\" string in python and directly comparable to a\n  string; however, making a binary array directly comparable with a string would\n  be surprising behavior in a JSON file.\n\n@since version 1.0.0\n*/\n#if JSON_HAS_THREE_WAY_COMPARISON\n    inline std::partial_ordering operator<=>(const value_t lhs, const value_t rhs) noexcept // *NOPAD*\n#else\n    inline bool operator<(const value_t lhs, const value_t rhs) noexcept\n#endif\n{\n    static constexpr std::array<std::uint8_t, 9> order = {{\n            0 /* null */, 3 /* object */, 4 /* array */, 5 /* string */,\n            1 /* boolean */, 2 /* integer */, 2 /* unsigned */, 2 /* float */,\n            6 /* binary */\n        }\n    };\n\n    const auto l_index = static_cast<std::size_t>(lhs);\n    const auto r_index = static_cast<std::size_t>(rhs);\n#if JSON_HAS_THREE_WAY_COMPARISON\n    if (l_index < order.size() && r_index < order.size())\n    {\n        return order[l_index] <=> order[r_index]; // *NOPAD*\n    }\n    return std::partial_ordering::unordered;\n#else\n    return l_index < order.size() && r_index < order.size() && order[l_index] < order[r_index];\n#endif\n}\n\n// GCC selects the built-in operator< over an operator rewritten from\n// a user-defined spaceship operator\n// Clang, MSVC, and ICC select the rewritten candidate\n// (see GCC bug https://gcc.gnu.org/bugzilla/show_bug.cgi?id=105200)\n#if JSON_HAS_THREE_WAY_COMPARISON && defined(__GNUC__)\ninline bool operator<(const value_t lhs, const value_t rhs) noexcept\n{\n    return std::is_lt(lhs <=> rhs); // *NOPAD*\n}\n#endif\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/string_escape.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n/*!\n@brief replace all occurrences of a substring by another string\n\n@param[in,out] s  the string to manipulate; changed so that all\n               occurrences of @a f are replaced with @a t\n@param[in]     f  the substring to replace with @a t\n@param[in]     t  the string to replace @a f\n\n@pre The search string @a f must not be empty. **This precondition is\nenforced with an assertion.**\n\n@since version 2.0.0\n*/\ntemplate<typename StringType>\ninline void replace_substring(StringType& s, const StringType& f,\n                              const StringType& t)\n{\n    JSON_ASSERT(!f.empty());\n    for (auto pos = s.find(f);                // find first occurrence of f\n            pos != StringType::npos;          // make sure f was found\n            s.replace(pos, f.size(), t),      // replace with t, and\n            pos = s.find(f, pos + t.size()))  // find next occurrence of f\n    {}\n}\n\n/*!\n * @brief string escaping as described in RFC 6901 (Sect. 4)\n * @param[in] s string to escape\n * @return    escaped string\n *\n * Note the order of escaping \"~\" to \"~0\" and \"/\" to \"~1\" is important.\n */\ntemplate<typename StringType>\ninline StringType escape(StringType s)\n{\n    replace_substring(s, StringType{\"~\"}, StringType{\"~0\"});\n    replace_substring(s, StringType{\"/\"}, StringType{\"~1\"});\n    return s;\n}\n\n/*!\n * @brief string unescaping as described in RFC 6901 (Sect. 4)\n * @param[in] s string to unescape\n * @return    unescaped string\n *\n * Note the order of escaping \"~1\" to \"/\" and \"~0\" to \"~\" is important.\n */\ntemplate<typename StringType>\nstatic void unescape(StringType& s)\n{\n    replace_substring(s, StringType{\"~1\"}, StringType{\"/\"});\n    replace_substring(s, StringType{\"~0\"}, StringType{\"~\"});\n}\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/input/position_t.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <cstddef> // size_t\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n/// struct to capture the start position of the current token\nstruct position_t\n{\n    /// the total number of characters read\n    std::size_t chars_read_total = 0;\n    /// the number of characters read in the current line\n    std::size_t chars_read_current_line = 0;\n    /// the number of lines read\n    std::size_t lines_read = 0;\n\n    /// conversion to size_t to preserve SAX interface\n    constexpr operator size_t() const\n    {\n        return chars_read_total;\n    }\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/meta/cpp_future.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-FileCopyrightText: 2018 The Abseil Authors\n// SPDX-License-Identifier: MIT\n\n\n\n#include <array> // array\n#include <cstddef> // size_t\n#include <type_traits> // conditional, enable_if, false_type, integral_constant, is_constructible, is_integral, is_same, remove_cv, remove_reference, true_type\n#include <utility> // index_sequence, make_index_sequence, index_sequence_for\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\ntemplate<typename T>\nusing uncvref_t = typename std::remove_cv<typename std::remove_reference<T>::type>::type;\n\n#ifdef JSON_HAS_CPP_14\n\n// the following utilities are natively available in C++14\nusing std::enable_if_t;\nusing std::index_sequence;\nusing std::make_index_sequence;\nusing std::index_sequence_for;\n\n#else\n\n// alias templates to reduce boilerplate\ntemplate<bool B, typename T = void>\nusing enable_if_t = typename std::enable_if<B, T>::type;\n\n// The following code is taken from https://github.com/abseil/abseil-cpp/blob/10cb35e459f5ecca5b2ff107635da0bfa41011b4/absl/utility/utility.h\n// which is part of Google Abseil (https://github.com/abseil/abseil-cpp), licensed under the Apache License 2.0.\n\n//// START OF CODE FROM GOOGLE ABSEIL\n\n// integer_sequence\n//\n// Class template representing a compile-time integer sequence. An instantiation\n// of `integer_sequence<T, Ints...>` has a sequence of integers encoded in its\n// type through its template arguments (which is a common need when\n// working with C++11 variadic templates). `absl::integer_sequence` is designed\n// to be a drop-in replacement for C++14's `std::integer_sequence`.\n//\n// Example:\n//\n//   template< class T, T... Ints >\n//   void user_function(integer_sequence<T, Ints...>);\n//\n//   int main()\n//   {\n//     // user_function's `T` will be deduced to `int` and `Ints...`\n//     // will be deduced to `0, 1, 2, 3, 4`.\n//     user_function(make_integer_sequence<int, 5>());\n//   }\ntemplate <typename T, T... Ints>\nstruct integer_sequence\n{\n    using value_type = T;\n    static constexpr std::size_t size() noexcept\n    {\n        return sizeof...(Ints);\n    }\n};\n\n// index_sequence\n//\n// A helper template for an `integer_sequence` of `size_t`,\n// `absl::index_sequence` is designed to be a drop-in replacement for C++14's\n// `std::index_sequence`.\ntemplate <size_t... Ints>\nusing index_sequence = integer_sequence<size_t, Ints...>;\n\nnamespace utility_internal\n{\n\ntemplate <typename Seq, size_t SeqSize, size_t Rem>\nstruct Extend;\n\n// Note that SeqSize == sizeof...(Ints). It's passed explicitly for efficiency.\ntemplate <typename T, T... Ints, size_t SeqSize>\nstruct Extend<integer_sequence<T, Ints...>, SeqSize, 0>\n{\n    using type = integer_sequence < T, Ints..., (Ints + SeqSize)... >;\n};\n\ntemplate <typename T, T... Ints, size_t SeqSize>\nstruct Extend<integer_sequence<T, Ints...>, SeqSize, 1>\n{\n    using type = integer_sequence < T, Ints..., (Ints + SeqSize)..., 2 * SeqSize >;\n};\n\n// Recursion helper for 'make_integer_sequence<T, N>'.\n// 'Gen<T, N>::type' is an alias for 'integer_sequence<T, 0, 1, ... N-1>'.\ntemplate <typename T, size_t N>\nstruct Gen\n{\n    using type =\n        typename Extend < typename Gen < T, N / 2 >::type, N / 2, N % 2 >::type;\n};\n\ntemplate <typename T>\nstruct Gen<T, 0>\n{\n    using type = integer_sequence<T>;\n};\n\n}  // namespace utility_internal\n\n// Compile-time sequences of integers\n\n// make_integer_sequence\n//\n// This template alias is equivalent to\n// `integer_sequence<int, 0, 1, ..., N-1>`, and is designed to be a drop-in\n// replacement for C++14's `std::make_integer_sequence`.\ntemplate <typename T, T N>\nusing make_integer_sequence = typename utility_internal::Gen<T, N>::type;\n\n// make_index_sequence\n//\n// This template alias is equivalent to `index_sequence<0, 1, ..., N-1>`,\n// and is designed to be a drop-in replacement for C++14's\n// `std::make_index_sequence`.\ntemplate <size_t N>\nusing make_index_sequence = make_integer_sequence<size_t, N>;\n\n// index_sequence_for\n//\n// Converts a typename pack into an index sequence of the same length, and\n// is designed to be a drop-in replacement for C++14's\n// `std::index_sequence_for()`\ntemplate <typename... Ts>\nusing index_sequence_for = make_index_sequence<sizeof...(Ts)>;\n\n//// END OF CODE FROM GOOGLE ABSEIL\n\n#endif\n\n// dispatch utility (taken from ranges-v3)\ntemplate<unsigned N> struct priority_tag : priority_tag < N - 1 > {};\ntemplate<> struct priority_tag<0> {};\n\n// taken from ranges-v3\ntemplate<typename T>\nstruct static_const\n{\n    static JSON_INLINE_VARIABLE constexpr T value{};\n};\n\n#ifndef JSON_HAS_CPP_17\n    template<typename T>\n    constexpr T static_const<T>::value;\n#endif\n\ntemplate<typename T, typename... Args>\ninline constexpr std::array<T, sizeof...(Args)> make_array(Args&& ... args)\n{\n    return std::array<T, sizeof...(Args)> {{static_cast<T>(std::forward<Args>(args))...}};\n}\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/meta/type_traits.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <limits> // numeric_limits\n#include <type_traits> // false_type, is_constructible, is_integral, is_same, true_type\n#include <utility> // declval\n#include <tuple> // tuple\n\n// #include <nlohmann/detail/iterators/iterator_traits.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <iterator> // random_access_iterator_tag\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n// #include <nlohmann/detail/meta/void_t.hpp>\n\n// #include <nlohmann/detail/meta/cpp_future.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\ntemplate<typename It, typename = void>\nstruct iterator_types {};\n\ntemplate<typename It>\nstruct iterator_types <\n    It,\n    void_t<typename It::difference_type, typename It::value_type, typename It::pointer,\n    typename It::reference, typename It::iterator_category >>\n{\n    using difference_type = typename It::difference_type;\n    using value_type = typename It::value_type;\n    using pointer = typename It::pointer;\n    using reference = typename It::reference;\n    using iterator_category = typename It::iterator_category;\n};\n\n// This is required as some compilers implement std::iterator_traits in a way that\n// doesn't work with SFINAE. See https://github.com/nlohmann/json/issues/1341.\ntemplate<typename T, typename = void>\nstruct iterator_traits\n{\n};\n\ntemplate<typename T>\nstruct iterator_traits < T, enable_if_t < !std::is_pointer<T>::value >>\n            : iterator_types<T>\n{\n};\n\ntemplate<typename T>\nstruct iterator_traits<T*, enable_if_t<std::is_object<T>::value>>\n{\n    using iterator_category = std::random_access_iterator_tag;\n    using value_type = T;\n    using difference_type = ptrdiff_t;\n    using pointer = T*;\n    using reference = T&;\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/meta/call_std/begin.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\n\nNLOHMANN_CAN_CALL_STD_FUNC_IMPL(begin);\n\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/meta/call_std/end.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\n\nNLOHMANN_CAN_CALL_STD_FUNC_IMPL(end);\n\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/meta/cpp_future.hpp>\n\n// #include <nlohmann/detail/meta/detected.hpp>\n\n// #include <nlohmann/json_fwd.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n#ifndef INCLUDE_NLOHMANN_JSON_FWD_HPP_\n    #define INCLUDE_NLOHMANN_JSON_FWD_HPP_\n\n    #include <cstdint> // int64_t, uint64_t\n    #include <map> // map\n    #include <memory> // allocator\n    #include <string> // string\n    #include <vector> // vector\n\n    // #include <nlohmann/detail/abi_macros.hpp>\n\n\n    /*!\n    @brief namespace for Niels Lohmann\n    @see https://github.com/nlohmann\n    @since version 1.0.0\n    */\n    NLOHMANN_JSON_NAMESPACE_BEGIN\n\n    /*!\n    @brief default JSONSerializer template argument\n\n    This serializer ignores the template arguments and uses ADL\n    ([argument-dependent lookup](https://en.cppreference.com/w/cpp/language/adl))\n    for serialization.\n    */\n    template<typename T = void, typename SFINAE = void>\n    struct adl_serializer;\n\n    /// a class to store JSON values\n    /// @sa https://json.nlohmann.me/api/basic_json/\n    template<template<typename U, typename V, typename... Args> class ObjectType =\n    std::map,\n    template<typename U, typename... Args> class ArrayType = std::vector,\n    class StringType = std::string, class BooleanType = bool,\n    class NumberIntegerType = std::int64_t,\n    class NumberUnsignedType = std::uint64_t,\n    class NumberFloatType = double,\n    template<typename U> class AllocatorType = std::allocator,\n    template<typename T, typename SFINAE = void> class JSONSerializer =\n    adl_serializer,\n    class BinaryType = std::vector<std::uint8_t>>\n    class basic_json;\n\n    /// @brief JSON Pointer defines a string syntax for identifying a specific value within a JSON document\n    /// @sa https://json.nlohmann.me/api/json_pointer/\n    template<typename RefStringType>\n    class json_pointer;\n\n    /*!\n    @brief default specialization\n    @sa https://json.nlohmann.me/api/json/\n    */\n    using json = basic_json<>;\n\n    /// @brief a minimal map-like container that preserves insertion order\n    /// @sa https://json.nlohmann.me/api/ordered_map/\n    template<class Key, class T, class IgnoredLess, class Allocator>\n    struct ordered_map;\n\n    /// @brief specialization that maintains the insertion order of object keys\n    /// @sa https://json.nlohmann.me/api/ordered_json/\n    using ordered_json = basic_json<nlohmann::ordered_map>;\n\n    NLOHMANN_JSON_NAMESPACE_END\n\n#endif  // INCLUDE_NLOHMANN_JSON_FWD_HPP_\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\n/*!\n@brief detail namespace with internal helper functions\n\nThis namespace collects functions that should not be exposed,\nimplementations of some @ref basic_json methods, and meta-programming helpers.\n\n@since version 2.1.0\n*/\nnamespace detail\n{\n\n/////////////\n// helpers //\n/////////////\n\n// Note to maintainers:\n//\n// Every trait in this file expects a non CV-qualified type.\n// The only exceptions are in the 'aliases for detected' section\n// (i.e. those of the form: decltype(T::member_function(std::declval<T>())))\n//\n// In this case, T has to be properly CV-qualified to constraint the function arguments\n// (e.g. to_json(BasicJsonType&, const T&))\n\ntemplate<typename> struct is_basic_json : std::false_type {};\n\nNLOHMANN_BASIC_JSON_TPL_DECLARATION\nstruct is_basic_json<NLOHMANN_BASIC_JSON_TPL> : std::true_type {};\n\n// used by exceptions create() member functions\n// true_type for pointer to possibly cv-qualified basic_json or std::nullptr_t\n// false_type otherwise\ntemplate<typename BasicJsonContext>\nstruct is_basic_json_context :\n    std::integral_constant < bool,\n    is_basic_json<typename std::remove_cv<typename std::remove_pointer<BasicJsonContext>::type>::type>::value\n    || std::is_same<BasicJsonContext, std::nullptr_t>::value >\n{};\n\n//////////////////////\n// json_ref helpers //\n//////////////////////\n\ntemplate<typename>\nclass json_ref;\n\ntemplate<typename>\nstruct is_json_ref : std::false_type {};\n\ntemplate<typename T>\nstruct is_json_ref<json_ref<T>> : std::true_type {};\n\n//////////////////////////\n// aliases for detected //\n//////////////////////////\n\ntemplate<typename T>\nusing mapped_type_t = typename T::mapped_type;\n\ntemplate<typename T>\nusing key_type_t = typename T::key_type;\n\ntemplate<typename T>\nusing value_type_t = typename T::value_type;\n\ntemplate<typename T>\nusing difference_type_t = typename T::difference_type;\n\ntemplate<typename T>\nusing pointer_t = typename T::pointer;\n\ntemplate<typename T>\nusing reference_t = typename T::reference;\n\ntemplate<typename T>\nusing iterator_category_t = typename T::iterator_category;\n\ntemplate<typename T, typename... Args>\nusing to_json_function = decltype(T::to_json(std::declval<Args>()...));\n\ntemplate<typename T, typename... Args>\nusing from_json_function = decltype(T::from_json(std::declval<Args>()...));\n\ntemplate<typename T, typename U>\nusing get_template_function = decltype(std::declval<T>().template get<U>());\n\n// trait checking if JSONSerializer<T>::from_json(json const&, udt&) exists\ntemplate<typename BasicJsonType, typename T, typename = void>\nstruct has_from_json : std::false_type {};\n\n// trait checking if j.get<T> is valid\n// use this trait instead of std::is_constructible or std::is_convertible,\n// both rely on, or make use of implicit conversions, and thus fail when T\n// has several constructors/operator= (see https://github.com/nlohmann/json/issues/958)\ntemplate <typename BasicJsonType, typename T>\nstruct is_getable\n{\n    static constexpr bool value = is_detected<get_template_function, const BasicJsonType&, T>::value;\n};\n\ntemplate<typename BasicJsonType, typename T>\nstruct has_from_json < BasicJsonType, T, enable_if_t < !is_basic_json<T>::value >>\n{\n    using serializer = typename BasicJsonType::template json_serializer<T, void>;\n\n    static constexpr bool value =\n        is_detected_exact<void, from_json_function, serializer,\n        const BasicJsonType&, T&>::value;\n};\n\n// This trait checks if JSONSerializer<T>::from_json(json const&) exists\n// this overload is used for non-default-constructible user-defined-types\ntemplate<typename BasicJsonType, typename T, typename = void>\nstruct has_non_default_from_json : std::false_type {};\n\ntemplate<typename BasicJsonType, typename T>\nstruct has_non_default_from_json < BasicJsonType, T, enable_if_t < !is_basic_json<T>::value >>\n{\n    using serializer = typename BasicJsonType::template json_serializer<T, void>;\n\n    static constexpr bool value =\n        is_detected_exact<T, from_json_function, serializer,\n        const BasicJsonType&>::value;\n};\n\n// This trait checks if BasicJsonType::json_serializer<T>::to_json exists\n// Do not evaluate the trait when T is a basic_json type, to avoid template instantiation infinite recursion.\ntemplate<typename BasicJsonType, typename T, typename = void>\nstruct has_to_json : std::false_type {};\n\ntemplate<typename BasicJsonType, typename T>\nstruct has_to_json < BasicJsonType, T, enable_if_t < !is_basic_json<T>::value >>\n{\n    using serializer = typename BasicJsonType::template json_serializer<T, void>;\n\n    static constexpr bool value =\n        is_detected_exact<void, to_json_function, serializer, BasicJsonType&,\n        T>::value;\n};\n\ntemplate<typename T>\nusing detect_key_compare = typename T::key_compare;\n\ntemplate<typename T>\nstruct has_key_compare : std::integral_constant<bool, is_detected<detect_key_compare, T>::value> {};\n\n// obtains the actual object key comparator\ntemplate<typename BasicJsonType>\nstruct actual_object_comparator\n{\n    using object_t = typename BasicJsonType::object_t;\n    using object_comparator_t = typename BasicJsonType::default_object_comparator_t;\n    using type = typename std::conditional < has_key_compare<object_t>::value,\n          typename object_t::key_compare, object_comparator_t>::type;\n};\n\ntemplate<typename BasicJsonType>\nusing actual_object_comparator_t = typename actual_object_comparator<BasicJsonType>::type;\n\n///////////////////\n// is_ functions //\n///////////////////\n\n// https://en.cppreference.com/w/cpp/types/conjunction\ntemplate<class...> struct conjunction : std::true_type { };\ntemplate<class B> struct conjunction<B> : B { };\ntemplate<class B, class... Bn>\nstruct conjunction<B, Bn...>\n: std::conditional<static_cast<bool>(B::value), conjunction<Bn...>, B>::type {};\n\n// https://en.cppreference.com/w/cpp/types/negation\ntemplate<class B> struct negation : std::integral_constant < bool, !B::value > { };\n\n// Reimplementation of is_constructible and is_default_constructible, due to them being broken for\n// std::pair and std::tuple until LWG 2367 fix (see https://cplusplus.github.io/LWG/lwg-defects.html#2367).\n// This causes compile errors in e.g. clang 3.5 or gcc 4.9.\ntemplate <typename T>\nstruct is_default_constructible : std::is_default_constructible<T> {};\n\ntemplate <typename T1, typename T2>\nstruct is_default_constructible<std::pair<T1, T2>>\n            : conjunction<is_default_constructible<T1>, is_default_constructible<T2>> {};\n\ntemplate <typename T1, typename T2>\nstruct is_default_constructible<const std::pair<T1, T2>>\n            : conjunction<is_default_constructible<T1>, is_default_constructible<T2>> {};\n\ntemplate <typename... Ts>\nstruct is_default_constructible<std::tuple<Ts...>>\n            : conjunction<is_default_constructible<Ts>...> {};\n\ntemplate <typename... Ts>\nstruct is_default_constructible<const std::tuple<Ts...>>\n            : conjunction<is_default_constructible<Ts>...> {};\n\n\ntemplate <typename T, typename... Args>\nstruct is_constructible : std::is_constructible<T, Args...> {};\n\ntemplate <typename T1, typename T2>\nstruct is_constructible<std::pair<T1, T2>> : is_default_constructible<std::pair<T1, T2>> {};\n\ntemplate <typename T1, typename T2>\nstruct is_constructible<const std::pair<T1, T2>> : is_default_constructible<const std::pair<T1, T2>> {};\n\ntemplate <typename... Ts>\nstruct is_constructible<std::tuple<Ts...>> : is_default_constructible<std::tuple<Ts...>> {};\n\ntemplate <typename... Ts>\nstruct is_constructible<const std::tuple<Ts...>> : is_default_constructible<const std::tuple<Ts...>> {};\n\n\ntemplate<typename T, typename = void>\nstruct is_iterator_traits : std::false_type {};\n\ntemplate<typename T>\nstruct is_iterator_traits<iterator_traits<T>>\n{\n  private:\n    using traits = iterator_traits<T>;\n\n  public:\n    static constexpr auto value =\n        is_detected<value_type_t, traits>::value &&\n        is_detected<difference_type_t, traits>::value &&\n        is_detected<pointer_t, traits>::value &&\n        is_detected<iterator_category_t, traits>::value &&\n        is_detected<reference_t, traits>::value;\n};\n\ntemplate<typename T>\nstruct is_range\n{\n  private:\n    using t_ref = typename std::add_lvalue_reference<T>::type;\n\n    using iterator = detected_t<result_of_begin, t_ref>;\n    using sentinel = detected_t<result_of_end, t_ref>;\n\n    // to be 100% correct, it should use https://en.cppreference.com/w/cpp/iterator/input_or_output_iterator\n    // and https://en.cppreference.com/w/cpp/iterator/sentinel_for\n    // but reimplementing these would be too much work, as a lot of other concepts are used underneath\n    static constexpr auto is_iterator_begin =\n        is_iterator_traits<iterator_traits<iterator>>::value;\n\n  public:\n    static constexpr bool value = !std::is_same<iterator, nonesuch>::value && !std::is_same<sentinel, nonesuch>::value && is_iterator_begin;\n};\n\ntemplate<typename R>\nusing iterator_t = enable_if_t<is_range<R>::value, result_of_begin<decltype(std::declval<R&>())>>;\n\ntemplate<typename T>\nusing range_value_t = value_type_t<iterator_traits<iterator_t<T>>>;\n\n// The following implementation of is_complete_type is taken from\n// https://blogs.msdn.microsoft.com/vcblog/2015/12/02/partial-support-for-expression-sfinae-in-vs-2015-update-1/\n// and is written by Xiang Fan who agreed to using it in this library.\n\ntemplate<typename T, typename = void>\nstruct is_complete_type : std::false_type {};\n\ntemplate<typename T>\nstruct is_complete_type<T, decltype(void(sizeof(T)))> : std::true_type {};\n\ntemplate<typename BasicJsonType, typename CompatibleObjectType,\n         typename = void>\nstruct is_compatible_object_type_impl : std::false_type {};\n\ntemplate<typename BasicJsonType, typename CompatibleObjectType>\nstruct is_compatible_object_type_impl <\n    BasicJsonType, CompatibleObjectType,\n    enable_if_t < is_detected<mapped_type_t, CompatibleObjectType>::value&&\n    is_detected<key_type_t, CompatibleObjectType>::value >>\n{\n    using object_t = typename BasicJsonType::object_t;\n\n    // macOS's is_constructible does not play well with nonesuch...\n    static constexpr bool value =\n        is_constructible<typename object_t::key_type,\n        typename CompatibleObjectType::key_type>::value &&\n        is_constructible<typename object_t::mapped_type,\n        typename CompatibleObjectType::mapped_type>::value;\n};\n\ntemplate<typename BasicJsonType, typename CompatibleObjectType>\nstruct is_compatible_object_type\n    : is_compatible_object_type_impl<BasicJsonType, CompatibleObjectType> {};\n\ntemplate<typename BasicJsonType, typename ConstructibleObjectType,\n         typename = void>\nstruct is_constructible_object_type_impl : std::false_type {};\n\ntemplate<typename BasicJsonType, typename ConstructibleObjectType>\nstruct is_constructible_object_type_impl <\n    BasicJsonType, ConstructibleObjectType,\n    enable_if_t < is_detected<mapped_type_t, ConstructibleObjectType>::value&&\n    is_detected<key_type_t, ConstructibleObjectType>::value >>\n{\n    using object_t = typename BasicJsonType::object_t;\n\n    static constexpr bool value =\n        (is_default_constructible<ConstructibleObjectType>::value &&\n         (std::is_move_assignable<ConstructibleObjectType>::value ||\n          std::is_copy_assignable<ConstructibleObjectType>::value) &&\n         (is_constructible<typename ConstructibleObjectType::key_type,\n          typename object_t::key_type>::value &&\n          std::is_same <\n          typename object_t::mapped_type,\n          typename ConstructibleObjectType::mapped_type >::value)) ||\n        (has_from_json<BasicJsonType,\n         typename ConstructibleObjectType::mapped_type>::value ||\n         has_non_default_from_json <\n         BasicJsonType,\n         typename ConstructibleObjectType::mapped_type >::value);\n};\n\ntemplate<typename BasicJsonType, typename ConstructibleObjectType>\nstruct is_constructible_object_type\n    : is_constructible_object_type_impl<BasicJsonType,\n      ConstructibleObjectType> {};\n\ntemplate<typename BasicJsonType, typename CompatibleStringType>\nstruct is_compatible_string_type\n{\n    static constexpr auto value =\n        is_constructible<typename BasicJsonType::string_t, CompatibleStringType>::value;\n};\n\ntemplate<typename BasicJsonType, typename ConstructibleStringType>\nstruct is_constructible_string_type\n{\n    // launder type through decltype() to fix compilation failure on ICPC\n#ifdef __INTEL_COMPILER\n    using laundered_type = decltype(std::declval<ConstructibleStringType>());\n#else\n    using laundered_type = ConstructibleStringType;\n#endif\n\n    static constexpr auto value =\n        conjunction <\n        is_constructible<laundered_type, typename BasicJsonType::string_t>,\n        is_detected_exact<typename BasicJsonType::string_t::value_type,\n        value_type_t, laundered_type >>::value;\n};\n\ntemplate<typename BasicJsonType, typename CompatibleArrayType, typename = void>\nstruct is_compatible_array_type_impl : std::false_type {};\n\ntemplate<typename BasicJsonType, typename CompatibleArrayType>\nstruct is_compatible_array_type_impl <\n    BasicJsonType, CompatibleArrayType,\n    enable_if_t <\n    is_detected<iterator_t, CompatibleArrayType>::value&&\n    is_iterator_traits<iterator_traits<detected_t<iterator_t, CompatibleArrayType>>>::value&&\n// special case for types like std::filesystem::path whose iterator's value_type are themselves\n// c.f. https://github.com/nlohmann/json/pull/3073\n    !std::is_same<CompatibleArrayType, detected_t<range_value_t, CompatibleArrayType>>::value >>\n{\n    static constexpr bool value =\n        is_constructible<BasicJsonType,\n        range_value_t<CompatibleArrayType>>::value;\n};\n\ntemplate<typename BasicJsonType, typename CompatibleArrayType>\nstruct is_compatible_array_type\n    : is_compatible_array_type_impl<BasicJsonType, CompatibleArrayType> {};\n\ntemplate<typename BasicJsonType, typename ConstructibleArrayType, typename = void>\nstruct is_constructible_array_type_impl : std::false_type {};\n\ntemplate<typename BasicJsonType, typename ConstructibleArrayType>\nstruct is_constructible_array_type_impl <\n    BasicJsonType, ConstructibleArrayType,\n    enable_if_t<std::is_same<ConstructibleArrayType,\n    typename BasicJsonType::value_type>::value >>\n            : std::true_type {};\n\ntemplate<typename BasicJsonType, typename ConstructibleArrayType>\nstruct is_constructible_array_type_impl <\n    BasicJsonType, ConstructibleArrayType,\n    enable_if_t < !std::is_same<ConstructibleArrayType,\n    typename BasicJsonType::value_type>::value&&\n    !is_compatible_string_type<BasicJsonType, ConstructibleArrayType>::value&&\n    is_default_constructible<ConstructibleArrayType>::value&&\n(std::is_move_assignable<ConstructibleArrayType>::value ||\n std::is_copy_assignable<ConstructibleArrayType>::value)&&\nis_detected<iterator_t, ConstructibleArrayType>::value&&\nis_iterator_traits<iterator_traits<detected_t<iterator_t, ConstructibleArrayType>>>::value&&\nis_detected<range_value_t, ConstructibleArrayType>::value&&\n// special case for types like std::filesystem::path whose iterator's value_type are themselves\n// c.f. https://github.com/nlohmann/json/pull/3073\n!std::is_same<ConstructibleArrayType, detected_t<range_value_t, ConstructibleArrayType>>::value&&\n        is_complete_type <\n        detected_t<range_value_t, ConstructibleArrayType >>::value >>\n{\n    using value_type = range_value_t<ConstructibleArrayType>;\n\n    static constexpr bool value =\n        std::is_same<value_type,\n        typename BasicJsonType::array_t::value_type>::value ||\n        has_from_json<BasicJsonType,\n        value_type>::value ||\n        has_non_default_from_json <\n        BasicJsonType,\n        value_type >::value;\n};\n\ntemplate<typename BasicJsonType, typename ConstructibleArrayType>\nstruct is_constructible_array_type\n    : is_constructible_array_type_impl<BasicJsonType, ConstructibleArrayType> {};\n\ntemplate<typename RealIntegerType, typename CompatibleNumberIntegerType,\n         typename = void>\nstruct is_compatible_integer_type_impl : std::false_type {};\n\ntemplate<typename RealIntegerType, typename CompatibleNumberIntegerType>\nstruct is_compatible_integer_type_impl <\n    RealIntegerType, CompatibleNumberIntegerType,\n    enable_if_t < std::is_integral<RealIntegerType>::value&&\n    std::is_integral<CompatibleNumberIntegerType>::value&&\n    !std::is_same<bool, CompatibleNumberIntegerType>::value >>\n{\n    // is there an assert somewhere on overflows?\n    using RealLimits = std::numeric_limits<RealIntegerType>;\n    using CompatibleLimits = std::numeric_limits<CompatibleNumberIntegerType>;\n\n    static constexpr auto value =\n        is_constructible<RealIntegerType,\n        CompatibleNumberIntegerType>::value &&\n        CompatibleLimits::is_integer &&\n        RealLimits::is_signed == CompatibleLimits::is_signed;\n};\n\ntemplate<typename RealIntegerType, typename CompatibleNumberIntegerType>\nstruct is_compatible_integer_type\n    : is_compatible_integer_type_impl<RealIntegerType,\n      CompatibleNumberIntegerType> {};\n\ntemplate<typename BasicJsonType, typename CompatibleType, typename = void>\nstruct is_compatible_type_impl: std::false_type {};\n\ntemplate<typename BasicJsonType, typename CompatibleType>\nstruct is_compatible_type_impl <\n    BasicJsonType, CompatibleType,\n    enable_if_t<is_complete_type<CompatibleType>::value >>\n{\n    static constexpr bool value =\n        has_to_json<BasicJsonType, CompatibleType>::value;\n};\n\ntemplate<typename BasicJsonType, typename CompatibleType>\nstruct is_compatible_type\n    : is_compatible_type_impl<BasicJsonType, CompatibleType> {};\n\ntemplate<typename T1, typename T2>\nstruct is_constructible_tuple : std::false_type {};\n\ntemplate<typename T1, typename... Args>\nstruct is_constructible_tuple<T1, std::tuple<Args...>> : conjunction<is_constructible<T1, Args>...> {};\n\ntemplate<typename BasicJsonType, typename T>\nstruct is_json_iterator_of : std::false_type {};\n\ntemplate<typename BasicJsonType>\nstruct is_json_iterator_of<BasicJsonType, typename BasicJsonType::iterator> : std::true_type {};\n\ntemplate<typename BasicJsonType>\nstruct is_json_iterator_of<BasicJsonType, typename BasicJsonType::const_iterator> : std::true_type\n{};\n\n// checks if a given type T is a template specialization of Primary\ntemplate<template <typename...> class Primary, typename T>\nstruct is_specialization_of : std::false_type {};\n\ntemplate<template <typename...> class Primary, typename... Args>\nstruct is_specialization_of<Primary, Primary<Args...>> : std::true_type {};\n\ntemplate<typename T>\nusing is_json_pointer = is_specialization_of<::nlohmann::json_pointer, uncvref_t<T>>;\n\n// checks if A and B are comparable using Compare functor\ntemplate<typename Compare, typename A, typename B, typename = void>\nstruct is_comparable : std::false_type {};\n\ntemplate<typename Compare, typename A, typename B>\nstruct is_comparable<Compare, A, B, void_t<\ndecltype(std::declval<Compare>()(std::declval<A>(), std::declval<B>())),\ndecltype(std::declval<Compare>()(std::declval<B>(), std::declval<A>()))\n>> : std::true_type {};\n\ntemplate<typename T>\nusing detect_is_transparent = typename T::is_transparent;\n\n// type trait to check if KeyType can be used as object key (without a BasicJsonType)\n// see is_usable_as_basic_json_key_type below\ntemplate<typename Comparator, typename ObjectKeyType, typename KeyTypeCVRef, bool RequireTransparentComparator = true,\n         bool ExcludeObjectKeyType = RequireTransparentComparator, typename KeyType = uncvref_t<KeyTypeCVRef>>\nusing is_usable_as_key_type = typename std::conditional <\n                              is_comparable<Comparator, ObjectKeyType, KeyTypeCVRef>::value\n                              && !(ExcludeObjectKeyType && std::is_same<KeyType,\n                                   ObjectKeyType>::value)\n                              && (!RequireTransparentComparator\n                                  || is_detected <detect_is_transparent, Comparator>::value)\n                              && !is_json_pointer<KeyType>::value,\n                              std::true_type,\n                              std::false_type >::type;\n\n// type trait to check if KeyType can be used as object key\n// true if:\n//   - KeyType is comparable with BasicJsonType::object_t::key_type\n//   - if ExcludeObjectKeyType is true, KeyType is not BasicJsonType::object_t::key_type\n//   - the comparator is transparent or RequireTransparentComparator is false\n//   - KeyType is not a JSON iterator or json_pointer\ntemplate<typename BasicJsonType, typename KeyTypeCVRef, bool RequireTransparentComparator = true,\n         bool ExcludeObjectKeyType = RequireTransparentComparator, typename KeyType = uncvref_t<KeyTypeCVRef>>\nusing is_usable_as_basic_json_key_type = typename std::conditional <\n        is_usable_as_key_type<typename BasicJsonType::object_comparator_t,\n        typename BasicJsonType::object_t::key_type, KeyTypeCVRef,\n        RequireTransparentComparator, ExcludeObjectKeyType>::value\n        && !is_json_iterator_of<BasicJsonType, KeyType>::value,\n        std::true_type,\n        std::false_type >::type;\n\ntemplate<typename ObjectType, typename KeyType>\nusing detect_erase_with_key_type = decltype(std::declval<ObjectType&>().erase(std::declval<KeyType>()));\n\n// type trait to check if object_t has an erase() member functions accepting KeyType\ntemplate<typename BasicJsonType, typename KeyType>\nusing has_erase_with_key_type = typename std::conditional <\n                                is_detected <\n                                detect_erase_with_key_type,\n                                typename BasicJsonType::object_t, KeyType >::value,\n                                std::true_type,\n                                std::false_type >::type;\n\n// a naive helper to check if a type is an ordered_map (exploits the fact that\n// ordered_map inherits capacity() from std::vector)\ntemplate <typename T>\nstruct is_ordered_map\n{\n    using one = char;\n\n    struct two\n    {\n        char x[2]; // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays)\n    };\n\n    template <typename C> static one test( decltype(&C::capacity) ) ;\n    template <typename C> static two test(...);\n\n    enum { value = sizeof(test<T>(nullptr)) == sizeof(char) }; // NOLINT(cppcoreguidelines-pro-type-vararg,hicpp-vararg)\n};\n\n// to avoid useless casts (see https://github.com/nlohmann/json/issues/2893#issuecomment-889152324)\ntemplate < typename T, typename U, enable_if_t < !std::is_same<T, U>::value, int > = 0 >\nT conditional_static_cast(U value)\n{\n    return static_cast<T>(value);\n}\n\ntemplate<typename T, typename U, enable_if_t<std::is_same<T, U>::value, int> = 0>\nT conditional_static_cast(U value)\n{\n    return value;\n}\n\ntemplate<typename... Types>\nusing all_integral = conjunction<std::is_integral<Types>...>;\n\ntemplate<typename... Types>\nusing all_signed = conjunction<std::is_signed<Types>...>;\n\ntemplate<typename... Types>\nusing all_unsigned = conjunction<std::is_unsigned<Types>...>;\n\n// there's a disjunction trait in another PR; replace when merged\ntemplate<typename... Types>\nusing same_sign = std::integral_constant < bool,\n      all_signed<Types...>::value || all_unsigned<Types...>::value >;\n\ntemplate<typename OfType, typename T>\nusing never_out_of_range = std::integral_constant < bool,\n      (std::is_signed<OfType>::value && (sizeof(T) < sizeof(OfType)))\n      || (same_sign<OfType, T>::value && sizeof(OfType) == sizeof(T)) >;\n\ntemplate<typename OfType, typename T,\n         bool OfTypeSigned = std::is_signed<OfType>::value,\n         bool TSigned = std::is_signed<T>::value>\nstruct value_in_range_of_impl2;\n\ntemplate<typename OfType, typename T>\nstruct value_in_range_of_impl2<OfType, T, false, false>\n{\n    static constexpr bool test(T val)\n    {\n        using CommonType = typename std::common_type<OfType, T>::type;\n        return static_cast<CommonType>(val) <= static_cast<CommonType>((std::numeric_limits<OfType>::max)());\n    }\n};\n\ntemplate<typename OfType, typename T>\nstruct value_in_range_of_impl2<OfType, T, true, false>\n{\n    static constexpr bool test(T val)\n    {\n        using CommonType = typename std::common_type<OfType, T>::type;\n        return static_cast<CommonType>(val) <= static_cast<CommonType>((std::numeric_limits<OfType>::max)());\n    }\n};\n\ntemplate<typename OfType, typename T>\nstruct value_in_range_of_impl2<OfType, T, false, true>\n{\n    static constexpr bool test(T val)\n    {\n        using CommonType = typename std::common_type<OfType, T>::type;\n        return val >= 0 && static_cast<CommonType>(val) <= static_cast<CommonType>((std::numeric_limits<OfType>::max)());\n    }\n};\n\n\ntemplate<typename OfType, typename T>\nstruct value_in_range_of_impl2<OfType, T, true, true>\n{\n    static constexpr bool test(T val)\n    {\n        using CommonType = typename std::common_type<OfType, T>::type;\n        return static_cast<CommonType>(val) >= static_cast<CommonType>((std::numeric_limits<OfType>::min)())\n               && static_cast<CommonType>(val) <= static_cast<CommonType>((std::numeric_limits<OfType>::max)());\n    }\n};\n\ntemplate<typename OfType, typename T,\n         bool NeverOutOfRange = never_out_of_range<OfType, T>::value,\n         typename = detail::enable_if_t<all_integral<OfType, T>::value>>\nstruct value_in_range_of_impl1;\n\ntemplate<typename OfType, typename T>\nstruct value_in_range_of_impl1<OfType, T, false>\n{\n    static constexpr bool test(T val)\n    {\n        return value_in_range_of_impl2<OfType, T>::test(val);\n    }\n};\n\ntemplate<typename OfType, typename T>\nstruct value_in_range_of_impl1<OfType, T, true>\n{\n    static constexpr bool test(T /*val*/)\n    {\n        return true;\n    }\n};\n\ntemplate<typename OfType, typename T>\ninline constexpr bool value_in_range_of(T val)\n{\n    return value_in_range_of_impl1<OfType, T>::test(val);\n}\n\ntemplate<bool Value>\nusing bool_constant = std::integral_constant<bool, Value>;\n\n///////////////////////////////////////////////////////////////////////////////\n// is_c_string\n///////////////////////////////////////////////////////////////////////////////\n\nnamespace impl\n{\n\ntemplate<typename T>\ninline constexpr bool is_c_string()\n{\n    using TUnExt = typename std::remove_extent<T>::type;\n    using TUnCVExt = typename std::remove_cv<TUnExt>::type;\n    using TUnPtr = typename std::remove_pointer<T>::type;\n    using TUnCVPtr = typename std::remove_cv<TUnPtr>::type;\n    return\n        (std::is_array<T>::value && std::is_same<TUnCVExt, char>::value)\n        || (std::is_pointer<T>::value && std::is_same<TUnCVPtr, char>::value);\n}\n\n}  // namespace impl\n\n// checks whether T is a [cv] char */[cv] char[] C string\ntemplate<typename T>\nstruct is_c_string : bool_constant<impl::is_c_string<T>()> {};\n\ntemplate<typename T>\nusing is_c_string_uncvref = is_c_string<uncvref_t<T>>;\n\n///////////////////////////////////////////////////////////////////////////////\n// is_transparent\n///////////////////////////////////////////////////////////////////////////////\n\nnamespace impl\n{\n\ntemplate<typename T>\ninline constexpr bool is_transparent()\n{\n    return is_detected<detect_is_transparent, T>::value;\n}\n\n}  // namespace impl\n\n// checks whether T has a member named is_transparent\ntemplate<typename T>\nstruct is_transparent : bool_constant<impl::is_transparent<T>()> {};\n\n///////////////////////////////////////////////////////////////////////////////\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/string_concat.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <cstring> // strlen\n#include <string> // string\n#include <utility> // forward\n\n// #include <nlohmann/detail/meta/cpp_future.hpp>\n\n// #include <nlohmann/detail/meta/detected.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\ninline std::size_t concat_length()\n{\n    return 0;\n}\n\ntemplate<typename... Args>\ninline std::size_t concat_length(const char* cstr, Args&& ... rest);\n\ntemplate<typename StringType, typename... Args>\ninline std::size_t concat_length(const StringType& str, Args&& ... rest);\n\ntemplate<typename... Args>\ninline std::size_t concat_length(const char /*c*/, Args&& ... rest)\n{\n    return 1 + concat_length(std::forward<Args>(rest)...);\n}\n\ntemplate<typename... Args>\ninline std::size_t concat_length(const char* cstr, Args&& ... rest)\n{\n    // cppcheck-suppress ignoredReturnValue\n    return ::strlen(cstr) + concat_length(std::forward<Args>(rest)...);\n}\n\ntemplate<typename StringType, typename... Args>\ninline std::size_t concat_length(const StringType& str, Args&& ... rest)\n{\n    return str.size() + concat_length(std::forward<Args>(rest)...);\n}\n\ntemplate<typename OutStringType>\ninline void concat_into(OutStringType& /*out*/)\n{}\n\ntemplate<typename StringType, typename Arg>\nusing string_can_append = decltype(std::declval<StringType&>().append(std::declval < Arg && > ()));\n\ntemplate<typename StringType, typename Arg>\nusing detect_string_can_append = is_detected<string_can_append, StringType, Arg>;\n\ntemplate<typename StringType, typename Arg>\nusing string_can_append_op = decltype(std::declval<StringType&>() += std::declval < Arg && > ());\n\ntemplate<typename StringType, typename Arg>\nusing detect_string_can_append_op = is_detected<string_can_append_op, StringType, Arg>;\n\ntemplate<typename StringType, typename Arg>\nusing string_can_append_iter = decltype(std::declval<StringType&>().append(std::declval<const Arg&>().begin(), std::declval<const Arg&>().end()));\n\ntemplate<typename StringType, typename Arg>\nusing detect_string_can_append_iter = is_detected<string_can_append_iter, StringType, Arg>;\n\ntemplate<typename StringType, typename Arg>\nusing string_can_append_data = decltype(std::declval<StringType&>().append(std::declval<const Arg&>().data(), std::declval<const Arg&>().size()));\n\ntemplate<typename StringType, typename Arg>\nusing detect_string_can_append_data = is_detected<string_can_append_data, StringType, Arg>;\n\ntemplate < typename OutStringType, typename Arg, typename... Args,\n           enable_if_t < !detect_string_can_append<OutStringType, Arg>::value\n                         && detect_string_can_append_op<OutStringType, Arg>::value, int > = 0 >\ninline void concat_into(OutStringType& out, Arg && arg, Args && ... rest);\n\ntemplate < typename OutStringType, typename Arg, typename... Args,\n           enable_if_t < !detect_string_can_append<OutStringType, Arg>::value\n                         && !detect_string_can_append_op<OutStringType, Arg>::value\n                         && detect_string_can_append_iter<OutStringType, Arg>::value, int > = 0 >\ninline void concat_into(OutStringType& out, const Arg& arg, Args && ... rest);\n\ntemplate < typename OutStringType, typename Arg, typename... Args,\n           enable_if_t < !detect_string_can_append<OutStringType, Arg>::value\n                         && !detect_string_can_append_op<OutStringType, Arg>::value\n                         && !detect_string_can_append_iter<OutStringType, Arg>::value\n                         && detect_string_can_append_data<OutStringType, Arg>::value, int > = 0 >\ninline void concat_into(OutStringType& out, const Arg& arg, Args && ... rest);\n\ntemplate<typename OutStringType, typename Arg, typename... Args,\n         enable_if_t<detect_string_can_append<OutStringType, Arg>::value, int> = 0>\ninline void concat_into(OutStringType& out, Arg && arg, Args && ... rest)\n{\n    out.append(std::forward<Arg>(arg));\n    concat_into(out, std::forward<Args>(rest)...);\n}\n\ntemplate < typename OutStringType, typename Arg, typename... Args,\n           enable_if_t < !detect_string_can_append<OutStringType, Arg>::value\n                         && detect_string_can_append_op<OutStringType, Arg>::value, int > >\ninline void concat_into(OutStringType& out, Arg&& arg, Args&& ... rest)\n{\n    out += std::forward<Arg>(arg);\n    concat_into(out, std::forward<Args>(rest)...);\n}\n\ntemplate < typename OutStringType, typename Arg, typename... Args,\n           enable_if_t < !detect_string_can_append<OutStringType, Arg>::value\n                         && !detect_string_can_append_op<OutStringType, Arg>::value\n                         && detect_string_can_append_iter<OutStringType, Arg>::value, int > >\ninline void concat_into(OutStringType& out, const Arg& arg, Args&& ... rest)\n{\n    out.append(arg.begin(), arg.end());\n    concat_into(out, std::forward<Args>(rest)...);\n}\n\ntemplate < typename OutStringType, typename Arg, typename... Args,\n           enable_if_t < !detect_string_can_append<OutStringType, Arg>::value\n                         && !detect_string_can_append_op<OutStringType, Arg>::value\n                         && !detect_string_can_append_iter<OutStringType, Arg>::value\n                         && detect_string_can_append_data<OutStringType, Arg>::value, int > >\ninline void concat_into(OutStringType& out, const Arg& arg, Args&& ... rest)\n{\n    out.append(arg.data(), arg.size());\n    concat_into(out, std::forward<Args>(rest)...);\n}\n\ntemplate<typename OutStringType = std::string, typename... Args>\ninline OutStringType concat(Args && ... args)\n{\n    OutStringType str;\n    str.reserve(concat_length(std::forward<Args>(args)...));\n    concat_into(str, std::forward<Args>(args)...);\n    return str;\n}\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n////////////////\n// exceptions //\n////////////////\n\n/// @brief general exception of the @ref basic_json class\n/// @sa https://json.nlohmann.me/api/basic_json/exception/\nclass exception : public std::exception\n{\n  public:\n    /// returns the explanatory string\n    const char* what() const noexcept override\n    {\n        return m.what();\n    }\n\n    /// the id of the exception\n    const int id; // NOLINT(cppcoreguidelines-non-private-member-variables-in-classes)\n\n  protected:\n    JSON_HEDLEY_NON_NULL(3)\n    exception(int id_, const char* what_arg) : id(id_), m(what_arg) {} // NOLINT(bugprone-throw-keyword-missing)\n\n    static std::string name(const std::string& ename, int id_)\n    {\n        return concat(\"[json.exception.\", ename, '.', std::to_string(id_), \"] \");\n    }\n\n    static std::string diagnostics(std::nullptr_t /*leaf_element*/)\n    {\n        return \"\";\n    }\n\n    template<typename BasicJsonType>\n    static std::string diagnostics(const BasicJsonType* leaf_element)\n    {\n#if JSON_DIAGNOSTICS\n        std::vector<std::string> tokens;\n        for (const auto* current = leaf_element; current != nullptr && current->m_parent != nullptr; current = current->m_parent)\n        {\n            switch (current->m_parent->type())\n            {\n                case value_t::array:\n                {\n                    for (std::size_t i = 0; i < current->m_parent->m_value.array->size(); ++i)\n                    {\n                        if (&current->m_parent->m_value.array->operator[](i) == current)\n                        {\n                            tokens.emplace_back(std::to_string(i));\n                            break;\n                        }\n                    }\n                    break;\n                }\n\n                case value_t::object:\n                {\n                    for (const auto& element : *current->m_parent->m_value.object)\n                    {\n                        if (&element.second == current)\n                        {\n                            tokens.emplace_back(element.first.c_str());\n                            break;\n                        }\n                    }\n                    break;\n                }\n\n                case value_t::null: // LCOV_EXCL_LINE\n                case value_t::string: // LCOV_EXCL_LINE\n                case value_t::boolean: // LCOV_EXCL_LINE\n                case value_t::number_integer: // LCOV_EXCL_LINE\n                case value_t::number_unsigned: // LCOV_EXCL_LINE\n                case value_t::number_float: // LCOV_EXCL_LINE\n                case value_t::binary: // LCOV_EXCL_LINE\n                case value_t::discarded: // LCOV_EXCL_LINE\n                default:   // LCOV_EXCL_LINE\n                    break; // LCOV_EXCL_LINE\n            }\n        }\n\n        if (tokens.empty())\n        {\n            return \"\";\n        }\n\n        auto str = std::accumulate(tokens.rbegin(), tokens.rend(), std::string{},\n                                   [](const std::string & a, const std::string & b)\n        {\n            return concat(a, '/', detail::escape(b));\n        });\n        return concat('(', str, \") \");\n#else\n        static_cast<void>(leaf_element);\n        return \"\";\n#endif\n    }\n\n  private:\n    /// an exception object as storage for error messages\n    std::runtime_error m;\n};\n\n/// @brief exception indicating a parse error\n/// @sa https://json.nlohmann.me/api/basic_json/parse_error/\nclass parse_error : public exception\n{\n  public:\n    /*!\n    @brief create a parse error exception\n    @param[in] id_       the id of the exception\n    @param[in] pos       the position where the error occurred (or with\n                         chars_read_total=0 if the position cannot be\n                         determined)\n    @param[in] what_arg  the explanatory string\n    @return parse_error object\n    */\n    template<typename BasicJsonContext, enable_if_t<is_basic_json_context<BasicJsonContext>::value, int> = 0>\n    static parse_error create(int id_, const position_t& pos, const std::string& what_arg, BasicJsonContext context)\n    {\n        std::string w = concat(exception::name(\"parse_error\", id_), \"parse error\",\n                               position_string(pos), \": \", exception::diagnostics(context), what_arg);\n        return {id_, pos.chars_read_total, w.c_str()};\n    }\n\n    template<typename BasicJsonContext, enable_if_t<is_basic_json_context<BasicJsonContext>::value, int> = 0>\n    static parse_error create(int id_, std::size_t byte_, const std::string& what_arg, BasicJsonContext context)\n    {\n        std::string w = concat(exception::name(\"parse_error\", id_), \"parse error\",\n                               (byte_ != 0 ? (concat(\" at byte \", std::to_string(byte_))) : \"\"),\n                               \": \", exception::diagnostics(context), what_arg);\n        return {id_, byte_, w.c_str()};\n    }\n\n    /*!\n    @brief byte index of the parse error\n\n    The byte index of the last read character in the input file.\n\n    @note For an input with n bytes, 1 is the index of the first character and\n          n+1 is the index of the terminating null byte or the end of file.\n          This also holds true when reading a byte vector (CBOR or MessagePack).\n    */\n    const std::size_t byte;\n\n  private:\n    parse_error(int id_, std::size_t byte_, const char* what_arg)\n        : exception(id_, what_arg), byte(byte_) {}\n\n    static std::string position_string(const position_t& pos)\n    {\n        return concat(\" at line \", std::to_string(pos.lines_read + 1),\n                      \", column \", std::to_string(pos.chars_read_current_line));\n    }\n};\n\n/// @brief exception indicating errors with iterators\n/// @sa https://json.nlohmann.me/api/basic_json/invalid_iterator/\nclass invalid_iterator : public exception\n{\n  public:\n    template<typename BasicJsonContext, enable_if_t<is_basic_json_context<BasicJsonContext>::value, int> = 0>\n    static invalid_iterator create(int id_, const std::string& what_arg, BasicJsonContext context)\n    {\n        std::string w = concat(exception::name(\"invalid_iterator\", id_), exception::diagnostics(context), what_arg);\n        return {id_, w.c_str()};\n    }\n\n  private:\n    JSON_HEDLEY_NON_NULL(3)\n    invalid_iterator(int id_, const char* what_arg)\n        : exception(id_, what_arg) {}\n};\n\n/// @brief exception indicating executing a member function with a wrong type\n/// @sa https://json.nlohmann.me/api/basic_json/type_error/\nclass type_error : public exception\n{\n  public:\n    template<typename BasicJsonContext, enable_if_t<is_basic_json_context<BasicJsonContext>::value, int> = 0>\n    static type_error create(int id_, const std::string& what_arg, BasicJsonContext context)\n    {\n        std::string w = concat(exception::name(\"type_error\", id_), exception::diagnostics(context), what_arg);\n        return {id_, w.c_str()};\n    }\n\n  private:\n    JSON_HEDLEY_NON_NULL(3)\n    type_error(int id_, const char* what_arg) : exception(id_, what_arg) {}\n};\n\n/// @brief exception indicating access out of the defined range\n/// @sa https://json.nlohmann.me/api/basic_json/out_of_range/\nclass out_of_range : public exception\n{\n  public:\n    template<typename BasicJsonContext, enable_if_t<is_basic_json_context<BasicJsonContext>::value, int> = 0>\n    static out_of_range create(int id_, const std::string& what_arg, BasicJsonContext context)\n    {\n        std::string w = concat(exception::name(\"out_of_range\", id_), exception::diagnostics(context), what_arg);\n        return {id_, w.c_str()};\n    }\n\n  private:\n    JSON_HEDLEY_NON_NULL(3)\n    out_of_range(int id_, const char* what_arg) : exception(id_, what_arg) {}\n};\n\n/// @brief exception indicating other library errors\n/// @sa https://json.nlohmann.me/api/basic_json/other_error/\nclass other_error : public exception\n{\n  public:\n    template<typename BasicJsonContext, enable_if_t<is_basic_json_context<BasicJsonContext>::value, int> = 0>\n    static other_error create(int id_, const std::string& what_arg, BasicJsonContext context)\n    {\n        std::string w = concat(exception::name(\"other_error\", id_), exception::diagnostics(context), what_arg);\n        return {id_, w.c_str()};\n    }\n\n  private:\n    JSON_HEDLEY_NON_NULL(3)\n    other_error(int id_, const char* what_arg) : exception(id_, what_arg) {}\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/meta/cpp_future.hpp>\n\n// #include <nlohmann/detail/meta/identity_tag.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n// dispatching helper struct\ntemplate <class T> struct identity_tag {};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/meta/std_fs.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n\n#if JSON_HAS_EXPERIMENTAL_FILESYSTEM\n#include <experimental/filesystem>\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\nnamespace std_fs = std::experimental::filesystem;\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n#elif JSON_HAS_FILESYSTEM\n#include <filesystem>\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\nnamespace std_fs = std::filesystem;\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n#endif\n\n// #include <nlohmann/detail/meta/type_traits.hpp>\n\n// #include <nlohmann/detail/string_concat.hpp>\n\n// #include <nlohmann/detail/value_t.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\ntemplate<typename BasicJsonType>\ninline void from_json(const BasicJsonType& j, typename std::nullptr_t& n)\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_null()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be null, but is \", j.type_name()), &j));\n    }\n    n = nullptr;\n}\n\n// overloads for basic_json template parameters\ntemplate < typename BasicJsonType, typename ArithmeticType,\n           enable_if_t < std::is_arithmetic<ArithmeticType>::value&&\n                         !std::is_same<ArithmeticType, typename BasicJsonType::boolean_t>::value,\n                         int > = 0 >\nvoid get_arithmetic_value(const BasicJsonType& j, ArithmeticType& val)\n{\n    switch (static_cast<value_t>(j))\n    {\n        case value_t::number_unsigned:\n        {\n            val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_unsigned_t*>());\n            break;\n        }\n        case value_t::number_integer:\n        {\n            val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_integer_t*>());\n            break;\n        }\n        case value_t::number_float:\n        {\n            val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_float_t*>());\n            break;\n        }\n\n        case value_t::null:\n        case value_t::object:\n        case value_t::array:\n        case value_t::string:\n        case value_t::boolean:\n        case value_t::binary:\n        case value_t::discarded:\n        default:\n            JSON_THROW(type_error::create(302, concat(\"type must be number, but is \", j.type_name()), &j));\n    }\n}\n\ntemplate<typename BasicJsonType>\ninline void from_json(const BasicJsonType& j, typename BasicJsonType::boolean_t& b)\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_boolean()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be boolean, but is \", j.type_name()), &j));\n    }\n    b = *j.template get_ptr<const typename BasicJsonType::boolean_t*>();\n}\n\ntemplate<typename BasicJsonType>\ninline void from_json(const BasicJsonType& j, typename BasicJsonType::string_t& s)\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_string()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be string, but is \", j.type_name()), &j));\n    }\n    s = *j.template get_ptr<const typename BasicJsonType::string_t*>();\n}\n\ntemplate <\n    typename BasicJsonType, typename StringType,\n    enable_if_t <\n        std::is_assignable<StringType&, const typename BasicJsonType::string_t>::value\n        && is_detected_exact<typename BasicJsonType::string_t::value_type, value_type_t, StringType>::value\n        && !std::is_same<typename BasicJsonType::string_t, StringType>::value\n        && !is_json_ref<StringType>::value, int > = 0 >\ninline void from_json(const BasicJsonType& j, StringType& s)\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_string()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be string, but is \", j.type_name()), &j));\n    }\n\n    s = *j.template get_ptr<const typename BasicJsonType::string_t*>();\n}\n\ntemplate<typename BasicJsonType>\ninline void from_json(const BasicJsonType& j, typename BasicJsonType::number_float_t& val)\n{\n    get_arithmetic_value(j, val);\n}\n\ntemplate<typename BasicJsonType>\ninline void from_json(const BasicJsonType& j, typename BasicJsonType::number_unsigned_t& val)\n{\n    get_arithmetic_value(j, val);\n}\n\ntemplate<typename BasicJsonType>\ninline void from_json(const BasicJsonType& j, typename BasicJsonType::number_integer_t& val)\n{\n    get_arithmetic_value(j, val);\n}\n\n#if !JSON_DISABLE_ENUM_SERIALIZATION\ntemplate<typename BasicJsonType, typename EnumType,\n         enable_if_t<std::is_enum<EnumType>::value, int> = 0>\ninline void from_json(const BasicJsonType& j, EnumType& e)\n{\n    typename std::underlying_type<EnumType>::type val;\n    get_arithmetic_value(j, val);\n    e = static_cast<EnumType>(val);\n}\n#endif  // JSON_DISABLE_ENUM_SERIALIZATION\n\n// forward_list doesn't have an insert method\ntemplate<typename BasicJsonType, typename T, typename Allocator,\n         enable_if_t<is_getable<BasicJsonType, T>::value, int> = 0>\ninline void from_json(const BasicJsonType& j, std::forward_list<T, Allocator>& l)\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_array()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be array, but is \", j.type_name()), &j));\n    }\n    l.clear();\n    std::transform(j.rbegin(), j.rend(),\n                   std::front_inserter(l), [](const BasicJsonType & i)\n    {\n        return i.template get<T>();\n    });\n}\n\n// valarray doesn't have an insert method\ntemplate<typename BasicJsonType, typename T,\n         enable_if_t<is_getable<BasicJsonType, T>::value, int> = 0>\ninline void from_json(const BasicJsonType& j, std::valarray<T>& l)\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_array()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be array, but is \", j.type_name()), &j));\n    }\n    l.resize(j.size());\n    std::transform(j.begin(), j.end(), std::begin(l),\n                   [](const BasicJsonType & elem)\n    {\n        return elem.template get<T>();\n    });\n}\n\ntemplate<typename BasicJsonType, typename T, std::size_t N>\nauto from_json(const BasicJsonType& j, T (&arr)[N])  // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays)\n-> decltype(j.template get<T>(), void())\n{\n    for (std::size_t i = 0; i < N; ++i)\n    {\n        arr[i] = j.at(i).template get<T>();\n    }\n}\n\ntemplate<typename BasicJsonType>\ninline void from_json_array_impl(const BasicJsonType& j, typename BasicJsonType::array_t& arr, priority_tag<3> /*unused*/)\n{\n    arr = *j.template get_ptr<const typename BasicJsonType::array_t*>();\n}\n\ntemplate<typename BasicJsonType, typename T, std::size_t N>\nauto from_json_array_impl(const BasicJsonType& j, std::array<T, N>& arr,\n                          priority_tag<2> /*unused*/)\n-> decltype(j.template get<T>(), void())\n{\n    for (std::size_t i = 0; i < N; ++i)\n    {\n        arr[i] = j.at(i).template get<T>();\n    }\n}\n\ntemplate<typename BasicJsonType, typename ConstructibleArrayType,\n         enable_if_t<\n             std::is_assignable<ConstructibleArrayType&, ConstructibleArrayType>::value,\n             int> = 0>\nauto from_json_array_impl(const BasicJsonType& j, ConstructibleArrayType& arr, priority_tag<1> /*unused*/)\n-> decltype(\n    arr.reserve(std::declval<typename ConstructibleArrayType::size_type>()),\n    j.template get<typename ConstructibleArrayType::value_type>(),\n    void())\n{\n    using std::end;\n\n    ConstructibleArrayType ret;\n    ret.reserve(j.size());\n    std::transform(j.begin(), j.end(),\n                   std::inserter(ret, end(ret)), [](const BasicJsonType & i)\n    {\n        // get<BasicJsonType>() returns *this, this won't call a from_json\n        // method when value_type is BasicJsonType\n        return i.template get<typename ConstructibleArrayType::value_type>();\n    });\n    arr = std::move(ret);\n}\n\ntemplate<typename BasicJsonType, typename ConstructibleArrayType,\n         enable_if_t<\n             std::is_assignable<ConstructibleArrayType&, ConstructibleArrayType>::value,\n             int> = 0>\ninline void from_json_array_impl(const BasicJsonType& j, ConstructibleArrayType& arr,\n                                 priority_tag<0> /*unused*/)\n{\n    using std::end;\n\n    ConstructibleArrayType ret;\n    std::transform(\n        j.begin(), j.end(), std::inserter(ret, end(ret)),\n        [](const BasicJsonType & i)\n    {\n        // get<BasicJsonType>() returns *this, this won't call a from_json\n        // method when value_type is BasicJsonType\n        return i.template get<typename ConstructibleArrayType::value_type>();\n    });\n    arr = std::move(ret);\n}\n\ntemplate < typename BasicJsonType, typename ConstructibleArrayType,\n           enable_if_t <\n               is_constructible_array_type<BasicJsonType, ConstructibleArrayType>::value&&\n               !is_constructible_object_type<BasicJsonType, ConstructibleArrayType>::value&&\n               !is_constructible_string_type<BasicJsonType, ConstructibleArrayType>::value&&\n               !std::is_same<ConstructibleArrayType, typename BasicJsonType::binary_t>::value&&\n               !is_basic_json<ConstructibleArrayType>::value,\n               int > = 0 >\nauto from_json(const BasicJsonType& j, ConstructibleArrayType& arr)\n-> decltype(from_json_array_impl(j, arr, priority_tag<3> {}),\nj.template get<typename ConstructibleArrayType::value_type>(),\nvoid())\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_array()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be array, but is \", j.type_name()), &j));\n    }\n\n    from_json_array_impl(j, arr, priority_tag<3> {});\n}\n\ntemplate < typename BasicJsonType, typename T, std::size_t... Idx >\nstd::array<T, sizeof...(Idx)> from_json_inplace_array_impl(BasicJsonType&& j,\n        identity_tag<std::array<T, sizeof...(Idx)>> /*unused*/, index_sequence<Idx...> /*unused*/)\n{\n    return { { std::forward<BasicJsonType>(j).at(Idx).template get<T>()... } };\n}\n\ntemplate < typename BasicJsonType, typename T, std::size_t N >\nauto from_json(BasicJsonType&& j, identity_tag<std::array<T, N>> tag)\n-> decltype(from_json_inplace_array_impl(std::forward<BasicJsonType>(j), tag, make_index_sequence<N> {}))\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_array()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be array, but is \", j.type_name()), &j));\n    }\n\n    return from_json_inplace_array_impl(std::forward<BasicJsonType>(j), tag, make_index_sequence<N> {});\n}\n\ntemplate<typename BasicJsonType>\ninline void from_json(const BasicJsonType& j, typename BasicJsonType::binary_t& bin)\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_binary()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be binary, but is \", j.type_name()), &j));\n    }\n\n    bin = *j.template get_ptr<const typename BasicJsonType::binary_t*>();\n}\n\ntemplate<typename BasicJsonType, typename ConstructibleObjectType,\n         enable_if_t<is_constructible_object_type<BasicJsonType, ConstructibleObjectType>::value, int> = 0>\ninline void from_json(const BasicJsonType& j, ConstructibleObjectType& obj)\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_object()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be object, but is \", j.type_name()), &j));\n    }\n\n    ConstructibleObjectType ret;\n    const auto* inner_object = j.template get_ptr<const typename BasicJsonType::object_t*>();\n    using value_type = typename ConstructibleObjectType::value_type;\n    std::transform(\n        inner_object->begin(), inner_object->end(),\n        std::inserter(ret, ret.begin()),\n        [](typename BasicJsonType::object_t::value_type const & p)\n    {\n        return value_type(p.first, p.second.template get<typename ConstructibleObjectType::mapped_type>());\n    });\n    obj = std::move(ret);\n}\n\n// overload for arithmetic types, not chosen for basic_json template arguments\n// (BooleanType, etc..); note: Is it really necessary to provide explicit\n// overloads for boolean_t etc. in case of a custom BooleanType which is not\n// an arithmetic type?\ntemplate < typename BasicJsonType, typename ArithmeticType,\n           enable_if_t <\n               std::is_arithmetic<ArithmeticType>::value&&\n               !std::is_same<ArithmeticType, typename BasicJsonType::number_unsigned_t>::value&&\n               !std::is_same<ArithmeticType, typename BasicJsonType::number_integer_t>::value&&\n               !std::is_same<ArithmeticType, typename BasicJsonType::number_float_t>::value&&\n               !std::is_same<ArithmeticType, typename BasicJsonType::boolean_t>::value,\n               int > = 0 >\ninline void from_json(const BasicJsonType& j, ArithmeticType& val)\n{\n    switch (static_cast<value_t>(j))\n    {\n        case value_t::number_unsigned:\n        {\n            val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_unsigned_t*>());\n            break;\n        }\n        case value_t::number_integer:\n        {\n            val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_integer_t*>());\n            break;\n        }\n        case value_t::number_float:\n        {\n            val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_float_t*>());\n            break;\n        }\n        case value_t::boolean:\n        {\n            val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::boolean_t*>());\n            break;\n        }\n\n        case value_t::null:\n        case value_t::object:\n        case value_t::array:\n        case value_t::string:\n        case value_t::binary:\n        case value_t::discarded:\n        default:\n            JSON_THROW(type_error::create(302, concat(\"type must be number, but is \", j.type_name()), &j));\n    }\n}\n\ntemplate<typename BasicJsonType, typename... Args, std::size_t... Idx>\nstd::tuple<Args...> from_json_tuple_impl_base(BasicJsonType&& j, index_sequence<Idx...> /*unused*/)\n{\n    return std::make_tuple(std::forward<BasicJsonType>(j).at(Idx).template get<Args>()...);\n}\n\ntemplate < typename BasicJsonType, class A1, class A2 >\nstd::pair<A1, A2> from_json_tuple_impl(BasicJsonType&& j, identity_tag<std::pair<A1, A2>> /*unused*/, priority_tag<0> /*unused*/)\n{\n    return {std::forward<BasicJsonType>(j).at(0).template get<A1>(),\n            std::forward<BasicJsonType>(j).at(1).template get<A2>()};\n}\n\ntemplate<typename BasicJsonType, typename A1, typename A2>\ninline void from_json_tuple_impl(BasicJsonType&& j, std::pair<A1, A2>& p, priority_tag<1> /*unused*/)\n{\n    p = from_json_tuple_impl(std::forward<BasicJsonType>(j), identity_tag<std::pair<A1, A2>> {}, priority_tag<0> {});\n}\n\ntemplate<typename BasicJsonType, typename... Args>\nstd::tuple<Args...> from_json_tuple_impl(BasicJsonType&& j, identity_tag<std::tuple<Args...>> /*unused*/, priority_tag<2> /*unused*/)\n{\n    return from_json_tuple_impl_base<BasicJsonType, Args...>(std::forward<BasicJsonType>(j), index_sequence_for<Args...> {});\n}\n\ntemplate<typename BasicJsonType, typename... Args>\ninline void from_json_tuple_impl(BasicJsonType&& j, std::tuple<Args...>& t, priority_tag<3> /*unused*/)\n{\n    t = from_json_tuple_impl_base<BasicJsonType, Args...>(std::forward<BasicJsonType>(j), index_sequence_for<Args...> {});\n}\n\ntemplate<typename BasicJsonType, typename TupleRelated>\nauto from_json(BasicJsonType&& j, TupleRelated&& t)\n-> decltype(from_json_tuple_impl(std::forward<BasicJsonType>(j), std::forward<TupleRelated>(t), priority_tag<3> {}))\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_array()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be array, but is \", j.type_name()), &j));\n    }\n\n    return from_json_tuple_impl(std::forward<BasicJsonType>(j), std::forward<TupleRelated>(t), priority_tag<3> {});\n}\n\ntemplate < typename BasicJsonType, typename Key, typename Value, typename Compare, typename Allocator,\n           typename = enable_if_t < !std::is_constructible <\n                                        typename BasicJsonType::string_t, Key >::value >>\ninline void from_json(const BasicJsonType& j, std::map<Key, Value, Compare, Allocator>& m)\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_array()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be array, but is \", j.type_name()), &j));\n    }\n    m.clear();\n    for (const auto& p : j)\n    {\n        if (JSON_HEDLEY_UNLIKELY(!p.is_array()))\n        {\n            JSON_THROW(type_error::create(302, concat(\"type must be array, but is \", p.type_name()), &j));\n        }\n        m.emplace(p.at(0).template get<Key>(), p.at(1).template get<Value>());\n    }\n}\n\ntemplate < typename BasicJsonType, typename Key, typename Value, typename Hash, typename KeyEqual, typename Allocator,\n           typename = enable_if_t < !std::is_constructible <\n                                        typename BasicJsonType::string_t, Key >::value >>\ninline void from_json(const BasicJsonType& j, std::unordered_map<Key, Value, Hash, KeyEqual, Allocator>& m)\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_array()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be array, but is \", j.type_name()), &j));\n    }\n    m.clear();\n    for (const auto& p : j)\n    {\n        if (JSON_HEDLEY_UNLIKELY(!p.is_array()))\n        {\n            JSON_THROW(type_error::create(302, concat(\"type must be array, but is \", p.type_name()), &j));\n        }\n        m.emplace(p.at(0).template get<Key>(), p.at(1).template get<Value>());\n    }\n}\n\n#if JSON_HAS_FILESYSTEM || JSON_HAS_EXPERIMENTAL_FILESYSTEM\ntemplate<typename BasicJsonType>\ninline void from_json(const BasicJsonType& j, std_fs::path& p)\n{\n    if (JSON_HEDLEY_UNLIKELY(!j.is_string()))\n    {\n        JSON_THROW(type_error::create(302, concat(\"type must be string, but is \", j.type_name()), &j));\n    }\n    p = *j.template get_ptr<const typename BasicJsonType::string_t*>();\n}\n#endif\n\nstruct from_json_fn\n{\n    template<typename BasicJsonType, typename T>\n    auto operator()(const BasicJsonType& j, T&& val) const\n    noexcept(noexcept(from_json(j, std::forward<T>(val))))\n    -> decltype(from_json(j, std::forward<T>(val)))\n    {\n        return from_json(j, std::forward<T>(val));\n    }\n};\n\n}  // namespace detail\n\n#ifndef JSON_HAS_CPP_17\n/// namespace to hold default `from_json` function\n/// to see why this is required:\n/// http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2015/n4381.html\nnamespace // NOLINT(cert-dcl59-cpp,fuchsia-header-anon-namespaces,google-build-namespaces)\n{\n#endif\nJSON_INLINE_VARIABLE constexpr const auto& from_json = // NOLINT(misc-definitions-in-headers)\n    detail::static_const<detail::from_json_fn>::value;\n#ifndef JSON_HAS_CPP_17\n}  // namespace\n#endif\n\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/conversions/to_json.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <algorithm> // copy\n#include <iterator> // begin, end\n#include <string> // string\n#include <tuple> // tuple, get\n#include <type_traits> // is_same, is_constructible, is_floating_point, is_enum, underlying_type\n#include <utility> // move, forward, declval, pair\n#include <valarray> // valarray\n#include <vector> // vector\n\n// #include <nlohmann/detail/iterators/iteration_proxy.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <cstddef> // size_t\n#include <iterator> // input_iterator_tag\n#include <string> // string, to_string\n#include <tuple> // tuple_size, get, tuple_element\n#include <utility> // move\n\n#if JSON_HAS_RANGES\n    #include <ranges> // enable_borrowed_range\n#endif\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n// #include <nlohmann/detail/meta/type_traits.hpp>\n\n// #include <nlohmann/detail/value_t.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\ntemplate<typename string_type>\nvoid int_to_string( string_type& target, std::size_t value )\n{\n    // For ADL\n    using std::to_string;\n    target = to_string(value);\n}\ntemplate<typename IteratorType> class iteration_proxy_value\n{\n  public:\n    using difference_type = std::ptrdiff_t;\n    using value_type = iteration_proxy_value;\n    using pointer = value_type *;\n    using reference = value_type &;\n    using iterator_category = std::input_iterator_tag;\n    using string_type = typename std::remove_cv< typename std::remove_reference<decltype( std::declval<IteratorType>().key() ) >::type >::type;\n\n  private:\n    /// the iterator\n    IteratorType anchor{};\n    /// an index for arrays (used to create key names)\n    std::size_t array_index = 0;\n    /// last stringified array index\n    mutable std::size_t array_index_last = 0;\n    /// a string representation of the array index\n    mutable string_type array_index_str = \"0\";\n    /// an empty string (to return a reference for primitive values)\n    string_type empty_str{};\n\n  public:\n    explicit iteration_proxy_value() = default;\n    explicit iteration_proxy_value(IteratorType it, std::size_t array_index_ = 0)\n    noexcept(std::is_nothrow_move_constructible<IteratorType>::value\n             && std::is_nothrow_default_constructible<string_type>::value)\n        : anchor(std::move(it))\n        , array_index(array_index_)\n    {}\n\n    iteration_proxy_value(iteration_proxy_value const&) = default;\n    iteration_proxy_value& operator=(iteration_proxy_value const&) = default;\n    // older GCCs are a bit fussy and require explicit noexcept specifiers on defaulted functions\n    iteration_proxy_value(iteration_proxy_value&&)\n    noexcept(std::is_nothrow_move_constructible<IteratorType>::value\n             && std::is_nothrow_move_constructible<string_type>::value) = default;\n    iteration_proxy_value& operator=(iteration_proxy_value&&)\n    noexcept(std::is_nothrow_move_assignable<IteratorType>::value\n             && std::is_nothrow_move_assignable<string_type>::value) = default;\n    ~iteration_proxy_value() = default;\n\n    /// dereference operator (needed for range-based for)\n    const iteration_proxy_value& operator*() const\n    {\n        return *this;\n    }\n\n    /// increment operator (needed for range-based for)\n    iteration_proxy_value& operator++()\n    {\n        ++anchor;\n        ++array_index;\n\n        return *this;\n    }\n\n    iteration_proxy_value operator++(int)& // NOLINT(cert-dcl21-cpp)\n    {\n        auto tmp = iteration_proxy_value(anchor, array_index);\n        ++anchor;\n        ++array_index;\n        return tmp;\n    }\n\n    /// equality operator (needed for InputIterator)\n    bool operator==(const iteration_proxy_value& o) const\n    {\n        return anchor == o.anchor;\n    }\n\n    /// inequality operator (needed for range-based for)\n    bool operator!=(const iteration_proxy_value& o) const\n    {\n        return anchor != o.anchor;\n    }\n\n    /// return key of the iterator\n    const string_type& key() const\n    {\n        JSON_ASSERT(anchor.m_object != nullptr);\n\n        switch (anchor.m_object->type())\n        {\n            // use integer array index as key\n            case value_t::array:\n            {\n                if (array_index != array_index_last)\n                {\n                    int_to_string( array_index_str, array_index );\n                    array_index_last = array_index;\n                }\n                return array_index_str;\n            }\n\n            // use key from the object\n            case value_t::object:\n                return anchor.key();\n\n            // use an empty key for all primitive types\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n                return empty_str;\n        }\n    }\n\n    /// return value of the iterator\n    typename IteratorType::reference value() const\n    {\n        return anchor.value();\n    }\n};\n\n/// proxy class for the items() function\ntemplate<typename IteratorType> class iteration_proxy\n{\n  private:\n    /// the container to iterate\n    typename IteratorType::pointer container = nullptr;\n\n  public:\n    explicit iteration_proxy() = default;\n\n    /// construct iteration proxy from a container\n    explicit iteration_proxy(typename IteratorType::reference cont) noexcept\n        : container(&cont) {}\n\n    iteration_proxy(iteration_proxy const&) = default;\n    iteration_proxy& operator=(iteration_proxy const&) = default;\n    iteration_proxy(iteration_proxy&&) noexcept = default;\n    iteration_proxy& operator=(iteration_proxy&&) noexcept = default;\n    ~iteration_proxy() = default;\n\n    /// return iterator begin (needed for range-based for)\n    iteration_proxy_value<IteratorType> begin() const noexcept\n    {\n        return iteration_proxy_value<IteratorType>(container->begin());\n    }\n\n    /// return iterator end (needed for range-based for)\n    iteration_proxy_value<IteratorType> end() const noexcept\n    {\n        return iteration_proxy_value<IteratorType>(container->end());\n    }\n};\n\n// Structured Bindings Support\n// For further reference see https://blog.tartanllama.xyz/structured-bindings/\n// And see https://github.com/nlohmann/json/pull/1391\ntemplate<std::size_t N, typename IteratorType, enable_if_t<N == 0, int> = 0>\nauto get(const nlohmann::detail::iteration_proxy_value<IteratorType>& i) -> decltype(i.key())\n{\n    return i.key();\n}\n// Structured Bindings Support\n// For further reference see https://blog.tartanllama.xyz/structured-bindings/\n// And see https://github.com/nlohmann/json/pull/1391\ntemplate<std::size_t N, typename IteratorType, enable_if_t<N == 1, int> = 0>\nauto get(const nlohmann::detail::iteration_proxy_value<IteratorType>& i) -> decltype(i.value())\n{\n    return i.value();\n}\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// The Addition to the STD Namespace is required to add\n// Structured Bindings Support to the iteration_proxy_value class\n// For further reference see https://blog.tartanllama.xyz/structured-bindings/\n// And see https://github.com/nlohmann/json/pull/1391\nnamespace std\n{\n\n#if defined(__clang__)\n    // Fix: https://github.com/nlohmann/json/issues/1401\n    #pragma clang diagnostic push\n    #pragma clang diagnostic ignored \"-Wmismatched-tags\"\n#endif\ntemplate<typename IteratorType>\nclass tuple_size<::nlohmann::detail::iteration_proxy_value<IteratorType>>\n            : public std::integral_constant<std::size_t, 2> {};\n\ntemplate<std::size_t N, typename IteratorType>\nclass tuple_element<N, ::nlohmann::detail::iteration_proxy_value<IteratorType >>\n{\n  public:\n    using type = decltype(\n                     get<N>(std::declval <\n                            ::nlohmann::detail::iteration_proxy_value<IteratorType >> ()));\n};\n#if defined(__clang__)\n    #pragma clang diagnostic pop\n#endif\n\n}  // namespace std\n\n#if JSON_HAS_RANGES\n    template <typename IteratorType>\n    inline constexpr bool ::std::ranges::enable_borrowed_range<::nlohmann::detail::iteration_proxy<IteratorType>> = true;\n#endif\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/meta/cpp_future.hpp>\n\n// #include <nlohmann/detail/meta/std_fs.hpp>\n\n// #include <nlohmann/detail/meta/type_traits.hpp>\n\n// #include <nlohmann/detail/value_t.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n//////////////////\n// constructors //\n//////////////////\n\n/*\n * Note all external_constructor<>::construct functions need to call\n * j.m_value.destroy(j.m_type) to avoid a memory leak in case j contains an\n * allocated value (e.g., a string). See bug issue\n * https://github.com/nlohmann/json/issues/2865 for more information.\n */\n\ntemplate<value_t> struct external_constructor;\n\ntemplate<>\nstruct external_constructor<value_t::boolean>\n{\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, typename BasicJsonType::boolean_t b) noexcept\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::boolean;\n        j.m_value = b;\n        j.assert_invariant();\n    }\n};\n\ntemplate<>\nstruct external_constructor<value_t::string>\n{\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, const typename BasicJsonType::string_t& s)\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::string;\n        j.m_value = s;\n        j.assert_invariant();\n    }\n\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, typename BasicJsonType::string_t&& s)\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::string;\n        j.m_value = std::move(s);\n        j.assert_invariant();\n    }\n\n    template < typename BasicJsonType, typename CompatibleStringType,\n               enable_if_t < !std::is_same<CompatibleStringType, typename BasicJsonType::string_t>::value,\n                             int > = 0 >\n    static void construct(BasicJsonType& j, const CompatibleStringType& str)\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::string;\n        j.m_value.string = j.template create<typename BasicJsonType::string_t>(str);\n        j.assert_invariant();\n    }\n};\n\ntemplate<>\nstruct external_constructor<value_t::binary>\n{\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, const typename BasicJsonType::binary_t& b)\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::binary;\n        j.m_value = typename BasicJsonType::binary_t(b);\n        j.assert_invariant();\n    }\n\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, typename BasicJsonType::binary_t&& b)\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::binary;\n        j.m_value = typename BasicJsonType::binary_t(std::move(b));\n        j.assert_invariant();\n    }\n};\n\ntemplate<>\nstruct external_constructor<value_t::number_float>\n{\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, typename BasicJsonType::number_float_t val) noexcept\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::number_float;\n        j.m_value = val;\n        j.assert_invariant();\n    }\n};\n\ntemplate<>\nstruct external_constructor<value_t::number_unsigned>\n{\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, typename BasicJsonType::number_unsigned_t val) noexcept\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::number_unsigned;\n        j.m_value = val;\n        j.assert_invariant();\n    }\n};\n\ntemplate<>\nstruct external_constructor<value_t::number_integer>\n{\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, typename BasicJsonType::number_integer_t val) noexcept\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::number_integer;\n        j.m_value = val;\n        j.assert_invariant();\n    }\n};\n\ntemplate<>\nstruct external_constructor<value_t::array>\n{\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, const typename BasicJsonType::array_t& arr)\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::array;\n        j.m_value = arr;\n        j.set_parents();\n        j.assert_invariant();\n    }\n\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, typename BasicJsonType::array_t&& arr)\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::array;\n        j.m_value = std::move(arr);\n        j.set_parents();\n        j.assert_invariant();\n    }\n\n    template < typename BasicJsonType, typename CompatibleArrayType,\n               enable_if_t < !std::is_same<CompatibleArrayType, typename BasicJsonType::array_t>::value,\n                             int > = 0 >\n    static void construct(BasicJsonType& j, const CompatibleArrayType& arr)\n    {\n        using std::begin;\n        using std::end;\n\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::array;\n        j.m_value.array = j.template create<typename BasicJsonType::array_t>(begin(arr), end(arr));\n        j.set_parents();\n        j.assert_invariant();\n    }\n\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, const std::vector<bool>& arr)\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::array;\n        j.m_value = value_t::array;\n        j.m_value.array->reserve(arr.size());\n        for (const bool x : arr)\n        {\n            j.m_value.array->push_back(x);\n            j.set_parent(j.m_value.array->back());\n        }\n        j.assert_invariant();\n    }\n\n    template<typename BasicJsonType, typename T,\n             enable_if_t<std::is_convertible<T, BasicJsonType>::value, int> = 0>\n    static void construct(BasicJsonType& j, const std::valarray<T>& arr)\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::array;\n        j.m_value = value_t::array;\n        j.m_value.array->resize(arr.size());\n        if (arr.size() > 0)\n        {\n            std::copy(std::begin(arr), std::end(arr), j.m_value.array->begin());\n        }\n        j.set_parents();\n        j.assert_invariant();\n    }\n};\n\ntemplate<>\nstruct external_constructor<value_t::object>\n{\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, const typename BasicJsonType::object_t& obj)\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::object;\n        j.m_value = obj;\n        j.set_parents();\n        j.assert_invariant();\n    }\n\n    template<typename BasicJsonType>\n    static void construct(BasicJsonType& j, typename BasicJsonType::object_t&& obj)\n    {\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::object;\n        j.m_value = std::move(obj);\n        j.set_parents();\n        j.assert_invariant();\n    }\n\n    template < typename BasicJsonType, typename CompatibleObjectType,\n               enable_if_t < !std::is_same<CompatibleObjectType, typename BasicJsonType::object_t>::value, int > = 0 >\n    static void construct(BasicJsonType& j, const CompatibleObjectType& obj)\n    {\n        using std::begin;\n        using std::end;\n\n        j.m_value.destroy(j.m_type);\n        j.m_type = value_t::object;\n        j.m_value.object = j.template create<typename BasicJsonType::object_t>(begin(obj), end(obj));\n        j.set_parents();\n        j.assert_invariant();\n    }\n};\n\n/////////////\n// to_json //\n/////////////\n\ntemplate<typename BasicJsonType, typename T,\n         enable_if_t<std::is_same<T, typename BasicJsonType::boolean_t>::value, int> = 0>\ninline void to_json(BasicJsonType& j, T b) noexcept\n{\n    external_constructor<value_t::boolean>::construct(j, b);\n}\n\ntemplate < typename BasicJsonType, typename BoolRef,\n           enable_if_t <\n               ((std::is_same<std::vector<bool>::reference, BoolRef>::value\n                 && !std::is_same <std::vector<bool>::reference, typename BasicJsonType::boolean_t&>::value)\n                || (std::is_same<std::vector<bool>::const_reference, BoolRef>::value\n                    && !std::is_same <detail::uncvref_t<std::vector<bool>::const_reference>,\n                                      typename BasicJsonType::boolean_t >::value))\n               && std::is_convertible<const BoolRef&, typename BasicJsonType::boolean_t>::value, int > = 0 >\ninline void to_json(BasicJsonType& j, const BoolRef& b) noexcept\n{\n    external_constructor<value_t::boolean>::construct(j, static_cast<typename BasicJsonType::boolean_t>(b));\n}\n\ntemplate<typename BasicJsonType, typename CompatibleString,\n         enable_if_t<std::is_constructible<typename BasicJsonType::string_t, CompatibleString>::value, int> = 0>\ninline void to_json(BasicJsonType& j, const CompatibleString& s)\n{\n    external_constructor<value_t::string>::construct(j, s);\n}\n\ntemplate<typename BasicJsonType>\ninline void to_json(BasicJsonType& j, typename BasicJsonType::string_t&& s)\n{\n    external_constructor<value_t::string>::construct(j, std::move(s));\n}\n\ntemplate<typename BasicJsonType, typename FloatType,\n         enable_if_t<std::is_floating_point<FloatType>::value, int> = 0>\ninline void to_json(BasicJsonType& j, FloatType val) noexcept\n{\n    external_constructor<value_t::number_float>::construct(j, static_cast<typename BasicJsonType::number_float_t>(val));\n}\n\ntemplate<typename BasicJsonType, typename CompatibleNumberUnsignedType,\n         enable_if_t<is_compatible_integer_type<typename BasicJsonType::number_unsigned_t, CompatibleNumberUnsignedType>::value, int> = 0>\ninline void to_json(BasicJsonType& j, CompatibleNumberUnsignedType val) noexcept\n{\n    external_constructor<value_t::number_unsigned>::construct(j, static_cast<typename BasicJsonType::number_unsigned_t>(val));\n}\n\ntemplate<typename BasicJsonType, typename CompatibleNumberIntegerType,\n         enable_if_t<is_compatible_integer_type<typename BasicJsonType::number_integer_t, CompatibleNumberIntegerType>::value, int> = 0>\ninline void to_json(BasicJsonType& j, CompatibleNumberIntegerType val) noexcept\n{\n    external_constructor<value_t::number_integer>::construct(j, static_cast<typename BasicJsonType::number_integer_t>(val));\n}\n\n#if !JSON_DISABLE_ENUM_SERIALIZATION\ntemplate<typename BasicJsonType, typename EnumType,\n         enable_if_t<std::is_enum<EnumType>::value, int> = 0>\ninline void to_json(BasicJsonType& j, EnumType e) noexcept\n{\n    using underlying_type = typename std::underlying_type<EnumType>::type;\n    external_constructor<value_t::number_integer>::construct(j, static_cast<underlying_type>(e));\n}\n#endif  // JSON_DISABLE_ENUM_SERIALIZATION\n\ntemplate<typename BasicJsonType>\ninline void to_json(BasicJsonType& j, const std::vector<bool>& e)\n{\n    external_constructor<value_t::array>::construct(j, e);\n}\n\ntemplate < typename BasicJsonType, typename CompatibleArrayType,\n           enable_if_t < is_compatible_array_type<BasicJsonType,\n                         CompatibleArrayType>::value&&\n                         !is_compatible_object_type<BasicJsonType, CompatibleArrayType>::value&&\n                         !is_compatible_string_type<BasicJsonType, CompatibleArrayType>::value&&\n                         !std::is_same<typename BasicJsonType::binary_t, CompatibleArrayType>::value&&\n                         !is_basic_json<CompatibleArrayType>::value,\n                         int > = 0 >\ninline void to_json(BasicJsonType& j, const CompatibleArrayType& arr)\n{\n    external_constructor<value_t::array>::construct(j, arr);\n}\n\ntemplate<typename BasicJsonType>\ninline void to_json(BasicJsonType& j, const typename BasicJsonType::binary_t& bin)\n{\n    external_constructor<value_t::binary>::construct(j, bin);\n}\n\ntemplate<typename BasicJsonType, typename T,\n         enable_if_t<std::is_convertible<T, BasicJsonType>::value, int> = 0>\ninline void to_json(BasicJsonType& j, const std::valarray<T>& arr)\n{\n    external_constructor<value_t::array>::construct(j, std::move(arr));\n}\n\ntemplate<typename BasicJsonType>\ninline void to_json(BasicJsonType& j, typename BasicJsonType::array_t&& arr)\n{\n    external_constructor<value_t::array>::construct(j, std::move(arr));\n}\n\ntemplate < typename BasicJsonType, typename CompatibleObjectType,\n           enable_if_t < is_compatible_object_type<BasicJsonType, CompatibleObjectType>::value&& !is_basic_json<CompatibleObjectType>::value, int > = 0 >\ninline void to_json(BasicJsonType& j, const CompatibleObjectType& obj)\n{\n    external_constructor<value_t::object>::construct(j, obj);\n}\n\ntemplate<typename BasicJsonType>\ninline void to_json(BasicJsonType& j, typename BasicJsonType::object_t&& obj)\n{\n    external_constructor<value_t::object>::construct(j, std::move(obj));\n}\n\ntemplate <\n    typename BasicJsonType, typename T, std::size_t N,\n    enable_if_t < !std::is_constructible<typename BasicJsonType::string_t,\n                  const T(&)[N]>::value, // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays)\n                  int > = 0 >\ninline void to_json(BasicJsonType& j, const T(&arr)[N]) // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays)\n{\n    external_constructor<value_t::array>::construct(j, arr);\n}\n\ntemplate < typename BasicJsonType, typename T1, typename T2, enable_if_t < std::is_constructible<BasicJsonType, T1>::value&& std::is_constructible<BasicJsonType, T2>::value, int > = 0 >\ninline void to_json(BasicJsonType& j, const std::pair<T1, T2>& p)\n{\n    j = { p.first, p.second };\n}\n\n// for https://github.com/nlohmann/json/pull/1134\ntemplate<typename BasicJsonType, typename T,\n         enable_if_t<std::is_same<T, iteration_proxy_value<typename BasicJsonType::iterator>>::value, int> = 0>\ninline void to_json(BasicJsonType& j, const T& b)\n{\n    j = { {b.key(), b.value()} };\n}\n\ntemplate<typename BasicJsonType, typename Tuple, std::size_t... Idx>\ninline void to_json_tuple_impl(BasicJsonType& j, const Tuple& t, index_sequence<Idx...> /*unused*/)\n{\n    j = { std::get<Idx>(t)... };\n}\n\ntemplate<typename BasicJsonType, typename T, enable_if_t<is_constructible_tuple<BasicJsonType, T>::value, int > = 0>\ninline void to_json(BasicJsonType& j, const T& t)\n{\n    to_json_tuple_impl(j, t, make_index_sequence<std::tuple_size<T>::value> {});\n}\n\n#if JSON_HAS_FILESYSTEM || JSON_HAS_EXPERIMENTAL_FILESYSTEM\ntemplate<typename BasicJsonType>\ninline void to_json(BasicJsonType& j, const std_fs::path& p)\n{\n    j = p.string();\n}\n#endif\n\nstruct to_json_fn\n{\n    template<typename BasicJsonType, typename T>\n    auto operator()(BasicJsonType& j, T&& val) const noexcept(noexcept(to_json(j, std::forward<T>(val))))\n    -> decltype(to_json(j, std::forward<T>(val)), void())\n    {\n        return to_json(j, std::forward<T>(val));\n    }\n};\n}  // namespace detail\n\n#ifndef JSON_HAS_CPP_17\n/// namespace to hold default `to_json` function\n/// to see why this is required:\n/// http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2015/n4381.html\nnamespace // NOLINT(cert-dcl59-cpp,fuchsia-header-anon-namespaces,google-build-namespaces)\n{\n#endif\nJSON_INLINE_VARIABLE constexpr const auto& to_json = // NOLINT(misc-definitions-in-headers)\n    detail::static_const<detail::to_json_fn>::value;\n#ifndef JSON_HAS_CPP_17\n}  // namespace\n#endif\n\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/meta/identity_tag.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\n\n/// @sa https://json.nlohmann.me/api/adl_serializer/\ntemplate<typename ValueType, typename>\nstruct adl_serializer\n{\n    /// @brief convert a JSON value to any value type\n    /// @sa https://json.nlohmann.me/api/adl_serializer/from_json/\n    template<typename BasicJsonType, typename TargetType = ValueType>\n    static auto from_json(BasicJsonType && j, TargetType& val) noexcept(\n        noexcept(::nlohmann::from_json(std::forward<BasicJsonType>(j), val)))\n    -> decltype(::nlohmann::from_json(std::forward<BasicJsonType>(j), val), void())\n    {\n        ::nlohmann::from_json(std::forward<BasicJsonType>(j), val);\n    }\n\n    /// @brief convert a JSON value to any value type\n    /// @sa https://json.nlohmann.me/api/adl_serializer/from_json/\n    template<typename BasicJsonType, typename TargetType = ValueType>\n    static auto from_json(BasicJsonType && j) noexcept(\n    noexcept(::nlohmann::from_json(std::forward<BasicJsonType>(j), detail::identity_tag<TargetType> {})))\n    -> decltype(::nlohmann::from_json(std::forward<BasicJsonType>(j), detail::identity_tag<TargetType> {}))\n    {\n        return ::nlohmann::from_json(std::forward<BasicJsonType>(j), detail::identity_tag<TargetType> {});\n    }\n\n    /// @brief convert any value type to a JSON value\n    /// @sa https://json.nlohmann.me/api/adl_serializer/to_json/\n    template<typename BasicJsonType, typename TargetType = ValueType>\n    static auto to_json(BasicJsonType& j, TargetType && val) noexcept(\n        noexcept(::nlohmann::to_json(j, std::forward<TargetType>(val))))\n    -> decltype(::nlohmann::to_json(j, std::forward<TargetType>(val)), void())\n    {\n        ::nlohmann::to_json(j, std::forward<TargetType>(val));\n    }\n};\n\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/byte_container_with_subtype.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <cstdint> // uint8_t, uint64_t\n#include <tuple> // tie\n#include <utility> // move\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\n\n/// @brief an internal type for a backed binary type\n/// @sa https://json.nlohmann.me/api/byte_container_with_subtype/\ntemplate<typename BinaryType>\nclass byte_container_with_subtype : public BinaryType\n{\n  public:\n    using container_type = BinaryType;\n    using subtype_type = std::uint64_t;\n\n    /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/byte_container_with_subtype/\n    byte_container_with_subtype() noexcept(noexcept(container_type()))\n        : container_type()\n    {}\n\n    /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/byte_container_with_subtype/\n    byte_container_with_subtype(const container_type& b) noexcept(noexcept(container_type(b)))\n        : container_type(b)\n    {}\n\n    /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/byte_container_with_subtype/\n    byte_container_with_subtype(container_type&& b) noexcept(noexcept(container_type(std::move(b))))\n        : container_type(std::move(b))\n    {}\n\n    /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/byte_container_with_subtype/\n    byte_container_with_subtype(const container_type& b, subtype_type subtype_) noexcept(noexcept(container_type(b)))\n        : container_type(b)\n        , m_subtype(subtype_)\n        , m_has_subtype(true)\n    {}\n\n    /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/byte_container_with_subtype/\n    byte_container_with_subtype(container_type&& b, subtype_type subtype_) noexcept(noexcept(container_type(std::move(b))))\n        : container_type(std::move(b))\n        , m_subtype(subtype_)\n        , m_has_subtype(true)\n    {}\n\n    bool operator==(const byte_container_with_subtype& rhs) const\n    {\n        return std::tie(static_cast<const BinaryType&>(*this), m_subtype, m_has_subtype) ==\n               std::tie(static_cast<const BinaryType&>(rhs), rhs.m_subtype, rhs.m_has_subtype);\n    }\n\n    bool operator!=(const byte_container_with_subtype& rhs) const\n    {\n        return !(rhs == *this);\n    }\n\n    /// @brief sets the binary subtype\n    /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/set_subtype/\n    void set_subtype(subtype_type subtype_) noexcept\n    {\n        m_subtype = subtype_;\n        m_has_subtype = true;\n    }\n\n    /// @brief return the binary subtype\n    /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/subtype/\n    constexpr subtype_type subtype() const noexcept\n    {\n        return m_has_subtype ? m_subtype : static_cast<subtype_type>(-1);\n    }\n\n    /// @brief return whether the value has a subtype\n    /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/has_subtype/\n    constexpr bool has_subtype() const noexcept\n    {\n        return m_has_subtype;\n    }\n\n    /// @brief clears the binary subtype\n    /// @sa https://json.nlohmann.me/api/byte_container_with_subtype/clear_subtype/\n    void clear_subtype() noexcept\n    {\n        m_subtype = 0;\n        m_has_subtype = false;\n    }\n\n  private:\n    subtype_type m_subtype = 0;\n    bool m_has_subtype = false;\n};\n\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/conversions/from_json.hpp>\n\n// #include <nlohmann/detail/conversions/to_json.hpp>\n\n// #include <nlohmann/detail/exceptions.hpp>\n\n// #include <nlohmann/detail/hash.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <cstdint> // uint8_t\n#include <cstddef> // size_t\n#include <functional> // hash\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n// #include <nlohmann/detail/value_t.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n// boost::hash_combine\ninline std::size_t combine(std::size_t seed, std::size_t h) noexcept\n{\n    seed ^= h + 0x9e3779b9 + (seed << 6U) + (seed >> 2U);\n    return seed;\n}\n\n/*!\n@brief hash a JSON value\n\nThe hash function tries to rely on std::hash where possible. Furthermore, the\ntype of the JSON value is taken into account to have different hash values for\nnull, 0, 0U, and false, etc.\n\n@tparam BasicJsonType basic_json specialization\n@param j JSON value to hash\n@return hash value of j\n*/\ntemplate<typename BasicJsonType>\nstd::size_t hash(const BasicJsonType& j)\n{\n    using string_t = typename BasicJsonType::string_t;\n    using number_integer_t = typename BasicJsonType::number_integer_t;\n    using number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n\n    const auto type = static_cast<std::size_t>(j.type());\n    switch (j.type())\n    {\n        case BasicJsonType::value_t::null:\n        case BasicJsonType::value_t::discarded:\n        {\n            return combine(type, 0);\n        }\n\n        case BasicJsonType::value_t::object:\n        {\n            auto seed = combine(type, j.size());\n            for (const auto& element : j.items())\n            {\n                const auto h = std::hash<string_t> {}(element.key());\n                seed = combine(seed, h);\n                seed = combine(seed, hash(element.value()));\n            }\n            return seed;\n        }\n\n        case BasicJsonType::value_t::array:\n        {\n            auto seed = combine(type, j.size());\n            for (const auto& element : j)\n            {\n                seed = combine(seed, hash(element));\n            }\n            return seed;\n        }\n\n        case BasicJsonType::value_t::string:\n        {\n            const auto h = std::hash<string_t> {}(j.template get_ref<const string_t&>());\n            return combine(type, h);\n        }\n\n        case BasicJsonType::value_t::boolean:\n        {\n            const auto h = std::hash<bool> {}(j.template get<bool>());\n            return combine(type, h);\n        }\n\n        case BasicJsonType::value_t::number_integer:\n        {\n            const auto h = std::hash<number_integer_t> {}(j.template get<number_integer_t>());\n            return combine(type, h);\n        }\n\n        case BasicJsonType::value_t::number_unsigned:\n        {\n            const auto h = std::hash<number_unsigned_t> {}(j.template get<number_unsigned_t>());\n            return combine(type, h);\n        }\n\n        case BasicJsonType::value_t::number_float:\n        {\n            const auto h = std::hash<number_float_t> {}(j.template get<number_float_t>());\n            return combine(type, h);\n        }\n\n        case BasicJsonType::value_t::binary:\n        {\n            auto seed = combine(type, j.get_binary().size());\n            const auto h = std::hash<bool> {}(j.get_binary().has_subtype());\n            seed = combine(seed, h);\n            seed = combine(seed, static_cast<std::size_t>(j.get_binary().subtype()));\n            for (const auto byte : j.get_binary())\n            {\n                seed = combine(seed, std::hash<std::uint8_t> {}(byte));\n            }\n            return seed;\n        }\n\n        default:                   // LCOV_EXCL_LINE\n            JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE\n            return 0;              // LCOV_EXCL_LINE\n    }\n}\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/input/binary_reader.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <algorithm> // generate_n\n#include <array> // array\n#include <cmath> // ldexp\n#include <cstddef> // size_t\n#include <cstdint> // uint8_t, uint16_t, uint32_t, uint64_t\n#include <cstdio> // snprintf\n#include <cstring> // memcpy\n#include <iterator> // back_inserter\n#include <limits> // numeric_limits\n#include <string> // char_traits, string\n#include <utility> // make_pair, move\n#include <vector> // vector\n\n// #include <nlohmann/detail/exceptions.hpp>\n\n// #include <nlohmann/detail/input/input_adapters.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <array> // array\n#include <cstddef> // size_t\n#include <cstring> // strlen\n#include <iterator> // begin, end, iterator_traits, random_access_iterator_tag, distance, next\n#include <memory> // shared_ptr, make_shared, addressof\n#include <numeric> // accumulate\n#include <string> // string, char_traits\n#include <type_traits> // enable_if, is_base_of, is_pointer, is_integral, remove_pointer\n#include <utility> // pair, declval\n\n#ifndef JSON_NO_IO\n    #include <cstdio>   // FILE *\n    #include <istream>  // istream\n#endif                  // JSON_NO_IO\n\n// #include <nlohmann/detail/iterators/iterator_traits.hpp>\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n/// the supported input formats\nenum class input_format_t { json, cbor, msgpack, ubjson, bson, bjdata };\n\n////////////////////\n// input adapters //\n////////////////////\n\n#ifndef JSON_NO_IO\n/*!\nInput adapter for stdio file access. This adapter read only 1 byte and do not use any\n buffer. This adapter is a very low level adapter.\n*/\nclass file_input_adapter\n{\n  public:\n    using char_type = char;\n\n    JSON_HEDLEY_NON_NULL(2)\n    explicit file_input_adapter(std::FILE* f) noexcept\n        : m_file(f)\n    {\n        JSON_ASSERT(m_file != nullptr);\n    }\n\n    // make class move-only\n    file_input_adapter(const file_input_adapter&) = delete;\n    file_input_adapter(file_input_adapter&&) noexcept = default;\n    file_input_adapter& operator=(const file_input_adapter&) = delete;\n    file_input_adapter& operator=(file_input_adapter&&) = delete;\n    ~file_input_adapter() = default;\n\n    std::char_traits<char>::int_type get_character() noexcept\n    {\n        return std::fgetc(m_file);\n    }\n\n  private:\n    /// the file pointer to read from\n    std::FILE* m_file;\n};\n\n\n/*!\nInput adapter for a (caching) istream. Ignores a UFT Byte Order Mark at\nbeginning of input. Does not support changing the underlying std::streambuf\nin mid-input. Maintains underlying std::istream and std::streambuf to support\nsubsequent use of standard std::istream operations to process any input\ncharacters following those used in parsing the JSON input.  Clears the\nstd::istream flags; any input errors (e.g., EOF) will be detected by the first\nsubsequent call for input from the std::istream.\n*/\nclass input_stream_adapter\n{\n  public:\n    using char_type = char;\n\n    ~input_stream_adapter()\n    {\n        // clear stream flags; we use underlying streambuf I/O, do not\n        // maintain ifstream flags, except eof\n        if (is != nullptr)\n        {\n            is->clear(is->rdstate() & std::ios::eofbit);\n        }\n    }\n\n    explicit input_stream_adapter(std::istream& i)\n        : is(&i), sb(i.rdbuf())\n    {}\n\n    // delete because of pointer members\n    input_stream_adapter(const input_stream_adapter&) = delete;\n    input_stream_adapter& operator=(input_stream_adapter&) = delete;\n    input_stream_adapter& operator=(input_stream_adapter&&) = delete;\n\n    input_stream_adapter(input_stream_adapter&& rhs) noexcept\n        : is(rhs.is), sb(rhs.sb)\n    {\n        rhs.is = nullptr;\n        rhs.sb = nullptr;\n    }\n\n    // std::istream/std::streambuf use std::char_traits<char>::to_int_type, to\n    // ensure that std::char_traits<char>::eof() and the character 0xFF do not\n    // end up as the same value, e.g. 0xFFFFFFFF.\n    std::char_traits<char>::int_type get_character()\n    {\n        auto res = sb->sbumpc();\n        // set eof manually, as we don't use the istream interface.\n        if (JSON_HEDLEY_UNLIKELY(res == std::char_traits<char>::eof()))\n        {\n            is->clear(is->rdstate() | std::ios::eofbit);\n        }\n        return res;\n    }\n\n  private:\n    /// the associated input stream\n    std::istream* is = nullptr;\n    std::streambuf* sb = nullptr;\n};\n#endif  // JSON_NO_IO\n\n// General-purpose iterator-based adapter. It might not be as fast as\n// theoretically possible for some containers, but it is extremely versatile.\ntemplate<typename IteratorType>\nclass iterator_input_adapter\n{\n  public:\n    using char_type = typename std::iterator_traits<IteratorType>::value_type;\n\n    iterator_input_adapter(IteratorType first, IteratorType last)\n        : current(std::move(first)), end(std::move(last))\n    {}\n\n    typename std::char_traits<char_type>::int_type get_character()\n    {\n        if (JSON_HEDLEY_LIKELY(current != end))\n        {\n            auto result = std::char_traits<char_type>::to_int_type(*current);\n            std::advance(current, 1);\n            return result;\n        }\n\n        return std::char_traits<char_type>::eof();\n    }\n\n  private:\n    IteratorType current;\n    IteratorType end;\n\n    template<typename BaseInputAdapter, size_t T>\n    friend struct wide_string_input_helper;\n\n    bool empty() const\n    {\n        return current == end;\n    }\n};\n\n\ntemplate<typename BaseInputAdapter, size_t T>\nstruct wide_string_input_helper;\n\ntemplate<typename BaseInputAdapter>\nstruct wide_string_input_helper<BaseInputAdapter, 4>\n{\n    // UTF-32\n    static void fill_buffer(BaseInputAdapter& input,\n                            std::array<std::char_traits<char>::int_type, 4>& utf8_bytes,\n                            size_t& utf8_bytes_index,\n                            size_t& utf8_bytes_filled)\n    {\n        utf8_bytes_index = 0;\n\n        if (JSON_HEDLEY_UNLIKELY(input.empty()))\n        {\n            utf8_bytes[0] = std::char_traits<char>::eof();\n            utf8_bytes_filled = 1;\n        }\n        else\n        {\n            // get the current character\n            const auto wc = input.get_character();\n\n            // UTF-32 to UTF-8 encoding\n            if (wc < 0x80)\n            {\n                utf8_bytes[0] = static_cast<std::char_traits<char>::int_type>(wc);\n                utf8_bytes_filled = 1;\n            }\n            else if (wc <= 0x7FF)\n            {\n                utf8_bytes[0] = static_cast<std::char_traits<char>::int_type>(0xC0u | ((static_cast<unsigned int>(wc) >> 6u) & 0x1Fu));\n                utf8_bytes[1] = static_cast<std::char_traits<char>::int_type>(0x80u | (static_cast<unsigned int>(wc) & 0x3Fu));\n                utf8_bytes_filled = 2;\n            }\n            else if (wc <= 0xFFFF)\n            {\n                utf8_bytes[0] = static_cast<std::char_traits<char>::int_type>(0xE0u | ((static_cast<unsigned int>(wc) >> 12u) & 0x0Fu));\n                utf8_bytes[1] = static_cast<std::char_traits<char>::int_type>(0x80u | ((static_cast<unsigned int>(wc) >> 6u) & 0x3Fu));\n                utf8_bytes[2] = static_cast<std::char_traits<char>::int_type>(0x80u | (static_cast<unsigned int>(wc) & 0x3Fu));\n                utf8_bytes_filled = 3;\n            }\n            else if (wc <= 0x10FFFF)\n            {\n                utf8_bytes[0] = static_cast<std::char_traits<char>::int_type>(0xF0u | ((static_cast<unsigned int>(wc) >> 18u) & 0x07u));\n                utf8_bytes[1] = static_cast<std::char_traits<char>::int_type>(0x80u | ((static_cast<unsigned int>(wc) >> 12u) & 0x3Fu));\n                utf8_bytes[2] = static_cast<std::char_traits<char>::int_type>(0x80u | ((static_cast<unsigned int>(wc) >> 6u) & 0x3Fu));\n                utf8_bytes[3] = static_cast<std::char_traits<char>::int_type>(0x80u | (static_cast<unsigned int>(wc) & 0x3Fu));\n                utf8_bytes_filled = 4;\n            }\n            else\n            {\n                // unknown character\n                utf8_bytes[0] = static_cast<std::char_traits<char>::int_type>(wc);\n                utf8_bytes_filled = 1;\n            }\n        }\n    }\n};\n\ntemplate<typename BaseInputAdapter>\nstruct wide_string_input_helper<BaseInputAdapter, 2>\n{\n    // UTF-16\n    static void fill_buffer(BaseInputAdapter& input,\n                            std::array<std::char_traits<char>::int_type, 4>& utf8_bytes,\n                            size_t& utf8_bytes_index,\n                            size_t& utf8_bytes_filled)\n    {\n        utf8_bytes_index = 0;\n\n        if (JSON_HEDLEY_UNLIKELY(input.empty()))\n        {\n            utf8_bytes[0] = std::char_traits<char>::eof();\n            utf8_bytes_filled = 1;\n        }\n        else\n        {\n            // get the current character\n            const auto wc = input.get_character();\n\n            // UTF-16 to UTF-8 encoding\n            if (wc < 0x80)\n            {\n                utf8_bytes[0] = static_cast<std::char_traits<char>::int_type>(wc);\n                utf8_bytes_filled = 1;\n            }\n            else if (wc <= 0x7FF)\n            {\n                utf8_bytes[0] = static_cast<std::char_traits<char>::int_type>(0xC0u | ((static_cast<unsigned int>(wc) >> 6u)));\n                utf8_bytes[1] = static_cast<std::char_traits<char>::int_type>(0x80u | (static_cast<unsigned int>(wc) & 0x3Fu));\n                utf8_bytes_filled = 2;\n            }\n            else if (0xD800 > wc || wc >= 0xE000)\n            {\n                utf8_bytes[0] = static_cast<std::char_traits<char>::int_type>(0xE0u | ((static_cast<unsigned int>(wc) >> 12u)));\n                utf8_bytes[1] = static_cast<std::char_traits<char>::int_type>(0x80u | ((static_cast<unsigned int>(wc) >> 6u) & 0x3Fu));\n                utf8_bytes[2] = static_cast<std::char_traits<char>::int_type>(0x80u | (static_cast<unsigned int>(wc) & 0x3Fu));\n                utf8_bytes_filled = 3;\n            }\n            else\n            {\n                if (JSON_HEDLEY_UNLIKELY(!input.empty()))\n                {\n                    const auto wc2 = static_cast<unsigned int>(input.get_character());\n                    const auto charcode = 0x10000u + (((static_cast<unsigned int>(wc) & 0x3FFu) << 10u) | (wc2 & 0x3FFu));\n                    utf8_bytes[0] = static_cast<std::char_traits<char>::int_type>(0xF0u | (charcode >> 18u));\n                    utf8_bytes[1] = static_cast<std::char_traits<char>::int_type>(0x80u | ((charcode >> 12u) & 0x3Fu));\n                    utf8_bytes[2] = static_cast<std::char_traits<char>::int_type>(0x80u | ((charcode >> 6u) & 0x3Fu));\n                    utf8_bytes[3] = static_cast<std::char_traits<char>::int_type>(0x80u | (charcode & 0x3Fu));\n                    utf8_bytes_filled = 4;\n                }\n                else\n                {\n                    utf8_bytes[0] = static_cast<std::char_traits<char>::int_type>(wc);\n                    utf8_bytes_filled = 1;\n                }\n            }\n        }\n    }\n};\n\n// Wraps another input apdater to convert wide character types into individual bytes.\ntemplate<typename BaseInputAdapter, typename WideCharType>\nclass wide_string_input_adapter\n{\n  public:\n    using char_type = char;\n\n    wide_string_input_adapter(BaseInputAdapter base)\n        : base_adapter(base) {}\n\n    typename std::char_traits<char>::int_type get_character() noexcept\n    {\n        // check if buffer needs to be filled\n        if (utf8_bytes_index == utf8_bytes_filled)\n        {\n            fill_buffer<sizeof(WideCharType)>();\n\n            JSON_ASSERT(utf8_bytes_filled > 0);\n            JSON_ASSERT(utf8_bytes_index == 0);\n        }\n\n        // use buffer\n        JSON_ASSERT(utf8_bytes_filled > 0);\n        JSON_ASSERT(utf8_bytes_index < utf8_bytes_filled);\n        return utf8_bytes[utf8_bytes_index++];\n    }\n\n  private:\n    BaseInputAdapter base_adapter;\n\n    template<size_t T>\n    void fill_buffer()\n    {\n        wide_string_input_helper<BaseInputAdapter, T>::fill_buffer(base_adapter, utf8_bytes, utf8_bytes_index, utf8_bytes_filled);\n    }\n\n    /// a buffer for UTF-8 bytes\n    std::array<std::char_traits<char>::int_type, 4> utf8_bytes = {{0, 0, 0, 0}};\n\n    /// index to the utf8_codes array for the next valid byte\n    std::size_t utf8_bytes_index = 0;\n    /// number of valid bytes in the utf8_codes array\n    std::size_t utf8_bytes_filled = 0;\n};\n\n\ntemplate<typename IteratorType, typename Enable = void>\nstruct iterator_input_adapter_factory\n{\n    using iterator_type = IteratorType;\n    using char_type = typename std::iterator_traits<iterator_type>::value_type;\n    using adapter_type = iterator_input_adapter<iterator_type>;\n\n    static adapter_type create(IteratorType first, IteratorType last)\n    {\n        return adapter_type(std::move(first), std::move(last));\n    }\n};\n\ntemplate<typename T>\nstruct is_iterator_of_multibyte\n{\n    using value_type = typename std::iterator_traits<T>::value_type;\n    enum\n    {\n        value = sizeof(value_type) > 1\n    };\n};\n\ntemplate<typename IteratorType>\nstruct iterator_input_adapter_factory<IteratorType, enable_if_t<is_iterator_of_multibyte<IteratorType>::value>>\n{\n    using iterator_type = IteratorType;\n    using char_type = typename std::iterator_traits<iterator_type>::value_type;\n    using base_adapter_type = iterator_input_adapter<iterator_type>;\n    using adapter_type = wide_string_input_adapter<base_adapter_type, char_type>;\n\n    static adapter_type create(IteratorType first, IteratorType last)\n    {\n        return adapter_type(base_adapter_type(std::move(first), std::move(last)));\n    }\n};\n\n// General purpose iterator-based input\ntemplate<typename IteratorType>\ntypename iterator_input_adapter_factory<IteratorType>::adapter_type input_adapter(IteratorType first, IteratorType last)\n{\n    using factory_type = iterator_input_adapter_factory<IteratorType>;\n    return factory_type::create(first, last);\n}\n\n// Convenience shorthand from container to iterator\n// Enables ADL on begin(container) and end(container)\n// Encloses the using declarations in namespace for not to leak them to outside scope\n\nnamespace container_input_adapter_factory_impl\n{\n\nusing std::begin;\nusing std::end;\n\ntemplate<typename ContainerType, typename Enable = void>\nstruct container_input_adapter_factory {};\n\ntemplate<typename ContainerType>\nstruct container_input_adapter_factory< ContainerType,\n       void_t<decltype(begin(std::declval<ContainerType>()), end(std::declval<ContainerType>()))>>\n       {\n           using adapter_type = decltype(input_adapter(begin(std::declval<ContainerType>()), end(std::declval<ContainerType>())));\n\n           static adapter_type create(const ContainerType& container)\n{\n    return input_adapter(begin(container), end(container));\n}\n       };\n\n}  // namespace container_input_adapter_factory_impl\n\ntemplate<typename ContainerType>\ntypename container_input_adapter_factory_impl::container_input_adapter_factory<ContainerType>::adapter_type input_adapter(const ContainerType& container)\n{\n    return container_input_adapter_factory_impl::container_input_adapter_factory<ContainerType>::create(container);\n}\n\n#ifndef JSON_NO_IO\n// Special cases with fast paths\ninline file_input_adapter input_adapter(std::FILE* file)\n{\n    return file_input_adapter(file);\n}\n\ninline input_stream_adapter input_adapter(std::istream& stream)\n{\n    return input_stream_adapter(stream);\n}\n\ninline input_stream_adapter input_adapter(std::istream&& stream)\n{\n    return input_stream_adapter(stream);\n}\n#endif  // JSON_NO_IO\n\nusing contiguous_bytes_input_adapter = decltype(input_adapter(std::declval<const char*>(), std::declval<const char*>()));\n\n// Null-delimited strings, and the like.\ntemplate < typename CharT,\n           typename std::enable_if <\n               std::is_pointer<CharT>::value&&\n               !std::is_array<CharT>::value&&\n               std::is_integral<typename std::remove_pointer<CharT>::type>::value&&\n               sizeof(typename std::remove_pointer<CharT>::type) == 1,\n               int >::type = 0 >\ncontiguous_bytes_input_adapter input_adapter(CharT b)\n{\n    auto length = std::strlen(reinterpret_cast<const char*>(b));\n    const auto* ptr = reinterpret_cast<const char*>(b);\n    return input_adapter(ptr, ptr + length);\n}\n\ntemplate<typename T, std::size_t N>\nauto input_adapter(T (&array)[N]) -> decltype(input_adapter(array, array + N)) // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays)\n{\n    return input_adapter(array, array + N);\n}\n\n// This class only handles inputs of input_buffer_adapter type.\n// It's required so that expressions like {ptr, len} can be implicitly cast\n// to the correct adapter.\nclass span_input_adapter\n{\n  public:\n    template < typename CharT,\n               typename std::enable_if <\n                   std::is_pointer<CharT>::value&&\n                   std::is_integral<typename std::remove_pointer<CharT>::type>::value&&\n                   sizeof(typename std::remove_pointer<CharT>::type) == 1,\n                   int >::type = 0 >\n    span_input_adapter(CharT b, std::size_t l)\n        : ia(reinterpret_cast<const char*>(b), reinterpret_cast<const char*>(b) + l) {}\n\n    template<class IteratorType,\n             typename std::enable_if<\n                 std::is_same<typename iterator_traits<IteratorType>::iterator_category, std::random_access_iterator_tag>::value,\n                 int>::type = 0>\n    span_input_adapter(IteratorType first, IteratorType last)\n        : ia(input_adapter(first, last)) {}\n\n    contiguous_bytes_input_adapter&& get()\n    {\n        return std::move(ia); // NOLINT(hicpp-move-const-arg,performance-move-const-arg)\n    }\n\n  private:\n    contiguous_bytes_input_adapter ia;\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/input/json_sax.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <cstddef>\n#include <string> // string\n#include <utility> // move\n#include <vector> // vector\n\n// #include <nlohmann/detail/exceptions.hpp>\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/string_concat.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\n\n/*!\n@brief SAX interface\n\nThis class describes the SAX interface used by @ref nlohmann::json::sax_parse.\nEach function is called in different situations while the input is parsed. The\nboolean return value informs the parser whether to continue processing the\ninput.\n*/\ntemplate<typename BasicJsonType>\nstruct json_sax\n{\n    using number_integer_t = typename BasicJsonType::number_integer_t;\n    using number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n    using string_t = typename BasicJsonType::string_t;\n    using binary_t = typename BasicJsonType::binary_t;\n\n    /*!\n    @brief a null value was read\n    @return whether parsing should proceed\n    */\n    virtual bool null() = 0;\n\n    /*!\n    @brief a boolean value was read\n    @param[in] val  boolean value\n    @return whether parsing should proceed\n    */\n    virtual bool boolean(bool val) = 0;\n\n    /*!\n    @brief an integer number was read\n    @param[in] val  integer value\n    @return whether parsing should proceed\n    */\n    virtual bool number_integer(number_integer_t val) = 0;\n\n    /*!\n    @brief an unsigned integer number was read\n    @param[in] val  unsigned integer value\n    @return whether parsing should proceed\n    */\n    virtual bool number_unsigned(number_unsigned_t val) = 0;\n\n    /*!\n    @brief a floating-point number was read\n    @param[in] val  floating-point value\n    @param[in] s    raw token value\n    @return whether parsing should proceed\n    */\n    virtual bool number_float(number_float_t val, const string_t& s) = 0;\n\n    /*!\n    @brief a string value was read\n    @param[in] val  string value\n    @return whether parsing should proceed\n    @note It is safe to move the passed string value.\n    */\n    virtual bool string(string_t& val) = 0;\n\n    /*!\n    @brief a binary value was read\n    @param[in] val  binary value\n    @return whether parsing should proceed\n    @note It is safe to move the passed binary value.\n    */\n    virtual bool binary(binary_t& val) = 0;\n\n    /*!\n    @brief the beginning of an object was read\n    @param[in] elements  number of object elements or -1 if unknown\n    @return whether parsing should proceed\n    @note binary formats may report the number of elements\n    */\n    virtual bool start_object(std::size_t elements) = 0;\n\n    /*!\n    @brief an object key was read\n    @param[in] val  object key\n    @return whether parsing should proceed\n    @note It is safe to move the passed string.\n    */\n    virtual bool key(string_t& val) = 0;\n\n    /*!\n    @brief the end of an object was read\n    @return whether parsing should proceed\n    */\n    virtual bool end_object() = 0;\n\n    /*!\n    @brief the beginning of an array was read\n    @param[in] elements  number of array elements or -1 if unknown\n    @return whether parsing should proceed\n    @note binary formats may report the number of elements\n    */\n    virtual bool start_array(std::size_t elements) = 0;\n\n    /*!\n    @brief the end of an array was read\n    @return whether parsing should proceed\n    */\n    virtual bool end_array() = 0;\n\n    /*!\n    @brief a parse error occurred\n    @param[in] position    the position in the input where the error occurs\n    @param[in] last_token  the last read token\n    @param[in] ex          an exception object describing the error\n    @return whether parsing should proceed (must return false)\n    */\n    virtual bool parse_error(std::size_t position,\n                             const std::string& last_token,\n                             const detail::exception& ex) = 0;\n\n    json_sax() = default;\n    json_sax(const json_sax&) = default;\n    json_sax(json_sax&&) noexcept = default;\n    json_sax& operator=(const json_sax&) = default;\n    json_sax& operator=(json_sax&&) noexcept = default;\n    virtual ~json_sax() = default;\n};\n\n\nnamespace detail\n{\n/*!\n@brief SAX implementation to create a JSON value from SAX events\n\nThis class implements the @ref json_sax interface and processes the SAX events\nto create a JSON value which makes it basically a DOM parser. The structure or\nhierarchy of the JSON value is managed by the stack `ref_stack` which contains\na pointer to the respective array or object for each recursion depth.\n\nAfter successful parsing, the value that is passed by reference to the\nconstructor contains the parsed value.\n\n@tparam BasicJsonType  the JSON type\n*/\ntemplate<typename BasicJsonType>\nclass json_sax_dom_parser\n{\n  public:\n    using number_integer_t = typename BasicJsonType::number_integer_t;\n    using number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n    using string_t = typename BasicJsonType::string_t;\n    using binary_t = typename BasicJsonType::binary_t;\n\n    /*!\n    @param[in,out] r  reference to a JSON value that is manipulated while\n                       parsing\n    @param[in] allow_exceptions_  whether parse errors yield exceptions\n    */\n    explicit json_sax_dom_parser(BasicJsonType& r, const bool allow_exceptions_ = true)\n        : root(r), allow_exceptions(allow_exceptions_)\n    {}\n\n    // make class move-only\n    json_sax_dom_parser(const json_sax_dom_parser&) = delete;\n    json_sax_dom_parser(json_sax_dom_parser&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor)\n    json_sax_dom_parser& operator=(const json_sax_dom_parser&) = delete;\n    json_sax_dom_parser& operator=(json_sax_dom_parser&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor)\n    ~json_sax_dom_parser() = default;\n\n    bool null()\n    {\n        handle_value(nullptr);\n        return true;\n    }\n\n    bool boolean(bool val)\n    {\n        handle_value(val);\n        return true;\n    }\n\n    bool number_integer(number_integer_t val)\n    {\n        handle_value(val);\n        return true;\n    }\n\n    bool number_unsigned(number_unsigned_t val)\n    {\n        handle_value(val);\n        return true;\n    }\n\n    bool number_float(number_float_t val, const string_t& /*unused*/)\n    {\n        handle_value(val);\n        return true;\n    }\n\n    bool string(string_t& val)\n    {\n        handle_value(val);\n        return true;\n    }\n\n    bool binary(binary_t& val)\n    {\n        handle_value(std::move(val));\n        return true;\n    }\n\n    bool start_object(std::size_t len)\n    {\n        ref_stack.push_back(handle_value(BasicJsonType::value_t::object));\n\n        if (JSON_HEDLEY_UNLIKELY(len != static_cast<std::size_t>(-1) && len > ref_stack.back()->max_size()))\n        {\n            JSON_THROW(out_of_range::create(408, concat(\"excessive object size: \", std::to_string(len)), ref_stack.back()));\n        }\n\n        return true;\n    }\n\n    bool key(string_t& val)\n    {\n        JSON_ASSERT(!ref_stack.empty());\n        JSON_ASSERT(ref_stack.back()->is_object());\n\n        // add null at given key and store the reference for later\n        object_element = &(ref_stack.back()->m_value.object->operator[](val));\n        return true;\n    }\n\n    bool end_object()\n    {\n        JSON_ASSERT(!ref_stack.empty());\n        JSON_ASSERT(ref_stack.back()->is_object());\n\n        ref_stack.back()->set_parents();\n        ref_stack.pop_back();\n        return true;\n    }\n\n    bool start_array(std::size_t len)\n    {\n        ref_stack.push_back(handle_value(BasicJsonType::value_t::array));\n\n        if (JSON_HEDLEY_UNLIKELY(len != static_cast<std::size_t>(-1) && len > ref_stack.back()->max_size()))\n        {\n            JSON_THROW(out_of_range::create(408, concat(\"excessive array size: \", std::to_string(len)), ref_stack.back()));\n        }\n\n        return true;\n    }\n\n    bool end_array()\n    {\n        JSON_ASSERT(!ref_stack.empty());\n        JSON_ASSERT(ref_stack.back()->is_array());\n\n        ref_stack.back()->set_parents();\n        ref_stack.pop_back();\n        return true;\n    }\n\n    template<class Exception>\n    bool parse_error(std::size_t /*unused*/, const std::string& /*unused*/,\n                     const Exception& ex)\n    {\n        errored = true;\n        static_cast<void>(ex);\n        if (allow_exceptions)\n        {\n            JSON_THROW(ex);\n        }\n        return false;\n    }\n\n    constexpr bool is_errored() const\n    {\n        return errored;\n    }\n\n  private:\n    /*!\n    @invariant If the ref stack is empty, then the passed value will be the new\n               root.\n    @invariant If the ref stack contains a value, then it is an array or an\n               object to which we can add elements\n    */\n    template<typename Value>\n    JSON_HEDLEY_RETURNS_NON_NULL\n    BasicJsonType* handle_value(Value&& v)\n    {\n        if (ref_stack.empty())\n        {\n            root = BasicJsonType(std::forward<Value>(v));\n            return &root;\n        }\n\n        JSON_ASSERT(ref_stack.back()->is_array() || ref_stack.back()->is_object());\n\n        if (ref_stack.back()->is_array())\n        {\n            ref_stack.back()->m_value.array->emplace_back(std::forward<Value>(v));\n            return &(ref_stack.back()->m_value.array->back());\n        }\n\n        JSON_ASSERT(ref_stack.back()->is_object());\n        JSON_ASSERT(object_element);\n        *object_element = BasicJsonType(std::forward<Value>(v));\n        return object_element;\n    }\n\n    /// the parsed JSON value\n    BasicJsonType& root;\n    /// stack to model hierarchy of values\n    std::vector<BasicJsonType*> ref_stack {};\n    /// helper to hold the reference for the next object element\n    BasicJsonType* object_element = nullptr;\n    /// whether a syntax error occurred\n    bool errored = false;\n    /// whether to throw exceptions in case of errors\n    const bool allow_exceptions = true;\n};\n\ntemplate<typename BasicJsonType>\nclass json_sax_dom_callback_parser\n{\n  public:\n    using number_integer_t = typename BasicJsonType::number_integer_t;\n    using number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n    using string_t = typename BasicJsonType::string_t;\n    using binary_t = typename BasicJsonType::binary_t;\n    using parser_callback_t = typename BasicJsonType::parser_callback_t;\n    using parse_event_t = typename BasicJsonType::parse_event_t;\n\n    json_sax_dom_callback_parser(BasicJsonType& r,\n                                 const parser_callback_t cb,\n                                 const bool allow_exceptions_ = true)\n        : root(r), callback(cb), allow_exceptions(allow_exceptions_)\n    {\n        keep_stack.push_back(true);\n    }\n\n    // make class move-only\n    json_sax_dom_callback_parser(const json_sax_dom_callback_parser&) = delete;\n    json_sax_dom_callback_parser(json_sax_dom_callback_parser&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor)\n    json_sax_dom_callback_parser& operator=(const json_sax_dom_callback_parser&) = delete;\n    json_sax_dom_callback_parser& operator=(json_sax_dom_callback_parser&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor)\n    ~json_sax_dom_callback_parser() = default;\n\n    bool null()\n    {\n        handle_value(nullptr);\n        return true;\n    }\n\n    bool boolean(bool val)\n    {\n        handle_value(val);\n        return true;\n    }\n\n    bool number_integer(number_integer_t val)\n    {\n        handle_value(val);\n        return true;\n    }\n\n    bool number_unsigned(number_unsigned_t val)\n    {\n        handle_value(val);\n        return true;\n    }\n\n    bool number_float(number_float_t val, const string_t& /*unused*/)\n    {\n        handle_value(val);\n        return true;\n    }\n\n    bool string(string_t& val)\n    {\n        handle_value(val);\n        return true;\n    }\n\n    bool binary(binary_t& val)\n    {\n        handle_value(std::move(val));\n        return true;\n    }\n\n    bool start_object(std::size_t len)\n    {\n        // check callback for object start\n        const bool keep = callback(static_cast<int>(ref_stack.size()), parse_event_t::object_start, discarded);\n        keep_stack.push_back(keep);\n\n        auto val = handle_value(BasicJsonType::value_t::object, true);\n        ref_stack.push_back(val.second);\n\n        // check object limit\n        if (ref_stack.back() && JSON_HEDLEY_UNLIKELY(len != static_cast<std::size_t>(-1) && len > ref_stack.back()->max_size()))\n        {\n            JSON_THROW(out_of_range::create(408, concat(\"excessive object size: \", std::to_string(len)), ref_stack.back()));\n        }\n\n        return true;\n    }\n\n    bool key(string_t& val)\n    {\n        BasicJsonType k = BasicJsonType(val);\n\n        // check callback for key\n        const bool keep = callback(static_cast<int>(ref_stack.size()), parse_event_t::key, k);\n        key_keep_stack.push_back(keep);\n\n        // add discarded value at given key and store the reference for later\n        if (keep && ref_stack.back())\n        {\n            object_element = &(ref_stack.back()->m_value.object->operator[](val) = discarded);\n        }\n\n        return true;\n    }\n\n    bool end_object()\n    {\n        if (ref_stack.back())\n        {\n            if (!callback(static_cast<int>(ref_stack.size()) - 1, parse_event_t::object_end, *ref_stack.back()))\n            {\n                // discard object\n                *ref_stack.back() = discarded;\n            }\n            else\n            {\n                ref_stack.back()->set_parents();\n            }\n        }\n\n        JSON_ASSERT(!ref_stack.empty());\n        JSON_ASSERT(!keep_stack.empty());\n        ref_stack.pop_back();\n        keep_stack.pop_back();\n\n        if (!ref_stack.empty() && ref_stack.back() && ref_stack.back()->is_structured())\n        {\n            // remove discarded value\n            for (auto it = ref_stack.back()->begin(); it != ref_stack.back()->end(); ++it)\n            {\n                if (it->is_discarded())\n                {\n                    ref_stack.back()->erase(it);\n                    break;\n                }\n            }\n        }\n\n        return true;\n    }\n\n    bool start_array(std::size_t len)\n    {\n        const bool keep = callback(static_cast<int>(ref_stack.size()), parse_event_t::array_start, discarded);\n        keep_stack.push_back(keep);\n\n        auto val = handle_value(BasicJsonType::value_t::array, true);\n        ref_stack.push_back(val.second);\n\n        // check array limit\n        if (ref_stack.back() && JSON_HEDLEY_UNLIKELY(len != static_cast<std::size_t>(-1) && len > ref_stack.back()->max_size()))\n        {\n            JSON_THROW(out_of_range::create(408, concat(\"excessive array size: \", std::to_string(len)), ref_stack.back()));\n        }\n\n        return true;\n    }\n\n    bool end_array()\n    {\n        bool keep = true;\n\n        if (ref_stack.back())\n        {\n            keep = callback(static_cast<int>(ref_stack.size()) - 1, parse_event_t::array_end, *ref_stack.back());\n            if (keep)\n            {\n                ref_stack.back()->set_parents();\n            }\n            else\n            {\n                // discard array\n                *ref_stack.back() = discarded;\n            }\n        }\n\n        JSON_ASSERT(!ref_stack.empty());\n        JSON_ASSERT(!keep_stack.empty());\n        ref_stack.pop_back();\n        keep_stack.pop_back();\n\n        // remove discarded value\n        if (!keep && !ref_stack.empty() && ref_stack.back()->is_array())\n        {\n            ref_stack.back()->m_value.array->pop_back();\n        }\n\n        return true;\n    }\n\n    template<class Exception>\n    bool parse_error(std::size_t /*unused*/, const std::string& /*unused*/,\n                     const Exception& ex)\n    {\n        errored = true;\n        static_cast<void>(ex);\n        if (allow_exceptions)\n        {\n            JSON_THROW(ex);\n        }\n        return false;\n    }\n\n    constexpr bool is_errored() const\n    {\n        return errored;\n    }\n\n  private:\n    /*!\n    @param[in] v  value to add to the JSON value we build during parsing\n    @param[in] skip_callback  whether we should skip calling the callback\n               function; this is required after start_array() and\n               start_object() SAX events, because otherwise we would call the\n               callback function with an empty array or object, respectively.\n\n    @invariant If the ref stack is empty, then the passed value will be the new\n               root.\n    @invariant If the ref stack contains a value, then it is an array or an\n               object to which we can add elements\n\n    @return pair of boolean (whether value should be kept) and pointer (to the\n            passed value in the ref_stack hierarchy; nullptr if not kept)\n    */\n    template<typename Value>\n    std::pair<bool, BasicJsonType*> handle_value(Value&& v, const bool skip_callback = false)\n    {\n        JSON_ASSERT(!keep_stack.empty());\n\n        // do not handle this value if we know it would be added to a discarded\n        // container\n        if (!keep_stack.back())\n        {\n            return {false, nullptr};\n        }\n\n        // create value\n        auto value = BasicJsonType(std::forward<Value>(v));\n\n        // check callback\n        const bool keep = skip_callback || callback(static_cast<int>(ref_stack.size()), parse_event_t::value, value);\n\n        // do not handle this value if we just learnt it shall be discarded\n        if (!keep)\n        {\n            return {false, nullptr};\n        }\n\n        if (ref_stack.empty())\n        {\n            root = std::move(value);\n            return {true, &root};\n        }\n\n        // skip this value if we already decided to skip the parent\n        // (https://github.com/nlohmann/json/issues/971#issuecomment-413678360)\n        if (!ref_stack.back())\n        {\n            return {false, nullptr};\n        }\n\n        // we now only expect arrays and objects\n        JSON_ASSERT(ref_stack.back()->is_array() || ref_stack.back()->is_object());\n\n        // array\n        if (ref_stack.back()->is_array())\n        {\n            ref_stack.back()->m_value.array->emplace_back(std::move(value));\n            return {true, &(ref_stack.back()->m_value.array->back())};\n        }\n\n        // object\n        JSON_ASSERT(ref_stack.back()->is_object());\n        // check if we should store an element for the current key\n        JSON_ASSERT(!key_keep_stack.empty());\n        const bool store_element = key_keep_stack.back();\n        key_keep_stack.pop_back();\n\n        if (!store_element)\n        {\n            return {false, nullptr};\n        }\n\n        JSON_ASSERT(object_element);\n        *object_element = std::move(value);\n        return {true, object_element};\n    }\n\n    /// the parsed JSON value\n    BasicJsonType& root;\n    /// stack to model hierarchy of values\n    std::vector<BasicJsonType*> ref_stack {};\n    /// stack to manage which values to keep\n    std::vector<bool> keep_stack {};\n    /// stack to manage which object keys to keep\n    std::vector<bool> key_keep_stack {};\n    /// helper to hold the reference for the next object element\n    BasicJsonType* object_element = nullptr;\n    /// whether a syntax error occurred\n    bool errored = false;\n    /// callback function\n    const parser_callback_t callback = nullptr;\n    /// whether to throw exceptions in case of errors\n    const bool allow_exceptions = true;\n    /// a discarded value for the callback\n    BasicJsonType discarded = BasicJsonType::value_t::discarded;\n};\n\ntemplate<typename BasicJsonType>\nclass json_sax_acceptor\n{\n  public:\n    using number_integer_t = typename BasicJsonType::number_integer_t;\n    using number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n    using string_t = typename BasicJsonType::string_t;\n    using binary_t = typename BasicJsonType::binary_t;\n\n    bool null()\n    {\n        return true;\n    }\n\n    bool boolean(bool /*unused*/)\n    {\n        return true;\n    }\n\n    bool number_integer(number_integer_t /*unused*/)\n    {\n        return true;\n    }\n\n    bool number_unsigned(number_unsigned_t /*unused*/)\n    {\n        return true;\n    }\n\n    bool number_float(number_float_t /*unused*/, const string_t& /*unused*/)\n    {\n        return true;\n    }\n\n    bool string(string_t& /*unused*/)\n    {\n        return true;\n    }\n\n    bool binary(binary_t& /*unused*/)\n    {\n        return true;\n    }\n\n    bool start_object(std::size_t /*unused*/ = static_cast<std::size_t>(-1))\n    {\n        return true;\n    }\n\n    bool key(string_t& /*unused*/)\n    {\n        return true;\n    }\n\n    bool end_object()\n    {\n        return true;\n    }\n\n    bool start_array(std::size_t /*unused*/ = static_cast<std::size_t>(-1))\n    {\n        return true;\n    }\n\n    bool end_array()\n    {\n        return true;\n    }\n\n    bool parse_error(std::size_t /*unused*/, const std::string& /*unused*/, const detail::exception& /*unused*/)\n    {\n        return false;\n    }\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/input/lexer.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <array> // array\n#include <clocale> // localeconv\n#include <cstddef> // size_t\n#include <cstdio> // snprintf\n#include <cstdlib> // strtof, strtod, strtold, strtoll, strtoull\n#include <initializer_list> // initializer_list\n#include <string> // char_traits, string\n#include <utility> // move\n#include <vector> // vector\n\n// #include <nlohmann/detail/input/input_adapters.hpp>\n\n// #include <nlohmann/detail/input/position_t.hpp>\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n///////////\n// lexer //\n///////////\n\ntemplate<typename BasicJsonType>\nclass lexer_base\n{\n  public:\n    /// token types for the parser\n    enum class token_type\n    {\n        uninitialized,    ///< indicating the scanner is uninitialized\n        literal_true,     ///< the `true` literal\n        literal_false,    ///< the `false` literal\n        literal_null,     ///< the `null` literal\n        value_string,     ///< a string -- use get_string() for actual value\n        value_unsigned,   ///< an unsigned integer -- use get_number_unsigned() for actual value\n        value_integer,    ///< a signed integer -- use get_number_integer() for actual value\n        value_float,      ///< an floating point number -- use get_number_float() for actual value\n        begin_array,      ///< the character for array begin `[`\n        begin_object,     ///< the character for object begin `{`\n        end_array,        ///< the character for array end `]`\n        end_object,       ///< the character for object end `}`\n        name_separator,   ///< the name separator `:`\n        value_separator,  ///< the value separator `,`\n        parse_error,      ///< indicating a parse error\n        end_of_input,     ///< indicating the end of the input buffer\n        literal_or_value  ///< a literal or the begin of a value (only for diagnostics)\n    };\n\n    /// return name of values of type token_type (only used for errors)\n    JSON_HEDLEY_RETURNS_NON_NULL\n    JSON_HEDLEY_CONST\n    static const char* token_type_name(const token_type t) noexcept\n    {\n        switch (t)\n        {\n            case token_type::uninitialized:\n                return \"<uninitialized>\";\n            case token_type::literal_true:\n                return \"true literal\";\n            case token_type::literal_false:\n                return \"false literal\";\n            case token_type::literal_null:\n                return \"null literal\";\n            case token_type::value_string:\n                return \"string literal\";\n            case token_type::value_unsigned:\n            case token_type::value_integer:\n            case token_type::value_float:\n                return \"number literal\";\n            case token_type::begin_array:\n                return \"'['\";\n            case token_type::begin_object:\n                return \"'{'\";\n            case token_type::end_array:\n                return \"']'\";\n            case token_type::end_object:\n                return \"'}'\";\n            case token_type::name_separator:\n                return \"':'\";\n            case token_type::value_separator:\n                return \"','\";\n            case token_type::parse_error:\n                return \"<parse error>\";\n            case token_type::end_of_input:\n                return \"end of input\";\n            case token_type::literal_or_value:\n                return \"'[', '{', or a literal\";\n            // LCOV_EXCL_START\n            default: // catch non-enum values\n                return \"unknown token\";\n                // LCOV_EXCL_STOP\n        }\n    }\n};\n/*!\n@brief lexical analysis\n\nThis class organizes the lexical analysis during JSON deserialization.\n*/\ntemplate<typename BasicJsonType, typename InputAdapterType>\nclass lexer : public lexer_base<BasicJsonType>\n{\n    using number_integer_t = typename BasicJsonType::number_integer_t;\n    using number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n    using string_t = typename BasicJsonType::string_t;\n    using char_type = typename InputAdapterType::char_type;\n    using char_int_type = typename std::char_traits<char_type>::int_type;\n\n  public:\n    using token_type = typename lexer_base<BasicJsonType>::token_type;\n\n    explicit lexer(InputAdapterType&& adapter, bool ignore_comments_ = false) noexcept\n        : ia(std::move(adapter))\n        , ignore_comments(ignore_comments_)\n        , decimal_point_char(static_cast<char_int_type>(get_decimal_point()))\n    {}\n\n    // delete because of pointer members\n    lexer(const lexer&) = delete;\n    lexer(lexer&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor)\n    lexer& operator=(lexer&) = delete;\n    lexer& operator=(lexer&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor)\n    ~lexer() = default;\n\n  private:\n    /////////////////////\n    // locales\n    /////////////////////\n\n    /// return the locale-dependent decimal point\n    JSON_HEDLEY_PURE\n    static char get_decimal_point() noexcept\n    {\n        const auto* loc = localeconv();\n        JSON_ASSERT(loc != nullptr);\n        return (loc->decimal_point == nullptr) ? '.' : *(loc->decimal_point);\n    }\n\n    /////////////////////\n    // scan functions\n    /////////////////////\n\n    /*!\n    @brief get codepoint from 4 hex characters following `\\u`\n\n    For input \"\\u c1 c2 c3 c4\" the codepoint is:\n      (c1 * 0x1000) + (c2 * 0x0100) + (c3 * 0x0010) + c4\n    = (c1 << 12) + (c2 << 8) + (c3 << 4) + (c4 << 0)\n\n    Furthermore, the possible characters '0'..'9', 'A'..'F', and 'a'..'f'\n    must be converted to the integers 0x0..0x9, 0xA..0xF, 0xA..0xF, resp. The\n    conversion is done by subtracting the offset (0x30, 0x37, and 0x57)\n    between the ASCII value of the character and the desired integer value.\n\n    @return codepoint (0x0000..0xFFFF) or -1 in case of an error (e.g. EOF or\n            non-hex character)\n    */\n    int get_codepoint()\n    {\n        // this function only makes sense after reading `\\u`\n        JSON_ASSERT(current == 'u');\n        int codepoint = 0;\n\n        const auto factors = { 12u, 8u, 4u, 0u };\n        for (const auto factor : factors)\n        {\n            get();\n\n            if (current >= '0' && current <= '9')\n            {\n                codepoint += static_cast<int>((static_cast<unsigned int>(current) - 0x30u) << factor);\n            }\n            else if (current >= 'A' && current <= 'F')\n            {\n                codepoint += static_cast<int>((static_cast<unsigned int>(current) - 0x37u) << factor);\n            }\n            else if (current >= 'a' && current <= 'f')\n            {\n                codepoint += static_cast<int>((static_cast<unsigned int>(current) - 0x57u) << factor);\n            }\n            else\n            {\n                return -1;\n            }\n        }\n\n        JSON_ASSERT(0x0000 <= codepoint && codepoint <= 0xFFFF);\n        return codepoint;\n    }\n\n    /*!\n    @brief check if the next byte(s) are inside a given range\n\n    Adds the current byte and, for each passed range, reads a new byte and\n    checks if it is inside the range. If a violation was detected, set up an\n    error message and return false. Otherwise, return true.\n\n    @param[in] ranges  list of integers; interpreted as list of pairs of\n                       inclusive lower and upper bound, respectively\n\n    @pre The passed list @a ranges must have 2, 4, or 6 elements; that is,\n         1, 2, or 3 pairs. This precondition is enforced by an assertion.\n\n    @return true if and only if no range violation was detected\n    */\n    bool next_byte_in_range(std::initializer_list<char_int_type> ranges)\n    {\n        JSON_ASSERT(ranges.size() == 2 || ranges.size() == 4 || ranges.size() == 6);\n        add(current);\n\n        for (auto range = ranges.begin(); range != ranges.end(); ++range)\n        {\n            get();\n            if (JSON_HEDLEY_LIKELY(*range <= current && current <= *(++range)))\n            {\n                add(current);\n            }\n            else\n            {\n                error_message = \"invalid string: ill-formed UTF-8 byte\";\n                return false;\n            }\n        }\n\n        return true;\n    }\n\n    /*!\n    @brief scan a string literal\n\n    This function scans a string according to Sect. 7 of RFC 8259. While\n    scanning, bytes are escaped and copied into buffer token_buffer. Then the\n    function returns successfully, token_buffer is *not* null-terminated (as it\n    may contain \\0 bytes), and token_buffer.size() is the number of bytes in the\n    string.\n\n    @return token_type::value_string if string could be successfully scanned,\n            token_type::parse_error otherwise\n\n    @note In case of errors, variable error_message contains a textual\n          description.\n    */\n    token_type scan_string()\n    {\n        // reset token_buffer (ignore opening quote)\n        reset();\n\n        // we entered the function by reading an open quote\n        JSON_ASSERT(current == '\\\"');\n\n        while (true)\n        {\n            // get next character\n            switch (get())\n            {\n                // end of file while parsing string\n                case std::char_traits<char_type>::eof():\n                {\n                    error_message = \"invalid string: missing closing quote\";\n                    return token_type::parse_error;\n                }\n\n                // closing quote\n                case '\\\"':\n                {\n                    return token_type::value_string;\n                }\n\n                // escapes\n                case '\\\\':\n                {\n                    switch (get())\n                    {\n                        // quotation mark\n                        case '\\\"':\n                            add('\\\"');\n                            break;\n                        // reverse solidus\n                        case '\\\\':\n                            add('\\\\');\n                            break;\n                        // solidus\n                        case '/':\n                            add('/');\n                            break;\n                        // backspace\n                        case 'b':\n                            add('\\b');\n                            break;\n                        // form feed\n                        case 'f':\n                            add('\\f');\n                            break;\n                        // line feed\n                        case 'n':\n                            add('\\n');\n                            break;\n                        // carriage return\n                        case 'r':\n                            add('\\r');\n                            break;\n                        // tab\n                        case 't':\n                            add('\\t');\n                            break;\n\n                        // unicode escapes\n                        case 'u':\n                        {\n                            const int codepoint1 = get_codepoint();\n                            int codepoint = codepoint1; // start with codepoint1\n\n                            if (JSON_HEDLEY_UNLIKELY(codepoint1 == -1))\n                            {\n                                error_message = \"invalid string: '\\\\u' must be followed by 4 hex digits\";\n                                return token_type::parse_error;\n                            }\n\n                            // check if code point is a high surrogate\n                            if (0xD800 <= codepoint1 && codepoint1 <= 0xDBFF)\n                            {\n                                // expect next \\uxxxx entry\n                                if (JSON_HEDLEY_LIKELY(get() == '\\\\' && get() == 'u'))\n                                {\n                                    const int codepoint2 = get_codepoint();\n\n                                    if (JSON_HEDLEY_UNLIKELY(codepoint2 == -1))\n                                    {\n                                        error_message = \"invalid string: '\\\\u' must be followed by 4 hex digits\";\n                                        return token_type::parse_error;\n                                    }\n\n                                    // check if codepoint2 is a low surrogate\n                                    if (JSON_HEDLEY_LIKELY(0xDC00 <= codepoint2 && codepoint2 <= 0xDFFF))\n                                    {\n                                        // overwrite codepoint\n                                        codepoint = static_cast<int>(\n                                                        // high surrogate occupies the most significant 22 bits\n                                                        (static_cast<unsigned int>(codepoint1) << 10u)\n                                                        // low surrogate occupies the least significant 15 bits\n                                                        + static_cast<unsigned int>(codepoint2)\n                                                        // there is still the 0xD800, 0xDC00 and 0x10000 noise\n                                                        // in the result, so we have to subtract with:\n                                                        // (0xD800 << 10) + DC00 - 0x10000 = 0x35FDC00\n                                                        - 0x35FDC00u);\n                                    }\n                                    else\n                                    {\n                                        error_message = \"invalid string: surrogate U+D800..U+DBFF must be followed by U+DC00..U+DFFF\";\n                                        return token_type::parse_error;\n                                    }\n                                }\n                                else\n                                {\n                                    error_message = \"invalid string: surrogate U+D800..U+DBFF must be followed by U+DC00..U+DFFF\";\n                                    return token_type::parse_error;\n                                }\n                            }\n                            else\n                            {\n                                if (JSON_HEDLEY_UNLIKELY(0xDC00 <= codepoint1 && codepoint1 <= 0xDFFF))\n                                {\n                                    error_message = \"invalid string: surrogate U+DC00..U+DFFF must follow U+D800..U+DBFF\";\n                                    return token_type::parse_error;\n                                }\n                            }\n\n                            // result of the above calculation yields a proper codepoint\n                            JSON_ASSERT(0x00 <= codepoint && codepoint <= 0x10FFFF);\n\n                            // translate codepoint into bytes\n                            if (codepoint < 0x80)\n                            {\n                                // 1-byte characters: 0xxxxxxx (ASCII)\n                                add(static_cast<char_int_type>(codepoint));\n                            }\n                            else if (codepoint <= 0x7FF)\n                            {\n                                // 2-byte characters: 110xxxxx 10xxxxxx\n                                add(static_cast<char_int_type>(0xC0u | (static_cast<unsigned int>(codepoint) >> 6u)));\n                                add(static_cast<char_int_type>(0x80u | (static_cast<unsigned int>(codepoint) & 0x3Fu)));\n                            }\n                            else if (codepoint <= 0xFFFF)\n                            {\n                                // 3-byte characters: 1110xxxx 10xxxxxx 10xxxxxx\n                                add(static_cast<char_int_type>(0xE0u | (static_cast<unsigned int>(codepoint) >> 12u)));\n                                add(static_cast<char_int_type>(0x80u | ((static_cast<unsigned int>(codepoint) >> 6u) & 0x3Fu)));\n                                add(static_cast<char_int_type>(0x80u | (static_cast<unsigned int>(codepoint) & 0x3Fu)));\n                            }\n                            else\n                            {\n                                // 4-byte characters: 11110xxx 10xxxxxx 10xxxxxx 10xxxxxx\n                                add(static_cast<char_int_type>(0xF0u | (static_cast<unsigned int>(codepoint) >> 18u)));\n                                add(static_cast<char_int_type>(0x80u | ((static_cast<unsigned int>(codepoint) >> 12u) & 0x3Fu)));\n                                add(static_cast<char_int_type>(0x80u | ((static_cast<unsigned int>(codepoint) >> 6u) & 0x3Fu)));\n                                add(static_cast<char_int_type>(0x80u | (static_cast<unsigned int>(codepoint) & 0x3Fu)));\n                            }\n\n                            break;\n                        }\n\n                        // other characters after escape\n                        default:\n                            error_message = \"invalid string: forbidden character after backslash\";\n                            return token_type::parse_error;\n                    }\n\n                    break;\n                }\n\n                // invalid control characters\n                case 0x00:\n                {\n                    error_message = \"invalid string: control character U+0000 (NUL) must be escaped to \\\\u0000\";\n                    return token_type::parse_error;\n                }\n\n                case 0x01:\n                {\n                    error_message = \"invalid string: control character U+0001 (SOH) must be escaped to \\\\u0001\";\n                    return token_type::parse_error;\n                }\n\n                case 0x02:\n                {\n                    error_message = \"invalid string: control character U+0002 (STX) must be escaped to \\\\u0002\";\n                    return token_type::parse_error;\n                }\n\n                case 0x03:\n                {\n                    error_message = \"invalid string: control character U+0003 (ETX) must be escaped to \\\\u0003\";\n                    return token_type::parse_error;\n                }\n\n                case 0x04:\n                {\n                    error_message = \"invalid string: control character U+0004 (EOT) must be escaped to \\\\u0004\";\n                    return token_type::parse_error;\n                }\n\n                case 0x05:\n                {\n                    error_message = \"invalid string: control character U+0005 (ENQ) must be escaped to \\\\u0005\";\n                    return token_type::parse_error;\n                }\n\n                case 0x06:\n                {\n                    error_message = \"invalid string: control character U+0006 (ACK) must be escaped to \\\\u0006\";\n                    return token_type::parse_error;\n                }\n\n                case 0x07:\n                {\n                    error_message = \"invalid string: control character U+0007 (BEL) must be escaped to \\\\u0007\";\n                    return token_type::parse_error;\n                }\n\n                case 0x08:\n                {\n                    error_message = \"invalid string: control character U+0008 (BS) must be escaped to \\\\u0008 or \\\\b\";\n                    return token_type::parse_error;\n                }\n\n                case 0x09:\n                {\n                    error_message = \"invalid string: control character U+0009 (HT) must be escaped to \\\\u0009 or \\\\t\";\n                    return token_type::parse_error;\n                }\n\n                case 0x0A:\n                {\n                    error_message = \"invalid string: control character U+000A (LF) must be escaped to \\\\u000A or \\\\n\";\n                    return token_type::parse_error;\n                }\n\n                case 0x0B:\n                {\n                    error_message = \"invalid string: control character U+000B (VT) must be escaped to \\\\u000B\";\n                    return token_type::parse_error;\n                }\n\n                case 0x0C:\n                {\n                    error_message = \"invalid string: control character U+000C (FF) must be escaped to \\\\u000C or \\\\f\";\n                    return token_type::parse_error;\n                }\n\n                case 0x0D:\n                {\n                    error_message = \"invalid string: control character U+000D (CR) must be escaped to \\\\u000D or \\\\r\";\n                    return token_type::parse_error;\n                }\n\n                case 0x0E:\n                {\n                    error_message = \"invalid string: control character U+000E (SO) must be escaped to \\\\u000E\";\n                    return token_type::parse_error;\n                }\n\n                case 0x0F:\n                {\n                    error_message = \"invalid string: control character U+000F (SI) must be escaped to \\\\u000F\";\n                    return token_type::parse_error;\n                }\n\n                case 0x10:\n                {\n                    error_message = \"invalid string: control character U+0010 (DLE) must be escaped to \\\\u0010\";\n                    return token_type::parse_error;\n                }\n\n                case 0x11:\n                {\n                    error_message = \"invalid string: control character U+0011 (DC1) must be escaped to \\\\u0011\";\n                    return token_type::parse_error;\n                }\n\n                case 0x12:\n                {\n                    error_message = \"invalid string: control character U+0012 (DC2) must be escaped to \\\\u0012\";\n                    return token_type::parse_error;\n                }\n\n                case 0x13:\n                {\n                    error_message = \"invalid string: control character U+0013 (DC3) must be escaped to \\\\u0013\";\n                    return token_type::parse_error;\n                }\n\n                case 0x14:\n                {\n                    error_message = \"invalid string: control character U+0014 (DC4) must be escaped to \\\\u0014\";\n                    return token_type::parse_error;\n                }\n\n                case 0x15:\n                {\n                    error_message = \"invalid string: control character U+0015 (NAK) must be escaped to \\\\u0015\";\n                    return token_type::parse_error;\n                }\n\n                case 0x16:\n                {\n                    error_message = \"invalid string: control character U+0016 (SYN) must be escaped to \\\\u0016\";\n                    return token_type::parse_error;\n                }\n\n                case 0x17:\n                {\n                    error_message = \"invalid string: control character U+0017 (ETB) must be escaped to \\\\u0017\";\n                    return token_type::parse_error;\n                }\n\n                case 0x18:\n                {\n                    error_message = \"invalid string: control character U+0018 (CAN) must be escaped to \\\\u0018\";\n                    return token_type::parse_error;\n                }\n\n                case 0x19:\n                {\n                    error_message = \"invalid string: control character U+0019 (EM) must be escaped to \\\\u0019\";\n                    return token_type::parse_error;\n                }\n\n                case 0x1A:\n                {\n                    error_message = \"invalid string: control character U+001A (SUB) must be escaped to \\\\u001A\";\n                    return token_type::parse_error;\n                }\n\n                case 0x1B:\n                {\n                    error_message = \"invalid string: control character U+001B (ESC) must be escaped to \\\\u001B\";\n                    return token_type::parse_error;\n                }\n\n                case 0x1C:\n                {\n                    error_message = \"invalid string: control character U+001C (FS) must be escaped to \\\\u001C\";\n                    return token_type::parse_error;\n                }\n\n                case 0x1D:\n                {\n                    error_message = \"invalid string: control character U+001D (GS) must be escaped to \\\\u001D\";\n                    return token_type::parse_error;\n                }\n\n                case 0x1E:\n                {\n                    error_message = \"invalid string: control character U+001E (RS) must be escaped to \\\\u001E\";\n                    return token_type::parse_error;\n                }\n\n                case 0x1F:\n                {\n                    error_message = \"invalid string: control character U+001F (US) must be escaped to \\\\u001F\";\n                    return token_type::parse_error;\n                }\n\n                // U+0020..U+007F (except U+0022 (quote) and U+005C (backspace))\n                case 0x20:\n                case 0x21:\n                case 0x23:\n                case 0x24:\n                case 0x25:\n                case 0x26:\n                case 0x27:\n                case 0x28:\n                case 0x29:\n                case 0x2A:\n                case 0x2B:\n                case 0x2C:\n                case 0x2D:\n                case 0x2E:\n                case 0x2F:\n                case 0x30:\n                case 0x31:\n                case 0x32:\n                case 0x33:\n                case 0x34:\n                case 0x35:\n                case 0x36:\n                case 0x37:\n                case 0x38:\n                case 0x39:\n                case 0x3A:\n                case 0x3B:\n                case 0x3C:\n                case 0x3D:\n                case 0x3E:\n                case 0x3F:\n                case 0x40:\n                case 0x41:\n                case 0x42:\n                case 0x43:\n                case 0x44:\n                case 0x45:\n                case 0x46:\n                case 0x47:\n                case 0x48:\n                case 0x49:\n                case 0x4A:\n                case 0x4B:\n                case 0x4C:\n                case 0x4D:\n                case 0x4E:\n                case 0x4F:\n                case 0x50:\n                case 0x51:\n                case 0x52:\n                case 0x53:\n                case 0x54:\n                case 0x55:\n                case 0x56:\n                case 0x57:\n                case 0x58:\n                case 0x59:\n                case 0x5A:\n                case 0x5B:\n                case 0x5D:\n                case 0x5E:\n                case 0x5F:\n                case 0x60:\n                case 0x61:\n                case 0x62:\n                case 0x63:\n                case 0x64:\n                case 0x65:\n                case 0x66:\n                case 0x67:\n                case 0x68:\n                case 0x69:\n                case 0x6A:\n                case 0x6B:\n                case 0x6C:\n                case 0x6D:\n                case 0x6E:\n                case 0x6F:\n                case 0x70:\n                case 0x71:\n                case 0x72:\n                case 0x73:\n                case 0x74:\n                case 0x75:\n                case 0x76:\n                case 0x77:\n                case 0x78:\n                case 0x79:\n                case 0x7A:\n                case 0x7B:\n                case 0x7C:\n                case 0x7D:\n                case 0x7E:\n                case 0x7F:\n                {\n                    add(current);\n                    break;\n                }\n\n                // U+0080..U+07FF: bytes C2..DF 80..BF\n                case 0xC2:\n                case 0xC3:\n                case 0xC4:\n                case 0xC5:\n                case 0xC6:\n                case 0xC7:\n                case 0xC8:\n                case 0xC9:\n                case 0xCA:\n                case 0xCB:\n                case 0xCC:\n                case 0xCD:\n                case 0xCE:\n                case 0xCF:\n                case 0xD0:\n                case 0xD1:\n                case 0xD2:\n                case 0xD3:\n                case 0xD4:\n                case 0xD5:\n                case 0xD6:\n                case 0xD7:\n                case 0xD8:\n                case 0xD9:\n                case 0xDA:\n                case 0xDB:\n                case 0xDC:\n                case 0xDD:\n                case 0xDE:\n                case 0xDF:\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!next_byte_in_range({0x80, 0xBF})))\n                    {\n                        return token_type::parse_error;\n                    }\n                    break;\n                }\n\n                // U+0800..U+0FFF: bytes E0 A0..BF 80..BF\n                case 0xE0:\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0xA0, 0xBF, 0x80, 0xBF}))))\n                    {\n                        return token_type::parse_error;\n                    }\n                    break;\n                }\n\n                // U+1000..U+CFFF: bytes E1..EC 80..BF 80..BF\n                // U+E000..U+FFFF: bytes EE..EF 80..BF 80..BF\n                case 0xE1:\n                case 0xE2:\n                case 0xE3:\n                case 0xE4:\n                case 0xE5:\n                case 0xE6:\n                case 0xE7:\n                case 0xE8:\n                case 0xE9:\n                case 0xEA:\n                case 0xEB:\n                case 0xEC:\n                case 0xEE:\n                case 0xEF:\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0x80, 0xBF, 0x80, 0xBF}))))\n                    {\n                        return token_type::parse_error;\n                    }\n                    break;\n                }\n\n                // U+D000..U+D7FF: bytes ED 80..9F 80..BF\n                case 0xED:\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0x80, 0x9F, 0x80, 0xBF}))))\n                    {\n                        return token_type::parse_error;\n                    }\n                    break;\n                }\n\n                // U+10000..U+3FFFF F0 90..BF 80..BF 80..BF\n                case 0xF0:\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0x90, 0xBF, 0x80, 0xBF, 0x80, 0xBF}))))\n                    {\n                        return token_type::parse_error;\n                    }\n                    break;\n                }\n\n                // U+40000..U+FFFFF F1..F3 80..BF 80..BF 80..BF\n                case 0xF1:\n                case 0xF2:\n                case 0xF3:\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0x80, 0xBF, 0x80, 0xBF, 0x80, 0xBF}))))\n                    {\n                        return token_type::parse_error;\n                    }\n                    break;\n                }\n\n                // U+100000..U+10FFFF F4 80..8F 80..BF 80..BF\n                case 0xF4:\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!(next_byte_in_range({0x80, 0x8F, 0x80, 0xBF, 0x80, 0xBF}))))\n                    {\n                        return token_type::parse_error;\n                    }\n                    break;\n                }\n\n                // remaining bytes (80..C1 and F5..FF) are ill-formed\n                default:\n                {\n                    error_message = \"invalid string: ill-formed UTF-8 byte\";\n                    return token_type::parse_error;\n                }\n            }\n        }\n    }\n\n    /*!\n     * @brief scan a comment\n     * @return whether comment could be scanned successfully\n     */\n    bool scan_comment()\n    {\n        switch (get())\n        {\n            // single-line comments skip input until a newline or EOF is read\n            case '/':\n            {\n                while (true)\n                {\n                    switch (get())\n                    {\n                        case '\\n':\n                        case '\\r':\n                        case std::char_traits<char_type>::eof():\n                        case '\\0':\n                            return true;\n\n                        default:\n                            break;\n                    }\n                }\n            }\n\n            // multi-line comments skip input until */ is read\n            case '*':\n            {\n                while (true)\n                {\n                    switch (get())\n                    {\n                        case std::char_traits<char_type>::eof():\n                        case '\\0':\n                        {\n                            error_message = \"invalid comment; missing closing '*/'\";\n                            return false;\n                        }\n\n                        case '*':\n                        {\n                            switch (get())\n                            {\n                                case '/':\n                                    return true;\n\n                                default:\n                                {\n                                    unget();\n                                    continue;\n                                }\n                            }\n                        }\n\n                        default:\n                            continue;\n                    }\n                }\n            }\n\n            // unexpected character after reading '/'\n            default:\n            {\n                error_message = \"invalid comment; expecting '/' or '*' after '/'\";\n                return false;\n            }\n        }\n    }\n\n    JSON_HEDLEY_NON_NULL(2)\n    static void strtof(float& f, const char* str, char** endptr) noexcept\n    {\n        f = std::strtof(str, endptr);\n    }\n\n    JSON_HEDLEY_NON_NULL(2)\n    static void strtof(double& f, const char* str, char** endptr) noexcept\n    {\n        f = std::strtod(str, endptr);\n    }\n\n    JSON_HEDLEY_NON_NULL(2)\n    static void strtof(long double& f, const char* str, char** endptr) noexcept\n    {\n        f = std::strtold(str, endptr);\n    }\n\n    /*!\n    @brief scan a number literal\n\n    This function scans a string according to Sect. 6 of RFC 8259.\n\n    The function is realized with a deterministic finite state machine derived\n    from the grammar described in RFC 8259. Starting in state \"init\", the\n    input is read and used to determined the next state. Only state \"done\"\n    accepts the number. State \"error\" is a trap state to model errors. In the\n    table below, \"anything\" means any character but the ones listed before.\n\n    state    | 0        | 1-9      | e E      | +       | -       | .        | anything\n    ---------|----------|----------|----------|---------|---------|----------|-----------\n    init     | zero     | any1     | [error]  | [error] | minus   | [error]  | [error]\n    minus    | zero     | any1     | [error]  | [error] | [error] | [error]  | [error]\n    zero     | done     | done     | exponent | done    | done    | decimal1 | done\n    any1     | any1     | any1     | exponent | done    | done    | decimal1 | done\n    decimal1 | decimal2 | decimal2 | [error]  | [error] | [error] | [error]  | [error]\n    decimal2 | decimal2 | decimal2 | exponent | done    | done    | done     | done\n    exponent | any2     | any2     | [error]  | sign    | sign    | [error]  | [error]\n    sign     | any2     | any2     | [error]  | [error] | [error] | [error]  | [error]\n    any2     | any2     | any2     | done     | done    | done    | done     | done\n\n    The state machine is realized with one label per state (prefixed with\n    \"scan_number_\") and `goto` statements between them. The state machine\n    contains cycles, but any cycle can be left when EOF is read. Therefore,\n    the function is guaranteed to terminate.\n\n    During scanning, the read bytes are stored in token_buffer. This string is\n    then converted to a signed integer, an unsigned integer, or a\n    floating-point number.\n\n    @return token_type::value_unsigned, token_type::value_integer, or\n            token_type::value_float if number could be successfully scanned,\n            token_type::parse_error otherwise\n\n    @note The scanner is independent of the current locale. Internally, the\n          locale's decimal point is used instead of `.` to work with the\n          locale-dependent converters.\n    */\n    token_type scan_number()  // lgtm [cpp/use-of-goto]\n    {\n        // reset token_buffer to store the number's bytes\n        reset();\n\n        // the type of the parsed number; initially set to unsigned; will be\n        // changed if minus sign, decimal point or exponent is read\n        token_type number_type = token_type::value_unsigned;\n\n        // state (init): we just found out we need to scan a number\n        switch (current)\n        {\n            case '-':\n            {\n                add(current);\n                goto scan_number_minus;\n            }\n\n            case '0':\n            {\n                add(current);\n                goto scan_number_zero;\n            }\n\n            case '1':\n            case '2':\n            case '3':\n            case '4':\n            case '5':\n            case '6':\n            case '7':\n            case '8':\n            case '9':\n            {\n                add(current);\n                goto scan_number_any1;\n            }\n\n            // all other characters are rejected outside scan_number()\n            default:            // LCOV_EXCL_LINE\n                JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE\n        }\n\nscan_number_minus:\n        // state: we just parsed a leading minus sign\n        number_type = token_type::value_integer;\n        switch (get())\n        {\n            case '0':\n            {\n                add(current);\n                goto scan_number_zero;\n            }\n\n            case '1':\n            case '2':\n            case '3':\n            case '4':\n            case '5':\n            case '6':\n            case '7':\n            case '8':\n            case '9':\n            {\n                add(current);\n                goto scan_number_any1;\n            }\n\n            default:\n            {\n                error_message = \"invalid number; expected digit after '-'\";\n                return token_type::parse_error;\n            }\n        }\n\nscan_number_zero:\n        // state: we just parse a zero (maybe with a leading minus sign)\n        switch (get())\n        {\n            case '.':\n            {\n                add(decimal_point_char);\n                goto scan_number_decimal1;\n            }\n\n            case 'e':\n            case 'E':\n            {\n                add(current);\n                goto scan_number_exponent;\n            }\n\n            default:\n                goto scan_number_done;\n        }\n\nscan_number_any1:\n        // state: we just parsed a number 0-9 (maybe with a leading minus sign)\n        switch (get())\n        {\n            case '0':\n            case '1':\n            case '2':\n            case '3':\n            case '4':\n            case '5':\n            case '6':\n            case '7':\n            case '8':\n            case '9':\n            {\n                add(current);\n                goto scan_number_any1;\n            }\n\n            case '.':\n            {\n                add(decimal_point_char);\n                goto scan_number_decimal1;\n            }\n\n            case 'e':\n            case 'E':\n            {\n                add(current);\n                goto scan_number_exponent;\n            }\n\n            default:\n                goto scan_number_done;\n        }\n\nscan_number_decimal1:\n        // state: we just parsed a decimal point\n        number_type = token_type::value_float;\n        switch (get())\n        {\n            case '0':\n            case '1':\n            case '2':\n            case '3':\n            case '4':\n            case '5':\n            case '6':\n            case '7':\n            case '8':\n            case '9':\n            {\n                add(current);\n                goto scan_number_decimal2;\n            }\n\n            default:\n            {\n                error_message = \"invalid number; expected digit after '.'\";\n                return token_type::parse_error;\n            }\n        }\n\nscan_number_decimal2:\n        // we just parsed at least one number after a decimal point\n        switch (get())\n        {\n            case '0':\n            case '1':\n            case '2':\n            case '3':\n            case '4':\n            case '5':\n            case '6':\n            case '7':\n            case '8':\n            case '9':\n            {\n                add(current);\n                goto scan_number_decimal2;\n            }\n\n            case 'e':\n            case 'E':\n            {\n                add(current);\n                goto scan_number_exponent;\n            }\n\n            default:\n                goto scan_number_done;\n        }\n\nscan_number_exponent:\n        // we just parsed an exponent\n        number_type = token_type::value_float;\n        switch (get())\n        {\n            case '+':\n            case '-':\n            {\n                add(current);\n                goto scan_number_sign;\n            }\n\n            case '0':\n            case '1':\n            case '2':\n            case '3':\n            case '4':\n            case '5':\n            case '6':\n            case '7':\n            case '8':\n            case '9':\n            {\n                add(current);\n                goto scan_number_any2;\n            }\n\n            default:\n            {\n                error_message =\n                    \"invalid number; expected '+', '-', or digit after exponent\";\n                return token_type::parse_error;\n            }\n        }\n\nscan_number_sign:\n        // we just parsed an exponent sign\n        switch (get())\n        {\n            case '0':\n            case '1':\n            case '2':\n            case '3':\n            case '4':\n            case '5':\n            case '6':\n            case '7':\n            case '8':\n            case '9':\n            {\n                add(current);\n                goto scan_number_any2;\n            }\n\n            default:\n            {\n                error_message = \"invalid number; expected digit after exponent sign\";\n                return token_type::parse_error;\n            }\n        }\n\nscan_number_any2:\n        // we just parsed a number after the exponent or exponent sign\n        switch (get())\n        {\n            case '0':\n            case '1':\n            case '2':\n            case '3':\n            case '4':\n            case '5':\n            case '6':\n            case '7':\n            case '8':\n            case '9':\n            {\n                add(current);\n                goto scan_number_any2;\n            }\n\n            default:\n                goto scan_number_done;\n        }\n\nscan_number_done:\n        // unget the character after the number (we only read it to know that\n        // we are done scanning a number)\n        unget();\n\n        char* endptr = nullptr; // NOLINT(cppcoreguidelines-pro-type-vararg,hicpp-vararg)\n        errno = 0;\n\n        // try to parse integers first and fall back to floats\n        if (number_type == token_type::value_unsigned)\n        {\n            const auto x = std::strtoull(token_buffer.data(), &endptr, 10);\n\n            // we checked the number format before\n            JSON_ASSERT(endptr == token_buffer.data() + token_buffer.size());\n\n            if (errno == 0)\n            {\n                value_unsigned = static_cast<number_unsigned_t>(x);\n                if (value_unsigned == x)\n                {\n                    return token_type::value_unsigned;\n                }\n            }\n        }\n        else if (number_type == token_type::value_integer)\n        {\n            const auto x = std::strtoll(token_buffer.data(), &endptr, 10);\n\n            // we checked the number format before\n            JSON_ASSERT(endptr == token_buffer.data() + token_buffer.size());\n\n            if (errno == 0)\n            {\n                value_integer = static_cast<number_integer_t>(x);\n                if (value_integer == x)\n                {\n                    return token_type::value_integer;\n                }\n            }\n        }\n\n        // this code is reached if we parse a floating-point number or if an\n        // integer conversion above failed\n        strtof(value_float, token_buffer.data(), &endptr);\n\n        // we checked the number format before\n        JSON_ASSERT(endptr == token_buffer.data() + token_buffer.size());\n\n        return token_type::value_float;\n    }\n\n    /*!\n    @param[in] literal_text  the literal text to expect\n    @param[in] length        the length of the passed literal text\n    @param[in] return_type   the token type to return on success\n    */\n    JSON_HEDLEY_NON_NULL(2)\n    token_type scan_literal(const char_type* literal_text, const std::size_t length,\n                            token_type return_type)\n    {\n        JSON_ASSERT(std::char_traits<char_type>::to_char_type(current) == literal_text[0]);\n        for (std::size_t i = 1; i < length; ++i)\n        {\n            if (JSON_HEDLEY_UNLIKELY(std::char_traits<char_type>::to_char_type(get()) != literal_text[i]))\n            {\n                error_message = \"invalid literal\";\n                return token_type::parse_error;\n            }\n        }\n        return return_type;\n    }\n\n    /////////////////////\n    // input management\n    /////////////////////\n\n    /// reset token_buffer; current character is beginning of token\n    void reset() noexcept\n    {\n        token_buffer.clear();\n        token_string.clear();\n        token_string.push_back(std::char_traits<char_type>::to_char_type(current));\n    }\n\n    /*\n    @brief get next character from the input\n\n    This function provides the interface to the used input adapter. It does\n    not throw in case the input reached EOF, but returns a\n    `std::char_traits<char>::eof()` in that case.  Stores the scanned characters\n    for use in error messages.\n\n    @return character read from the input\n    */\n    char_int_type get()\n    {\n        ++position.chars_read_total;\n        ++position.chars_read_current_line;\n\n        if (next_unget)\n        {\n            // just reset the next_unget variable and work with current\n            next_unget = false;\n        }\n        else\n        {\n            current = ia.get_character();\n        }\n\n        if (JSON_HEDLEY_LIKELY(current != std::char_traits<char_type>::eof()))\n        {\n            token_string.push_back(std::char_traits<char_type>::to_char_type(current));\n        }\n\n        if (current == '\\n')\n        {\n            ++position.lines_read;\n            position.chars_read_current_line = 0;\n        }\n\n        return current;\n    }\n\n    /*!\n    @brief unget current character (read it again on next get)\n\n    We implement unget by setting variable next_unget to true. The input is not\n    changed - we just simulate ungetting by modifying chars_read_total,\n    chars_read_current_line, and token_string. The next call to get() will\n    behave as if the unget character is read again.\n    */\n    void unget()\n    {\n        next_unget = true;\n\n        --position.chars_read_total;\n\n        // in case we \"unget\" a newline, we have to also decrement the lines_read\n        if (position.chars_read_current_line == 0)\n        {\n            if (position.lines_read > 0)\n            {\n                --position.lines_read;\n            }\n        }\n        else\n        {\n            --position.chars_read_current_line;\n        }\n\n        if (JSON_HEDLEY_LIKELY(current != std::char_traits<char_type>::eof()))\n        {\n            JSON_ASSERT(!token_string.empty());\n            token_string.pop_back();\n        }\n    }\n\n    /// add a character to token_buffer\n    void add(char_int_type c)\n    {\n        token_buffer.push_back(static_cast<typename string_t::value_type>(c));\n    }\n\n  public:\n    /////////////////////\n    // value getters\n    /////////////////////\n\n    /// return integer value\n    constexpr number_integer_t get_number_integer() const noexcept\n    {\n        return value_integer;\n    }\n\n    /// return unsigned integer value\n    constexpr number_unsigned_t get_number_unsigned() const noexcept\n    {\n        return value_unsigned;\n    }\n\n    /// return floating-point value\n    constexpr number_float_t get_number_float() const noexcept\n    {\n        return value_float;\n    }\n\n    /// return current string value (implicitly resets the token; useful only once)\n    string_t& get_string()\n    {\n        return token_buffer;\n    }\n\n    /////////////////////\n    // diagnostics\n    /////////////////////\n\n    /// return position of last read token\n    constexpr position_t get_position() const noexcept\n    {\n        return position;\n    }\n\n    /// return the last read token (for errors only).  Will never contain EOF\n    /// (an arbitrary value that is not a valid char value, often -1), because\n    /// 255 may legitimately occur.  May contain NUL, which should be escaped.\n    std::string get_token_string() const\n    {\n        // escape control characters\n        std::string result;\n        for (const auto c : token_string)\n        {\n            if (static_cast<unsigned char>(c) <= '\\x1F')\n            {\n                // escape control characters\n                std::array<char, 9> cs{{}};\n                static_cast<void>((std::snprintf)(cs.data(), cs.size(), \"<U+%.4X>\", static_cast<unsigned char>(c))); // NOLINT(cppcoreguidelines-pro-type-vararg,hicpp-vararg)\n                result += cs.data();\n            }\n            else\n            {\n                // add character as is\n                result.push_back(static_cast<std::string::value_type>(c));\n            }\n        }\n\n        return result;\n    }\n\n    /// return syntax error message\n    JSON_HEDLEY_RETURNS_NON_NULL\n    constexpr const char* get_error_message() const noexcept\n    {\n        return error_message;\n    }\n\n    /////////////////////\n    // actual scanner\n    /////////////////////\n\n    /*!\n    @brief skip the UTF-8 byte order mark\n    @return true iff there is no BOM or the correct BOM has been skipped\n    */\n    bool skip_bom()\n    {\n        if (get() == 0xEF)\n        {\n            // check if we completely parse the BOM\n            return get() == 0xBB && get() == 0xBF;\n        }\n\n        // the first character is not the beginning of the BOM; unget it to\n        // process is later\n        unget();\n        return true;\n    }\n\n    void skip_whitespace()\n    {\n        do\n        {\n            get();\n        }\n        while (current == ' ' || current == '\\t' || current == '\\n' || current == '\\r');\n    }\n\n    token_type scan()\n    {\n        // initially, skip the BOM\n        if (position.chars_read_total == 0 && !skip_bom())\n        {\n            error_message = \"invalid BOM; must be 0xEF 0xBB 0xBF if given\";\n            return token_type::parse_error;\n        }\n\n        // read next character and ignore whitespace\n        skip_whitespace();\n\n        // ignore comments\n        while (ignore_comments && current == '/')\n        {\n            if (!scan_comment())\n            {\n                return token_type::parse_error;\n            }\n\n            // skip following whitespace\n            skip_whitespace();\n        }\n\n        switch (current)\n        {\n            // structural characters\n            case '[':\n                return token_type::begin_array;\n            case ']':\n                return token_type::end_array;\n            case '{':\n                return token_type::begin_object;\n            case '}':\n                return token_type::end_object;\n            case ':':\n                return token_type::name_separator;\n            case ',':\n                return token_type::value_separator;\n\n            // literals\n            case 't':\n            {\n                std::array<char_type, 4> true_literal = {{static_cast<char_type>('t'), static_cast<char_type>('r'), static_cast<char_type>('u'), static_cast<char_type>('e')}};\n                return scan_literal(true_literal.data(), true_literal.size(), token_type::literal_true);\n            }\n            case 'f':\n            {\n                std::array<char_type, 5> false_literal = {{static_cast<char_type>('f'), static_cast<char_type>('a'), static_cast<char_type>('l'), static_cast<char_type>('s'), static_cast<char_type>('e')}};\n                return scan_literal(false_literal.data(), false_literal.size(), token_type::literal_false);\n            }\n            case 'n':\n            {\n                std::array<char_type, 4> null_literal = {{static_cast<char_type>('n'), static_cast<char_type>('u'), static_cast<char_type>('l'), static_cast<char_type>('l')}};\n                return scan_literal(null_literal.data(), null_literal.size(), token_type::literal_null);\n            }\n\n            // string\n            case '\\\"':\n                return scan_string();\n\n            // number\n            case '-':\n            case '0':\n            case '1':\n            case '2':\n            case '3':\n            case '4':\n            case '5':\n            case '6':\n            case '7':\n            case '8':\n            case '9':\n                return scan_number();\n\n            // end of input (the null byte is needed when parsing from\n            // string literals)\n            case '\\0':\n            case std::char_traits<char_type>::eof():\n                return token_type::end_of_input;\n\n            // error\n            default:\n                error_message = \"invalid literal\";\n                return token_type::parse_error;\n        }\n    }\n\n  private:\n    /// input adapter\n    InputAdapterType ia;\n\n    /// whether comments should be ignored (true) or signaled as errors (false)\n    const bool ignore_comments = false;\n\n    /// the current character\n    char_int_type current = std::char_traits<char_type>::eof();\n\n    /// whether the next get() call should just return current\n    bool next_unget = false;\n\n    /// the start position of the current token\n    position_t position {};\n\n    /// raw input token string (for error messages)\n    std::vector<char_type> token_string {};\n\n    /// buffer for variable-length tokens (numbers, strings)\n    string_t token_buffer {};\n\n    /// a description of occurred lexer errors\n    const char* error_message = \"\";\n\n    // number values\n    number_integer_t value_integer = 0;\n    number_unsigned_t value_unsigned = 0;\n    number_float_t value_float = 0;\n\n    /// the decimal point\n    const char_int_type decimal_point_char = '.';\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/meta/is_sax.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <cstdint> // size_t\n#include <utility> // declval\n#include <string> // string\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n// #include <nlohmann/detail/meta/detected.hpp>\n\n// #include <nlohmann/detail/meta/type_traits.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\ntemplate<typename T>\nusing null_function_t = decltype(std::declval<T&>().null());\n\ntemplate<typename T>\nusing boolean_function_t =\n    decltype(std::declval<T&>().boolean(std::declval<bool>()));\n\ntemplate<typename T, typename Integer>\nusing number_integer_function_t =\n    decltype(std::declval<T&>().number_integer(std::declval<Integer>()));\n\ntemplate<typename T, typename Unsigned>\nusing number_unsigned_function_t =\n    decltype(std::declval<T&>().number_unsigned(std::declval<Unsigned>()));\n\ntemplate<typename T, typename Float, typename String>\nusing number_float_function_t = decltype(std::declval<T&>().number_float(\n                                    std::declval<Float>(), std::declval<const String&>()));\n\ntemplate<typename T, typename String>\nusing string_function_t =\n    decltype(std::declval<T&>().string(std::declval<String&>()));\n\ntemplate<typename T, typename Binary>\nusing binary_function_t =\n    decltype(std::declval<T&>().binary(std::declval<Binary&>()));\n\ntemplate<typename T>\nusing start_object_function_t =\n    decltype(std::declval<T&>().start_object(std::declval<std::size_t>()));\n\ntemplate<typename T, typename String>\nusing key_function_t =\n    decltype(std::declval<T&>().key(std::declval<String&>()));\n\ntemplate<typename T>\nusing end_object_function_t = decltype(std::declval<T&>().end_object());\n\ntemplate<typename T>\nusing start_array_function_t =\n    decltype(std::declval<T&>().start_array(std::declval<std::size_t>()));\n\ntemplate<typename T>\nusing end_array_function_t = decltype(std::declval<T&>().end_array());\n\ntemplate<typename T, typename Exception>\nusing parse_error_function_t = decltype(std::declval<T&>().parse_error(\n        std::declval<std::size_t>(), std::declval<const std::string&>(),\n        std::declval<const Exception&>()));\n\ntemplate<typename SAX, typename BasicJsonType>\nstruct is_sax\n{\n  private:\n    static_assert(is_basic_json<BasicJsonType>::value,\n                  \"BasicJsonType must be of type basic_json<...>\");\n\n    using number_integer_t = typename BasicJsonType::number_integer_t;\n    using number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n    using string_t = typename BasicJsonType::string_t;\n    using binary_t = typename BasicJsonType::binary_t;\n    using exception_t = typename BasicJsonType::exception;\n\n  public:\n    static constexpr bool value =\n        is_detected_exact<bool, null_function_t, SAX>::value &&\n        is_detected_exact<bool, boolean_function_t, SAX>::value &&\n        is_detected_exact<bool, number_integer_function_t, SAX, number_integer_t>::value &&\n        is_detected_exact<bool, number_unsigned_function_t, SAX, number_unsigned_t>::value &&\n        is_detected_exact<bool, number_float_function_t, SAX, number_float_t, string_t>::value &&\n        is_detected_exact<bool, string_function_t, SAX, string_t>::value &&\n        is_detected_exact<bool, binary_function_t, SAX, binary_t>::value &&\n        is_detected_exact<bool, start_object_function_t, SAX>::value &&\n        is_detected_exact<bool, key_function_t, SAX, string_t>::value &&\n        is_detected_exact<bool, end_object_function_t, SAX>::value &&\n        is_detected_exact<bool, start_array_function_t, SAX>::value &&\n        is_detected_exact<bool, end_array_function_t, SAX>::value &&\n        is_detected_exact<bool, parse_error_function_t, SAX, exception_t>::value;\n};\n\ntemplate<typename SAX, typename BasicJsonType>\nstruct is_sax_static_asserts\n{\n  private:\n    static_assert(is_basic_json<BasicJsonType>::value,\n                  \"BasicJsonType must be of type basic_json<...>\");\n\n    using number_integer_t = typename BasicJsonType::number_integer_t;\n    using number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n    using string_t = typename BasicJsonType::string_t;\n    using binary_t = typename BasicJsonType::binary_t;\n    using exception_t = typename BasicJsonType::exception;\n\n  public:\n    static_assert(is_detected_exact<bool, null_function_t, SAX>::value,\n                  \"Missing/invalid function: bool null()\");\n    static_assert(is_detected_exact<bool, boolean_function_t, SAX>::value,\n                  \"Missing/invalid function: bool boolean(bool)\");\n    static_assert(is_detected_exact<bool, boolean_function_t, SAX>::value,\n                  \"Missing/invalid function: bool boolean(bool)\");\n    static_assert(\n        is_detected_exact<bool, number_integer_function_t, SAX,\n        number_integer_t>::value,\n        \"Missing/invalid function: bool number_integer(number_integer_t)\");\n    static_assert(\n        is_detected_exact<bool, number_unsigned_function_t, SAX,\n        number_unsigned_t>::value,\n        \"Missing/invalid function: bool number_unsigned(number_unsigned_t)\");\n    static_assert(is_detected_exact<bool, number_float_function_t, SAX,\n                  number_float_t, string_t>::value,\n                  \"Missing/invalid function: bool number_float(number_float_t, const string_t&)\");\n    static_assert(\n        is_detected_exact<bool, string_function_t, SAX, string_t>::value,\n        \"Missing/invalid function: bool string(string_t&)\");\n    static_assert(\n        is_detected_exact<bool, binary_function_t, SAX, binary_t>::value,\n        \"Missing/invalid function: bool binary(binary_t&)\");\n    static_assert(is_detected_exact<bool, start_object_function_t, SAX>::value,\n                  \"Missing/invalid function: bool start_object(std::size_t)\");\n    static_assert(is_detected_exact<bool, key_function_t, SAX, string_t>::value,\n                  \"Missing/invalid function: bool key(string_t&)\");\n    static_assert(is_detected_exact<bool, end_object_function_t, SAX>::value,\n                  \"Missing/invalid function: bool end_object()\");\n    static_assert(is_detected_exact<bool, start_array_function_t, SAX>::value,\n                  \"Missing/invalid function: bool start_array(std::size_t)\");\n    static_assert(is_detected_exact<bool, end_array_function_t, SAX>::value,\n                  \"Missing/invalid function: bool end_array()\");\n    static_assert(\n        is_detected_exact<bool, parse_error_function_t, SAX, exception_t>::value,\n        \"Missing/invalid function: bool parse_error(std::size_t, const \"\n        \"std::string&, const exception&)\");\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/meta/type_traits.hpp>\n\n// #include <nlohmann/detail/string_concat.hpp>\n\n// #include <nlohmann/detail/value_t.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n/// how to treat CBOR tags\nenum class cbor_tag_handler_t\n{\n    error,   ///< throw a parse_error exception in case of a tag\n    ignore,  ///< ignore tags\n    store    ///< store tags as binary type\n};\n\n/*!\n@brief determine system byte order\n\n@return true if and only if system's byte order is little endian\n\n@note from https://stackoverflow.com/a/1001328/266378\n*/\nstatic inline bool little_endianness(int num = 1) noexcept\n{\n    return *reinterpret_cast<char*>(&num) == 1;\n}\n\n\n///////////////////\n// binary reader //\n///////////////////\n\n/*!\n@brief deserialization of CBOR, MessagePack, and UBJSON values\n*/\ntemplate<typename BasicJsonType, typename InputAdapterType, typename SAX = json_sax_dom_parser<BasicJsonType>>\nclass binary_reader\n{\n    using number_integer_t = typename BasicJsonType::number_integer_t;\n    using number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n    using string_t = typename BasicJsonType::string_t;\n    using binary_t = typename BasicJsonType::binary_t;\n    using json_sax_t = SAX;\n    using char_type = typename InputAdapterType::char_type;\n    using char_int_type = typename std::char_traits<char_type>::int_type;\n\n  public:\n    /*!\n    @brief create a binary reader\n\n    @param[in] adapter  input adapter to read from\n    */\n    explicit binary_reader(InputAdapterType&& adapter, const input_format_t format = input_format_t::json) noexcept : ia(std::move(adapter)), input_format(format)\n    {\n        (void)detail::is_sax_static_asserts<SAX, BasicJsonType> {};\n    }\n\n    // make class move-only\n    binary_reader(const binary_reader&) = delete;\n    binary_reader(binary_reader&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor)\n    binary_reader& operator=(const binary_reader&) = delete;\n    binary_reader& operator=(binary_reader&&) = default; // NOLINT(hicpp-noexcept-move,performance-noexcept-move-constructor)\n    ~binary_reader() = default;\n\n    /*!\n    @param[in] format  the binary format to parse\n    @param[in] sax_    a SAX event processor\n    @param[in] strict  whether to expect the input to be consumed completed\n    @param[in] tag_handler  how to treat CBOR tags\n\n    @return whether parsing was successful\n    */\n    JSON_HEDLEY_NON_NULL(3)\n    bool sax_parse(const input_format_t format,\n                   json_sax_t* sax_,\n                   const bool strict = true,\n                   const cbor_tag_handler_t tag_handler = cbor_tag_handler_t::error)\n    {\n        sax = sax_;\n        bool result = false;\n\n        switch (format)\n        {\n            case input_format_t::bson:\n                result = parse_bson_internal();\n                break;\n\n            case input_format_t::cbor:\n                result = parse_cbor_internal(true, tag_handler);\n                break;\n\n            case input_format_t::msgpack:\n                result = parse_msgpack_internal();\n                break;\n\n            case input_format_t::ubjson:\n            case input_format_t::bjdata:\n                result = parse_ubjson_internal();\n                break;\n\n            case input_format_t::json: // LCOV_EXCL_LINE\n            default:            // LCOV_EXCL_LINE\n                JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE\n        }\n\n        // strict mode: next byte must be EOF\n        if (result && strict)\n        {\n            if (input_format == input_format_t::ubjson || input_format == input_format_t::bjdata)\n            {\n                get_ignore_noop();\n            }\n            else\n            {\n                get();\n            }\n\n            if (JSON_HEDLEY_UNLIKELY(current != std::char_traits<char_type>::eof()))\n            {\n                return sax->parse_error(chars_read, get_token_string(), parse_error::create(110, chars_read,\n                                        exception_message(input_format, concat(\"expected end of input; last byte: 0x\", get_token_string()), \"value\"), nullptr));\n            }\n        }\n\n        return result;\n    }\n\n  private:\n    //////////\n    // BSON //\n    //////////\n\n    /*!\n    @brief Reads in a BSON-object and passes it to the SAX-parser.\n    @return whether a valid BSON-value was passed to the SAX parser\n    */\n    bool parse_bson_internal()\n    {\n        std::int32_t document_size{};\n        get_number<std::int32_t, true>(input_format_t::bson, document_size);\n\n        if (JSON_HEDLEY_UNLIKELY(!sax->start_object(static_cast<std::size_t>(-1))))\n        {\n            return false;\n        }\n\n        if (JSON_HEDLEY_UNLIKELY(!parse_bson_element_list(/*is_array*/false)))\n        {\n            return false;\n        }\n\n        return sax->end_object();\n    }\n\n    /*!\n    @brief Parses a C-style string from the BSON input.\n    @param[in,out] result  A reference to the string variable where the read\n                            string is to be stored.\n    @return `true` if the \\x00-byte indicating the end of the string was\n             encountered before the EOF; false` indicates an unexpected EOF.\n    */\n    bool get_bson_cstr(string_t& result)\n    {\n        auto out = std::back_inserter(result);\n        while (true)\n        {\n            get();\n            if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::bson, \"cstring\")))\n            {\n                return false;\n            }\n            if (current == 0x00)\n            {\n                return true;\n            }\n            *out++ = static_cast<typename string_t::value_type>(current);\n        }\n    }\n\n    /*!\n    @brief Parses a zero-terminated string of length @a len from the BSON\n           input.\n    @param[in] len  The length (including the zero-byte at the end) of the\n                    string to be read.\n    @param[in,out] result  A reference to the string variable where the read\n                            string is to be stored.\n    @tparam NumberType The type of the length @a len\n    @pre len >= 1\n    @return `true` if the string was successfully parsed\n    */\n    template<typename NumberType>\n    bool get_bson_string(const NumberType len, string_t& result)\n    {\n        if (JSON_HEDLEY_UNLIKELY(len < 1))\n        {\n            auto last_token = get_token_string();\n            return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read,\n                                    exception_message(input_format_t::bson, concat(\"string length must be at least 1, is \", std::to_string(len)), \"string\"), nullptr));\n        }\n\n        return get_string(input_format_t::bson, len - static_cast<NumberType>(1), result) && get() != std::char_traits<char_type>::eof();\n    }\n\n    /*!\n    @brief Parses a byte array input of length @a len from the BSON input.\n    @param[in] len  The length of the byte array to be read.\n    @param[in,out] result  A reference to the binary variable where the read\n                            array is to be stored.\n    @tparam NumberType The type of the length @a len\n    @pre len >= 0\n    @return `true` if the byte array was successfully parsed\n    */\n    template<typename NumberType>\n    bool get_bson_binary(const NumberType len, binary_t& result)\n    {\n        if (JSON_HEDLEY_UNLIKELY(len < 0))\n        {\n            auto last_token = get_token_string();\n            return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read,\n                                    exception_message(input_format_t::bson, concat(\"byte array length cannot be negative, is \", std::to_string(len)), \"binary\"), nullptr));\n        }\n\n        // All BSON binary values have a subtype\n        std::uint8_t subtype{};\n        get_number<std::uint8_t>(input_format_t::bson, subtype);\n        result.set_subtype(subtype);\n\n        return get_binary(input_format_t::bson, len, result);\n    }\n\n    /*!\n    @brief Read a BSON document element of the given @a element_type.\n    @param[in] element_type The BSON element type, c.f. http://bsonspec.org/spec.html\n    @param[in] element_type_parse_position The position in the input stream,\n               where the `element_type` was read.\n    @warning Not all BSON element types are supported yet. An unsupported\n             @a element_type will give rise to a parse_error.114:\n             Unsupported BSON record type 0x...\n    @return whether a valid BSON-object/array was passed to the SAX parser\n    */\n    bool parse_bson_element_internal(const char_int_type element_type,\n                                     const std::size_t element_type_parse_position)\n    {\n        switch (element_type)\n        {\n            case 0x01: // double\n            {\n                double number{};\n                return get_number<double, true>(input_format_t::bson, number) && sax->number_float(static_cast<number_float_t>(number), \"\");\n            }\n\n            case 0x02: // string\n            {\n                std::int32_t len{};\n                string_t value;\n                return get_number<std::int32_t, true>(input_format_t::bson, len) && get_bson_string(len, value) && sax->string(value);\n            }\n\n            case 0x03: // object\n            {\n                return parse_bson_internal();\n            }\n\n            case 0x04: // array\n            {\n                return parse_bson_array();\n            }\n\n            case 0x05: // binary\n            {\n                std::int32_t len{};\n                binary_t value;\n                return get_number<std::int32_t, true>(input_format_t::bson, len) && get_bson_binary(len, value) && sax->binary(value);\n            }\n\n            case 0x08: // boolean\n            {\n                return sax->boolean(get() != 0);\n            }\n\n            case 0x0A: // null\n            {\n                return sax->null();\n            }\n\n            case 0x10: // int32\n            {\n                std::int32_t value{};\n                return get_number<std::int32_t, true>(input_format_t::bson, value) && sax->number_integer(value);\n            }\n\n            case 0x12: // int64\n            {\n                std::int64_t value{};\n                return get_number<std::int64_t, true>(input_format_t::bson, value) && sax->number_integer(value);\n            }\n\n            default: // anything else not supported (yet)\n            {\n                std::array<char, 3> cr{{}};\n                static_cast<void>((std::snprintf)(cr.data(), cr.size(), \"%.2hhX\", static_cast<unsigned char>(element_type))); // NOLINT(cppcoreguidelines-pro-type-vararg,hicpp-vararg)\n                std::string cr_str{cr.data()};\n                return sax->parse_error(element_type_parse_position, cr_str,\n                                        parse_error::create(114, element_type_parse_position, concat(\"Unsupported BSON record type 0x\", cr_str), nullptr));\n            }\n        }\n    }\n\n    /*!\n    @brief Read a BSON element list (as specified in the BSON-spec)\n\n    The same binary layout is used for objects and arrays, hence it must be\n    indicated with the argument @a is_array which one is expected\n    (true --> array, false --> object).\n\n    @param[in] is_array Determines if the element list being read is to be\n                        treated as an object (@a is_array == false), or as an\n                        array (@a is_array == true).\n    @return whether a valid BSON-object/array was passed to the SAX parser\n    */\n    bool parse_bson_element_list(const bool is_array)\n    {\n        string_t key;\n\n        while (auto element_type = get())\n        {\n            if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::bson, \"element list\")))\n            {\n                return false;\n            }\n\n            const std::size_t element_type_parse_position = chars_read;\n            if (JSON_HEDLEY_UNLIKELY(!get_bson_cstr(key)))\n            {\n                return false;\n            }\n\n            if (!is_array && !sax->key(key))\n            {\n                return false;\n            }\n\n            if (JSON_HEDLEY_UNLIKELY(!parse_bson_element_internal(element_type, element_type_parse_position)))\n            {\n                return false;\n            }\n\n            // get_bson_cstr only appends\n            key.clear();\n        }\n\n        return true;\n    }\n\n    /*!\n    @brief Reads an array from the BSON input and passes it to the SAX-parser.\n    @return whether a valid BSON-array was passed to the SAX parser\n    */\n    bool parse_bson_array()\n    {\n        std::int32_t document_size{};\n        get_number<std::int32_t, true>(input_format_t::bson, document_size);\n\n        if (JSON_HEDLEY_UNLIKELY(!sax->start_array(static_cast<std::size_t>(-1))))\n        {\n            return false;\n        }\n\n        if (JSON_HEDLEY_UNLIKELY(!parse_bson_element_list(/*is_array*/true)))\n        {\n            return false;\n        }\n\n        return sax->end_array();\n    }\n\n    //////////\n    // CBOR //\n    //////////\n\n    /*!\n    @param[in] get_char  whether a new character should be retrieved from the\n                         input (true) or whether the last read character should\n                         be considered instead (false)\n    @param[in] tag_handler how CBOR tags should be treated\n\n    @return whether a valid CBOR value was passed to the SAX parser\n    */\n    bool parse_cbor_internal(const bool get_char,\n                             const cbor_tag_handler_t tag_handler)\n    {\n        switch (get_char ? get() : current)\n        {\n            // EOF\n            case std::char_traits<char_type>::eof():\n                return unexpect_eof(input_format_t::cbor, \"value\");\n\n            // Integer 0x00..0x17 (0..23)\n            case 0x00:\n            case 0x01:\n            case 0x02:\n            case 0x03:\n            case 0x04:\n            case 0x05:\n            case 0x06:\n            case 0x07:\n            case 0x08:\n            case 0x09:\n            case 0x0A:\n            case 0x0B:\n            case 0x0C:\n            case 0x0D:\n            case 0x0E:\n            case 0x0F:\n            case 0x10:\n            case 0x11:\n            case 0x12:\n            case 0x13:\n            case 0x14:\n            case 0x15:\n            case 0x16:\n            case 0x17:\n                return sax->number_unsigned(static_cast<number_unsigned_t>(current));\n\n            case 0x18: // Unsigned integer (one-byte uint8_t follows)\n            {\n                std::uint8_t number{};\n                return get_number(input_format_t::cbor, number) && sax->number_unsigned(number);\n            }\n\n            case 0x19: // Unsigned integer (two-byte uint16_t follows)\n            {\n                std::uint16_t number{};\n                return get_number(input_format_t::cbor, number) && sax->number_unsigned(number);\n            }\n\n            case 0x1A: // Unsigned integer (four-byte uint32_t follows)\n            {\n                std::uint32_t number{};\n                return get_number(input_format_t::cbor, number) && sax->number_unsigned(number);\n            }\n\n            case 0x1B: // Unsigned integer (eight-byte uint64_t follows)\n            {\n                std::uint64_t number{};\n                return get_number(input_format_t::cbor, number) && sax->number_unsigned(number);\n            }\n\n            // Negative integer -1-0x00..-1-0x17 (-1..-24)\n            case 0x20:\n            case 0x21:\n            case 0x22:\n            case 0x23:\n            case 0x24:\n            case 0x25:\n            case 0x26:\n            case 0x27:\n            case 0x28:\n            case 0x29:\n            case 0x2A:\n            case 0x2B:\n            case 0x2C:\n            case 0x2D:\n            case 0x2E:\n            case 0x2F:\n            case 0x30:\n            case 0x31:\n            case 0x32:\n            case 0x33:\n            case 0x34:\n            case 0x35:\n            case 0x36:\n            case 0x37:\n                return sax->number_integer(static_cast<std::int8_t>(0x20 - 1 - current));\n\n            case 0x38: // Negative integer (one-byte uint8_t follows)\n            {\n                std::uint8_t number{};\n                return get_number(input_format_t::cbor, number) && sax->number_integer(static_cast<number_integer_t>(-1) - number);\n            }\n\n            case 0x39: // Negative integer -1-n (two-byte uint16_t follows)\n            {\n                std::uint16_t number{};\n                return get_number(input_format_t::cbor, number) && sax->number_integer(static_cast<number_integer_t>(-1) - number);\n            }\n\n            case 0x3A: // Negative integer -1-n (four-byte uint32_t follows)\n            {\n                std::uint32_t number{};\n                return get_number(input_format_t::cbor, number) && sax->number_integer(static_cast<number_integer_t>(-1) - number);\n            }\n\n            case 0x3B: // Negative integer -1-n (eight-byte uint64_t follows)\n            {\n                std::uint64_t number{};\n                return get_number(input_format_t::cbor, number) && sax->number_integer(static_cast<number_integer_t>(-1)\n                        - static_cast<number_integer_t>(number));\n            }\n\n            // Binary data (0x00..0x17 bytes follow)\n            case 0x40:\n            case 0x41:\n            case 0x42:\n            case 0x43:\n            case 0x44:\n            case 0x45:\n            case 0x46:\n            case 0x47:\n            case 0x48:\n            case 0x49:\n            case 0x4A:\n            case 0x4B:\n            case 0x4C:\n            case 0x4D:\n            case 0x4E:\n            case 0x4F:\n            case 0x50:\n            case 0x51:\n            case 0x52:\n            case 0x53:\n            case 0x54:\n            case 0x55:\n            case 0x56:\n            case 0x57:\n            case 0x58: // Binary data (one-byte uint8_t for n follows)\n            case 0x59: // Binary data (two-byte uint16_t for n follow)\n            case 0x5A: // Binary data (four-byte uint32_t for n follow)\n            case 0x5B: // Binary data (eight-byte uint64_t for n follow)\n            case 0x5F: // Binary data (indefinite length)\n            {\n                binary_t b;\n                return get_cbor_binary(b) && sax->binary(b);\n            }\n\n            // UTF-8 string (0x00..0x17 bytes follow)\n            case 0x60:\n            case 0x61:\n            case 0x62:\n            case 0x63:\n            case 0x64:\n            case 0x65:\n            case 0x66:\n            case 0x67:\n            case 0x68:\n            case 0x69:\n            case 0x6A:\n            case 0x6B:\n            case 0x6C:\n            case 0x6D:\n            case 0x6E:\n            case 0x6F:\n            case 0x70:\n            case 0x71:\n            case 0x72:\n            case 0x73:\n            case 0x74:\n            case 0x75:\n            case 0x76:\n            case 0x77:\n            case 0x78: // UTF-8 string (one-byte uint8_t for n follows)\n            case 0x79: // UTF-8 string (two-byte uint16_t for n follow)\n            case 0x7A: // UTF-8 string (four-byte uint32_t for n follow)\n            case 0x7B: // UTF-8 string (eight-byte uint64_t for n follow)\n            case 0x7F: // UTF-8 string (indefinite length)\n            {\n                string_t s;\n                return get_cbor_string(s) && sax->string(s);\n            }\n\n            // array (0x00..0x17 data items follow)\n            case 0x80:\n            case 0x81:\n            case 0x82:\n            case 0x83:\n            case 0x84:\n            case 0x85:\n            case 0x86:\n            case 0x87:\n            case 0x88:\n            case 0x89:\n            case 0x8A:\n            case 0x8B:\n            case 0x8C:\n            case 0x8D:\n            case 0x8E:\n            case 0x8F:\n            case 0x90:\n            case 0x91:\n            case 0x92:\n            case 0x93:\n            case 0x94:\n            case 0x95:\n            case 0x96:\n            case 0x97:\n                return get_cbor_array(\n                           conditional_static_cast<std::size_t>(static_cast<unsigned int>(current) & 0x1Fu), tag_handler);\n\n            case 0x98: // array (one-byte uint8_t for n follows)\n            {\n                std::uint8_t len{};\n                return get_number(input_format_t::cbor, len) && get_cbor_array(static_cast<std::size_t>(len), tag_handler);\n            }\n\n            case 0x99: // array (two-byte uint16_t for n follow)\n            {\n                std::uint16_t len{};\n                return get_number(input_format_t::cbor, len) && get_cbor_array(static_cast<std::size_t>(len), tag_handler);\n            }\n\n            case 0x9A: // array (four-byte uint32_t for n follow)\n            {\n                std::uint32_t len{};\n                return get_number(input_format_t::cbor, len) && get_cbor_array(conditional_static_cast<std::size_t>(len), tag_handler);\n            }\n\n            case 0x9B: // array (eight-byte uint64_t for n follow)\n            {\n                std::uint64_t len{};\n                return get_number(input_format_t::cbor, len) && get_cbor_array(conditional_static_cast<std::size_t>(len), tag_handler);\n            }\n\n            case 0x9F: // array (indefinite length)\n                return get_cbor_array(static_cast<std::size_t>(-1), tag_handler);\n\n            // map (0x00..0x17 pairs of data items follow)\n            case 0xA0:\n            case 0xA1:\n            case 0xA2:\n            case 0xA3:\n            case 0xA4:\n            case 0xA5:\n            case 0xA6:\n            case 0xA7:\n            case 0xA8:\n            case 0xA9:\n            case 0xAA:\n            case 0xAB:\n            case 0xAC:\n            case 0xAD:\n            case 0xAE:\n            case 0xAF:\n            case 0xB0:\n            case 0xB1:\n            case 0xB2:\n            case 0xB3:\n            case 0xB4:\n            case 0xB5:\n            case 0xB6:\n            case 0xB7:\n                return get_cbor_object(conditional_static_cast<std::size_t>(static_cast<unsigned int>(current) & 0x1Fu), tag_handler);\n\n            case 0xB8: // map (one-byte uint8_t for n follows)\n            {\n                std::uint8_t len{};\n                return get_number(input_format_t::cbor, len) && get_cbor_object(static_cast<std::size_t>(len), tag_handler);\n            }\n\n            case 0xB9: // map (two-byte uint16_t for n follow)\n            {\n                std::uint16_t len{};\n                return get_number(input_format_t::cbor, len) && get_cbor_object(static_cast<std::size_t>(len), tag_handler);\n            }\n\n            case 0xBA: // map (four-byte uint32_t for n follow)\n            {\n                std::uint32_t len{};\n                return get_number(input_format_t::cbor, len) && get_cbor_object(conditional_static_cast<std::size_t>(len), tag_handler);\n            }\n\n            case 0xBB: // map (eight-byte uint64_t for n follow)\n            {\n                std::uint64_t len{};\n                return get_number(input_format_t::cbor, len) && get_cbor_object(conditional_static_cast<std::size_t>(len), tag_handler);\n            }\n\n            case 0xBF: // map (indefinite length)\n                return get_cbor_object(static_cast<std::size_t>(-1), tag_handler);\n\n            case 0xC6: // tagged item\n            case 0xC7:\n            case 0xC8:\n            case 0xC9:\n            case 0xCA:\n            case 0xCB:\n            case 0xCC:\n            case 0xCD:\n            case 0xCE:\n            case 0xCF:\n            case 0xD0:\n            case 0xD1:\n            case 0xD2:\n            case 0xD3:\n            case 0xD4:\n            case 0xD8: // tagged item (1 bytes follow)\n            case 0xD9: // tagged item (2 bytes follow)\n            case 0xDA: // tagged item (4 bytes follow)\n            case 0xDB: // tagged item (8 bytes follow)\n            {\n                switch (tag_handler)\n                {\n                    case cbor_tag_handler_t::error:\n                    {\n                        auto last_token = get_token_string();\n                        return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read,\n                                                exception_message(input_format_t::cbor, concat(\"invalid byte: 0x\", last_token), \"value\"), nullptr));\n                    }\n\n                    case cbor_tag_handler_t::ignore:\n                    {\n                        // ignore binary subtype\n                        switch (current)\n                        {\n                            case 0xD8:\n                            {\n                                std::uint8_t subtype_to_ignore{};\n                                get_number(input_format_t::cbor, subtype_to_ignore);\n                                break;\n                            }\n                            case 0xD9:\n                            {\n                                std::uint16_t subtype_to_ignore{};\n                                get_number(input_format_t::cbor, subtype_to_ignore);\n                                break;\n                            }\n                            case 0xDA:\n                            {\n                                std::uint32_t subtype_to_ignore{};\n                                get_number(input_format_t::cbor, subtype_to_ignore);\n                                break;\n                            }\n                            case 0xDB:\n                            {\n                                std::uint64_t subtype_to_ignore{};\n                                get_number(input_format_t::cbor, subtype_to_ignore);\n                                break;\n                            }\n                            default:\n                                break;\n                        }\n                        return parse_cbor_internal(true, tag_handler);\n                    }\n\n                    case cbor_tag_handler_t::store:\n                    {\n                        binary_t b;\n                        // use binary subtype and store in binary container\n                        switch (current)\n                        {\n                            case 0xD8:\n                            {\n                                std::uint8_t subtype{};\n                                get_number(input_format_t::cbor, subtype);\n                                b.set_subtype(detail::conditional_static_cast<typename binary_t::subtype_type>(subtype));\n                                break;\n                            }\n                            case 0xD9:\n                            {\n                                std::uint16_t subtype{};\n                                get_number(input_format_t::cbor, subtype);\n                                b.set_subtype(detail::conditional_static_cast<typename binary_t::subtype_type>(subtype));\n                                break;\n                            }\n                            case 0xDA:\n                            {\n                                std::uint32_t subtype{};\n                                get_number(input_format_t::cbor, subtype);\n                                b.set_subtype(detail::conditional_static_cast<typename binary_t::subtype_type>(subtype));\n                                break;\n                            }\n                            case 0xDB:\n                            {\n                                std::uint64_t subtype{};\n                                get_number(input_format_t::cbor, subtype);\n                                b.set_subtype(detail::conditional_static_cast<typename binary_t::subtype_type>(subtype));\n                                break;\n                            }\n                            default:\n                                return parse_cbor_internal(true, tag_handler);\n                        }\n                        get();\n                        return get_cbor_binary(b) && sax->binary(b);\n                    }\n\n                    default:                 // LCOV_EXCL_LINE\n                        JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE\n                        return false;        // LCOV_EXCL_LINE\n                }\n            }\n\n            case 0xF4: // false\n                return sax->boolean(false);\n\n            case 0xF5: // true\n                return sax->boolean(true);\n\n            case 0xF6: // null\n                return sax->null();\n\n            case 0xF9: // Half-Precision Float (two-byte IEEE 754)\n            {\n                const auto byte1_raw = get();\n                if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::cbor, \"number\")))\n                {\n                    return false;\n                }\n                const auto byte2_raw = get();\n                if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::cbor, \"number\")))\n                {\n                    return false;\n                }\n\n                const auto byte1 = static_cast<unsigned char>(byte1_raw);\n                const auto byte2 = static_cast<unsigned char>(byte2_raw);\n\n                // code from RFC 7049, Appendix D, Figure 3:\n                // As half-precision floating-point numbers were only added\n                // to IEEE 754 in 2008, today's programming platforms often\n                // still only have limited support for them. It is very\n                // easy to include at least decoding support for them even\n                // without such support. An example of a small decoder for\n                // half-precision floating-point numbers in the C language\n                // is shown in Fig. 3.\n                const auto half = static_cast<unsigned int>((byte1 << 8u) + byte2);\n                const double val = [&half]\n                {\n                    const int exp = (half >> 10u) & 0x1Fu;\n                    const unsigned int mant = half & 0x3FFu;\n                    JSON_ASSERT(0 <= exp&& exp <= 32);\n                    JSON_ASSERT(mant <= 1024);\n                    switch (exp)\n                    {\n                        case 0:\n                            return std::ldexp(mant, -24);\n                        case 31:\n                            return (mant == 0)\n                            ? std::numeric_limits<double>::infinity()\n                            : std::numeric_limits<double>::quiet_NaN();\n                        default:\n                            return std::ldexp(mant + 1024, exp - 25);\n                    }\n                }();\n                return sax->number_float((half & 0x8000u) != 0\n                                         ? static_cast<number_float_t>(-val)\n                                         : static_cast<number_float_t>(val), \"\");\n            }\n\n            case 0xFA: // Single-Precision Float (four-byte IEEE 754)\n            {\n                float number{};\n                return get_number(input_format_t::cbor, number) && sax->number_float(static_cast<number_float_t>(number), \"\");\n            }\n\n            case 0xFB: // Double-Precision Float (eight-byte IEEE 754)\n            {\n                double number{};\n                return get_number(input_format_t::cbor, number) && sax->number_float(static_cast<number_float_t>(number), \"\");\n            }\n\n            default: // anything else (0xFF is handled inside the other types)\n            {\n                auto last_token = get_token_string();\n                return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read,\n                                        exception_message(input_format_t::cbor, concat(\"invalid byte: 0x\", last_token), \"value\"), nullptr));\n            }\n        }\n    }\n\n    /*!\n    @brief reads a CBOR string\n\n    This function first reads starting bytes to determine the expected\n    string length and then copies this number of bytes into a string.\n    Additionally, CBOR's strings with indefinite lengths are supported.\n\n    @param[out] result  created string\n\n    @return whether string creation completed\n    */\n    bool get_cbor_string(string_t& result)\n    {\n        if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::cbor, \"string\")))\n        {\n            return false;\n        }\n\n        switch (current)\n        {\n            // UTF-8 string (0x00..0x17 bytes follow)\n            case 0x60:\n            case 0x61:\n            case 0x62:\n            case 0x63:\n            case 0x64:\n            case 0x65:\n            case 0x66:\n            case 0x67:\n            case 0x68:\n            case 0x69:\n            case 0x6A:\n            case 0x6B:\n            case 0x6C:\n            case 0x6D:\n            case 0x6E:\n            case 0x6F:\n            case 0x70:\n            case 0x71:\n            case 0x72:\n            case 0x73:\n            case 0x74:\n            case 0x75:\n            case 0x76:\n            case 0x77:\n            {\n                return get_string(input_format_t::cbor, static_cast<unsigned int>(current) & 0x1Fu, result);\n            }\n\n            case 0x78: // UTF-8 string (one-byte uint8_t for n follows)\n            {\n                std::uint8_t len{};\n                return get_number(input_format_t::cbor, len) && get_string(input_format_t::cbor, len, result);\n            }\n\n            case 0x79: // UTF-8 string (two-byte uint16_t for n follow)\n            {\n                std::uint16_t len{};\n                return get_number(input_format_t::cbor, len) && get_string(input_format_t::cbor, len, result);\n            }\n\n            case 0x7A: // UTF-8 string (four-byte uint32_t for n follow)\n            {\n                std::uint32_t len{};\n                return get_number(input_format_t::cbor, len) && get_string(input_format_t::cbor, len, result);\n            }\n\n            case 0x7B: // UTF-8 string (eight-byte uint64_t for n follow)\n            {\n                std::uint64_t len{};\n                return get_number(input_format_t::cbor, len) && get_string(input_format_t::cbor, len, result);\n            }\n\n            case 0x7F: // UTF-8 string (indefinite length)\n            {\n                while (get() != 0xFF)\n                {\n                    string_t chunk;\n                    if (!get_cbor_string(chunk))\n                    {\n                        return false;\n                    }\n                    result.append(chunk);\n                }\n                return true;\n            }\n\n            default:\n            {\n                auto last_token = get_token_string();\n                return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read,\n                                        exception_message(input_format_t::cbor, concat(\"expected length specification (0x60-0x7B) or indefinite string type (0x7F); last byte: 0x\", last_token), \"string\"), nullptr));\n            }\n        }\n    }\n\n    /*!\n    @brief reads a CBOR byte array\n\n    This function first reads starting bytes to determine the expected\n    byte array length and then copies this number of bytes into the byte array.\n    Additionally, CBOR's byte arrays with indefinite lengths are supported.\n\n    @param[out] result  created byte array\n\n    @return whether byte array creation completed\n    */\n    bool get_cbor_binary(binary_t& result)\n    {\n        if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::cbor, \"binary\")))\n        {\n            return false;\n        }\n\n        switch (current)\n        {\n            // Binary data (0x00..0x17 bytes follow)\n            case 0x40:\n            case 0x41:\n            case 0x42:\n            case 0x43:\n            case 0x44:\n            case 0x45:\n            case 0x46:\n            case 0x47:\n            case 0x48:\n            case 0x49:\n            case 0x4A:\n            case 0x4B:\n            case 0x4C:\n            case 0x4D:\n            case 0x4E:\n            case 0x4F:\n            case 0x50:\n            case 0x51:\n            case 0x52:\n            case 0x53:\n            case 0x54:\n            case 0x55:\n            case 0x56:\n            case 0x57:\n            {\n                return get_binary(input_format_t::cbor, static_cast<unsigned int>(current) & 0x1Fu, result);\n            }\n\n            case 0x58: // Binary data (one-byte uint8_t for n follows)\n            {\n                std::uint8_t len{};\n                return get_number(input_format_t::cbor, len) &&\n                       get_binary(input_format_t::cbor, len, result);\n            }\n\n            case 0x59: // Binary data (two-byte uint16_t for n follow)\n            {\n                std::uint16_t len{};\n                return get_number(input_format_t::cbor, len) &&\n                       get_binary(input_format_t::cbor, len, result);\n            }\n\n            case 0x5A: // Binary data (four-byte uint32_t for n follow)\n            {\n                std::uint32_t len{};\n                return get_number(input_format_t::cbor, len) &&\n                       get_binary(input_format_t::cbor, len, result);\n            }\n\n            case 0x5B: // Binary data (eight-byte uint64_t for n follow)\n            {\n                std::uint64_t len{};\n                return get_number(input_format_t::cbor, len) &&\n                       get_binary(input_format_t::cbor, len, result);\n            }\n\n            case 0x5F: // Binary data (indefinite length)\n            {\n                while (get() != 0xFF)\n                {\n                    binary_t chunk;\n                    if (!get_cbor_binary(chunk))\n                    {\n                        return false;\n                    }\n                    result.insert(result.end(), chunk.begin(), chunk.end());\n                }\n                return true;\n            }\n\n            default:\n            {\n                auto last_token = get_token_string();\n                return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read,\n                                        exception_message(input_format_t::cbor, concat(\"expected length specification (0x40-0x5B) or indefinite binary array type (0x5F); last byte: 0x\", last_token), \"binary\"), nullptr));\n            }\n        }\n    }\n\n    /*!\n    @param[in] len  the length of the array or static_cast<std::size_t>(-1) for an\n                    array of indefinite size\n    @param[in] tag_handler how CBOR tags should be treated\n    @return whether array creation completed\n    */\n    bool get_cbor_array(const std::size_t len,\n                        const cbor_tag_handler_t tag_handler)\n    {\n        if (JSON_HEDLEY_UNLIKELY(!sax->start_array(len)))\n        {\n            return false;\n        }\n\n        if (len != static_cast<std::size_t>(-1))\n        {\n            for (std::size_t i = 0; i < len; ++i)\n            {\n                if (JSON_HEDLEY_UNLIKELY(!parse_cbor_internal(true, tag_handler)))\n                {\n                    return false;\n                }\n            }\n        }\n        else\n        {\n            while (get() != 0xFF)\n            {\n                if (JSON_HEDLEY_UNLIKELY(!parse_cbor_internal(false, tag_handler)))\n                {\n                    return false;\n                }\n            }\n        }\n\n        return sax->end_array();\n    }\n\n    /*!\n    @param[in] len  the length of the object or static_cast<std::size_t>(-1) for an\n                    object of indefinite size\n    @param[in] tag_handler how CBOR tags should be treated\n    @return whether object creation completed\n    */\n    bool get_cbor_object(const std::size_t len,\n                         const cbor_tag_handler_t tag_handler)\n    {\n        if (JSON_HEDLEY_UNLIKELY(!sax->start_object(len)))\n        {\n            return false;\n        }\n\n        if (len != 0)\n        {\n            string_t key;\n            if (len != static_cast<std::size_t>(-1))\n            {\n                for (std::size_t i = 0; i < len; ++i)\n                {\n                    get();\n                    if (JSON_HEDLEY_UNLIKELY(!get_cbor_string(key) || !sax->key(key)))\n                    {\n                        return false;\n                    }\n\n                    if (JSON_HEDLEY_UNLIKELY(!parse_cbor_internal(true, tag_handler)))\n                    {\n                        return false;\n                    }\n                    key.clear();\n                }\n            }\n            else\n            {\n                while (get() != 0xFF)\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!get_cbor_string(key) || !sax->key(key)))\n                    {\n                        return false;\n                    }\n\n                    if (JSON_HEDLEY_UNLIKELY(!parse_cbor_internal(true, tag_handler)))\n                    {\n                        return false;\n                    }\n                    key.clear();\n                }\n            }\n        }\n\n        return sax->end_object();\n    }\n\n    /////////////\n    // MsgPack //\n    /////////////\n\n    /*!\n    @return whether a valid MessagePack value was passed to the SAX parser\n    */\n    bool parse_msgpack_internal()\n    {\n        switch (get())\n        {\n            // EOF\n            case std::char_traits<char_type>::eof():\n                return unexpect_eof(input_format_t::msgpack, \"value\");\n\n            // positive fixint\n            case 0x00:\n            case 0x01:\n            case 0x02:\n            case 0x03:\n            case 0x04:\n            case 0x05:\n            case 0x06:\n            case 0x07:\n            case 0x08:\n            case 0x09:\n            case 0x0A:\n            case 0x0B:\n            case 0x0C:\n            case 0x0D:\n            case 0x0E:\n            case 0x0F:\n            case 0x10:\n            case 0x11:\n            case 0x12:\n            case 0x13:\n            case 0x14:\n            case 0x15:\n            case 0x16:\n            case 0x17:\n            case 0x18:\n            case 0x19:\n            case 0x1A:\n            case 0x1B:\n            case 0x1C:\n            case 0x1D:\n            case 0x1E:\n            case 0x1F:\n            case 0x20:\n            case 0x21:\n            case 0x22:\n            case 0x23:\n            case 0x24:\n            case 0x25:\n            case 0x26:\n            case 0x27:\n            case 0x28:\n            case 0x29:\n            case 0x2A:\n            case 0x2B:\n            case 0x2C:\n            case 0x2D:\n            case 0x2E:\n            case 0x2F:\n            case 0x30:\n            case 0x31:\n            case 0x32:\n            case 0x33:\n            case 0x34:\n            case 0x35:\n            case 0x36:\n            case 0x37:\n            case 0x38:\n            case 0x39:\n            case 0x3A:\n            case 0x3B:\n            case 0x3C:\n            case 0x3D:\n            case 0x3E:\n            case 0x3F:\n            case 0x40:\n            case 0x41:\n            case 0x42:\n            case 0x43:\n            case 0x44:\n            case 0x45:\n            case 0x46:\n            case 0x47:\n            case 0x48:\n            case 0x49:\n            case 0x4A:\n            case 0x4B:\n            case 0x4C:\n            case 0x4D:\n            case 0x4E:\n            case 0x4F:\n            case 0x50:\n            case 0x51:\n            case 0x52:\n            case 0x53:\n            case 0x54:\n            case 0x55:\n            case 0x56:\n            case 0x57:\n            case 0x58:\n            case 0x59:\n            case 0x5A:\n            case 0x5B:\n            case 0x5C:\n            case 0x5D:\n            case 0x5E:\n            case 0x5F:\n            case 0x60:\n            case 0x61:\n            case 0x62:\n            case 0x63:\n            case 0x64:\n            case 0x65:\n            case 0x66:\n            case 0x67:\n            case 0x68:\n            case 0x69:\n            case 0x6A:\n            case 0x6B:\n            case 0x6C:\n            case 0x6D:\n            case 0x6E:\n            case 0x6F:\n            case 0x70:\n            case 0x71:\n            case 0x72:\n            case 0x73:\n            case 0x74:\n            case 0x75:\n            case 0x76:\n            case 0x77:\n            case 0x78:\n            case 0x79:\n            case 0x7A:\n            case 0x7B:\n            case 0x7C:\n            case 0x7D:\n            case 0x7E:\n            case 0x7F:\n                return sax->number_unsigned(static_cast<number_unsigned_t>(current));\n\n            // fixmap\n            case 0x80:\n            case 0x81:\n            case 0x82:\n            case 0x83:\n            case 0x84:\n            case 0x85:\n            case 0x86:\n            case 0x87:\n            case 0x88:\n            case 0x89:\n            case 0x8A:\n            case 0x8B:\n            case 0x8C:\n            case 0x8D:\n            case 0x8E:\n            case 0x8F:\n                return get_msgpack_object(conditional_static_cast<std::size_t>(static_cast<unsigned int>(current) & 0x0Fu));\n\n            // fixarray\n            case 0x90:\n            case 0x91:\n            case 0x92:\n            case 0x93:\n            case 0x94:\n            case 0x95:\n            case 0x96:\n            case 0x97:\n            case 0x98:\n            case 0x99:\n            case 0x9A:\n            case 0x9B:\n            case 0x9C:\n            case 0x9D:\n            case 0x9E:\n            case 0x9F:\n                return get_msgpack_array(conditional_static_cast<std::size_t>(static_cast<unsigned int>(current) & 0x0Fu));\n\n            // fixstr\n            case 0xA0:\n            case 0xA1:\n            case 0xA2:\n            case 0xA3:\n            case 0xA4:\n            case 0xA5:\n            case 0xA6:\n            case 0xA7:\n            case 0xA8:\n            case 0xA9:\n            case 0xAA:\n            case 0xAB:\n            case 0xAC:\n            case 0xAD:\n            case 0xAE:\n            case 0xAF:\n            case 0xB0:\n            case 0xB1:\n            case 0xB2:\n            case 0xB3:\n            case 0xB4:\n            case 0xB5:\n            case 0xB6:\n            case 0xB7:\n            case 0xB8:\n            case 0xB9:\n            case 0xBA:\n            case 0xBB:\n            case 0xBC:\n            case 0xBD:\n            case 0xBE:\n            case 0xBF:\n            case 0xD9: // str 8\n            case 0xDA: // str 16\n            case 0xDB: // str 32\n            {\n                string_t s;\n                return get_msgpack_string(s) && sax->string(s);\n            }\n\n            case 0xC0: // nil\n                return sax->null();\n\n            case 0xC2: // false\n                return sax->boolean(false);\n\n            case 0xC3: // true\n                return sax->boolean(true);\n\n            case 0xC4: // bin 8\n            case 0xC5: // bin 16\n            case 0xC6: // bin 32\n            case 0xC7: // ext 8\n            case 0xC8: // ext 16\n            case 0xC9: // ext 32\n            case 0xD4: // fixext 1\n            case 0xD5: // fixext 2\n            case 0xD6: // fixext 4\n            case 0xD7: // fixext 8\n            case 0xD8: // fixext 16\n            {\n                binary_t b;\n                return get_msgpack_binary(b) && sax->binary(b);\n            }\n\n            case 0xCA: // float 32\n            {\n                float number{};\n                return get_number(input_format_t::msgpack, number) && sax->number_float(static_cast<number_float_t>(number), \"\");\n            }\n\n            case 0xCB: // float 64\n            {\n                double number{};\n                return get_number(input_format_t::msgpack, number) && sax->number_float(static_cast<number_float_t>(number), \"\");\n            }\n\n            case 0xCC: // uint 8\n            {\n                std::uint8_t number{};\n                return get_number(input_format_t::msgpack, number) && sax->number_unsigned(number);\n            }\n\n            case 0xCD: // uint 16\n            {\n                std::uint16_t number{};\n                return get_number(input_format_t::msgpack, number) && sax->number_unsigned(number);\n            }\n\n            case 0xCE: // uint 32\n            {\n                std::uint32_t number{};\n                return get_number(input_format_t::msgpack, number) && sax->number_unsigned(number);\n            }\n\n            case 0xCF: // uint 64\n            {\n                std::uint64_t number{};\n                return get_number(input_format_t::msgpack, number) && sax->number_unsigned(number);\n            }\n\n            case 0xD0: // int 8\n            {\n                std::int8_t number{};\n                return get_number(input_format_t::msgpack, number) && sax->number_integer(number);\n            }\n\n            case 0xD1: // int 16\n            {\n                std::int16_t number{};\n                return get_number(input_format_t::msgpack, number) && sax->number_integer(number);\n            }\n\n            case 0xD2: // int 32\n            {\n                std::int32_t number{};\n                return get_number(input_format_t::msgpack, number) && sax->number_integer(number);\n            }\n\n            case 0xD3: // int 64\n            {\n                std::int64_t number{};\n                return get_number(input_format_t::msgpack, number) && sax->number_integer(number);\n            }\n\n            case 0xDC: // array 16\n            {\n                std::uint16_t len{};\n                return get_number(input_format_t::msgpack, len) && get_msgpack_array(static_cast<std::size_t>(len));\n            }\n\n            case 0xDD: // array 32\n            {\n                std::uint32_t len{};\n                return get_number(input_format_t::msgpack, len) && get_msgpack_array(conditional_static_cast<std::size_t>(len));\n            }\n\n            case 0xDE: // map 16\n            {\n                std::uint16_t len{};\n                return get_number(input_format_t::msgpack, len) && get_msgpack_object(static_cast<std::size_t>(len));\n            }\n\n            case 0xDF: // map 32\n            {\n                std::uint32_t len{};\n                return get_number(input_format_t::msgpack, len) && get_msgpack_object(conditional_static_cast<std::size_t>(len));\n            }\n\n            // negative fixint\n            case 0xE0:\n            case 0xE1:\n            case 0xE2:\n            case 0xE3:\n            case 0xE4:\n            case 0xE5:\n            case 0xE6:\n            case 0xE7:\n            case 0xE8:\n            case 0xE9:\n            case 0xEA:\n            case 0xEB:\n            case 0xEC:\n            case 0xED:\n            case 0xEE:\n            case 0xEF:\n            case 0xF0:\n            case 0xF1:\n            case 0xF2:\n            case 0xF3:\n            case 0xF4:\n            case 0xF5:\n            case 0xF6:\n            case 0xF7:\n            case 0xF8:\n            case 0xF9:\n            case 0xFA:\n            case 0xFB:\n            case 0xFC:\n            case 0xFD:\n            case 0xFE:\n            case 0xFF:\n                return sax->number_integer(static_cast<std::int8_t>(current));\n\n            default: // anything else\n            {\n                auto last_token = get_token_string();\n                return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read,\n                                        exception_message(input_format_t::msgpack, concat(\"invalid byte: 0x\", last_token), \"value\"), nullptr));\n            }\n        }\n    }\n\n    /*!\n    @brief reads a MessagePack string\n\n    This function first reads starting bytes to determine the expected\n    string length and then copies this number of bytes into a string.\n\n    @param[out] result  created string\n\n    @return whether string creation completed\n    */\n    bool get_msgpack_string(string_t& result)\n    {\n        if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format_t::msgpack, \"string\")))\n        {\n            return false;\n        }\n\n        switch (current)\n        {\n            // fixstr\n            case 0xA0:\n            case 0xA1:\n            case 0xA2:\n            case 0xA3:\n            case 0xA4:\n            case 0xA5:\n            case 0xA6:\n            case 0xA7:\n            case 0xA8:\n            case 0xA9:\n            case 0xAA:\n            case 0xAB:\n            case 0xAC:\n            case 0xAD:\n            case 0xAE:\n            case 0xAF:\n            case 0xB0:\n            case 0xB1:\n            case 0xB2:\n            case 0xB3:\n            case 0xB4:\n            case 0xB5:\n            case 0xB6:\n            case 0xB7:\n            case 0xB8:\n            case 0xB9:\n            case 0xBA:\n            case 0xBB:\n            case 0xBC:\n            case 0xBD:\n            case 0xBE:\n            case 0xBF:\n            {\n                return get_string(input_format_t::msgpack, static_cast<unsigned int>(current) & 0x1Fu, result);\n            }\n\n            case 0xD9: // str 8\n            {\n                std::uint8_t len{};\n                return get_number(input_format_t::msgpack, len) && get_string(input_format_t::msgpack, len, result);\n            }\n\n            case 0xDA: // str 16\n            {\n                std::uint16_t len{};\n                return get_number(input_format_t::msgpack, len) && get_string(input_format_t::msgpack, len, result);\n            }\n\n            case 0xDB: // str 32\n            {\n                std::uint32_t len{};\n                return get_number(input_format_t::msgpack, len) && get_string(input_format_t::msgpack, len, result);\n            }\n\n            default:\n            {\n                auto last_token = get_token_string();\n                return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read,\n                                        exception_message(input_format_t::msgpack, concat(\"expected length specification (0xA0-0xBF, 0xD9-0xDB); last byte: 0x\", last_token), \"string\"), nullptr));\n            }\n        }\n    }\n\n    /*!\n    @brief reads a MessagePack byte array\n\n    This function first reads starting bytes to determine the expected\n    byte array length and then copies this number of bytes into a byte array.\n\n    @param[out] result  created byte array\n\n    @return whether byte array creation completed\n    */\n    bool get_msgpack_binary(binary_t& result)\n    {\n        // helper function to set the subtype\n        auto assign_and_return_true = [&result](std::int8_t subtype)\n        {\n            result.set_subtype(static_cast<std::uint8_t>(subtype));\n            return true;\n        };\n\n        switch (current)\n        {\n            case 0xC4: // bin 8\n            {\n                std::uint8_t len{};\n                return get_number(input_format_t::msgpack, len) &&\n                       get_binary(input_format_t::msgpack, len, result);\n            }\n\n            case 0xC5: // bin 16\n            {\n                std::uint16_t len{};\n                return get_number(input_format_t::msgpack, len) &&\n                       get_binary(input_format_t::msgpack, len, result);\n            }\n\n            case 0xC6: // bin 32\n            {\n                std::uint32_t len{};\n                return get_number(input_format_t::msgpack, len) &&\n                       get_binary(input_format_t::msgpack, len, result);\n            }\n\n            case 0xC7: // ext 8\n            {\n                std::uint8_t len{};\n                std::int8_t subtype{};\n                return get_number(input_format_t::msgpack, len) &&\n                       get_number(input_format_t::msgpack, subtype) &&\n                       get_binary(input_format_t::msgpack, len, result) &&\n                       assign_and_return_true(subtype);\n            }\n\n            case 0xC8: // ext 16\n            {\n                std::uint16_t len{};\n                std::int8_t subtype{};\n                return get_number(input_format_t::msgpack, len) &&\n                       get_number(input_format_t::msgpack, subtype) &&\n                       get_binary(input_format_t::msgpack, len, result) &&\n                       assign_and_return_true(subtype);\n            }\n\n            case 0xC9: // ext 32\n            {\n                std::uint32_t len{};\n                std::int8_t subtype{};\n                return get_number(input_format_t::msgpack, len) &&\n                       get_number(input_format_t::msgpack, subtype) &&\n                       get_binary(input_format_t::msgpack, len, result) &&\n                       assign_and_return_true(subtype);\n            }\n\n            case 0xD4: // fixext 1\n            {\n                std::int8_t subtype{};\n                return get_number(input_format_t::msgpack, subtype) &&\n                       get_binary(input_format_t::msgpack, 1, result) &&\n                       assign_and_return_true(subtype);\n            }\n\n            case 0xD5: // fixext 2\n            {\n                std::int8_t subtype{};\n                return get_number(input_format_t::msgpack, subtype) &&\n                       get_binary(input_format_t::msgpack, 2, result) &&\n                       assign_and_return_true(subtype);\n            }\n\n            case 0xD6: // fixext 4\n            {\n                std::int8_t subtype{};\n                return get_number(input_format_t::msgpack, subtype) &&\n                       get_binary(input_format_t::msgpack, 4, result) &&\n                       assign_and_return_true(subtype);\n            }\n\n            case 0xD7: // fixext 8\n            {\n                std::int8_t subtype{};\n                return get_number(input_format_t::msgpack, subtype) &&\n                       get_binary(input_format_t::msgpack, 8, result) &&\n                       assign_and_return_true(subtype);\n            }\n\n            case 0xD8: // fixext 16\n            {\n                std::int8_t subtype{};\n                return get_number(input_format_t::msgpack, subtype) &&\n                       get_binary(input_format_t::msgpack, 16, result) &&\n                       assign_and_return_true(subtype);\n            }\n\n            default:           // LCOV_EXCL_LINE\n                return false;  // LCOV_EXCL_LINE\n        }\n    }\n\n    /*!\n    @param[in] len  the length of the array\n    @return whether array creation completed\n    */\n    bool get_msgpack_array(const std::size_t len)\n    {\n        if (JSON_HEDLEY_UNLIKELY(!sax->start_array(len)))\n        {\n            return false;\n        }\n\n        for (std::size_t i = 0; i < len; ++i)\n        {\n            if (JSON_HEDLEY_UNLIKELY(!parse_msgpack_internal()))\n            {\n                return false;\n            }\n        }\n\n        return sax->end_array();\n    }\n\n    /*!\n    @param[in] len  the length of the object\n    @return whether object creation completed\n    */\n    bool get_msgpack_object(const std::size_t len)\n    {\n        if (JSON_HEDLEY_UNLIKELY(!sax->start_object(len)))\n        {\n            return false;\n        }\n\n        string_t key;\n        for (std::size_t i = 0; i < len; ++i)\n        {\n            get();\n            if (JSON_HEDLEY_UNLIKELY(!get_msgpack_string(key) || !sax->key(key)))\n            {\n                return false;\n            }\n\n            if (JSON_HEDLEY_UNLIKELY(!parse_msgpack_internal()))\n            {\n                return false;\n            }\n            key.clear();\n        }\n\n        return sax->end_object();\n    }\n\n    ////////////\n    // UBJSON //\n    ////////////\n\n    /*!\n    @param[in] get_char  whether a new character should be retrieved from the\n                         input (true, default) or whether the last read\n                         character should be considered instead\n\n    @return whether a valid UBJSON value was passed to the SAX parser\n    */\n    bool parse_ubjson_internal(const bool get_char = true)\n    {\n        return get_ubjson_value(get_char ? get_ignore_noop() : current);\n    }\n\n    /*!\n    @brief reads a UBJSON string\n\n    This function is either called after reading the 'S' byte explicitly\n    indicating a string, or in case of an object key where the 'S' byte can be\n    left out.\n\n    @param[out] result   created string\n    @param[in] get_char  whether a new character should be retrieved from the\n                         input (true, default) or whether the last read\n                         character should be considered instead\n\n    @return whether string creation completed\n    */\n    bool get_ubjson_string(string_t& result, const bool get_char = true)\n    {\n        if (get_char)\n        {\n            get();  // TODO(niels): may we ignore N here?\n        }\n\n        if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, \"value\")))\n        {\n            return false;\n        }\n\n        switch (current)\n        {\n            case 'U':\n            {\n                std::uint8_t len{};\n                return get_number(input_format, len) && get_string(input_format, len, result);\n            }\n\n            case 'i':\n            {\n                std::int8_t len{};\n                return get_number(input_format, len) && get_string(input_format, len, result);\n            }\n\n            case 'I':\n            {\n                std::int16_t len{};\n                return get_number(input_format, len) && get_string(input_format, len, result);\n            }\n\n            case 'l':\n            {\n                std::int32_t len{};\n                return get_number(input_format, len) && get_string(input_format, len, result);\n            }\n\n            case 'L':\n            {\n                std::int64_t len{};\n                return get_number(input_format, len) && get_string(input_format, len, result);\n            }\n\n            case 'u':\n            {\n                if (input_format != input_format_t::bjdata)\n                {\n                    break;\n                }\n                std::uint16_t len{};\n                return get_number(input_format, len) && get_string(input_format, len, result);\n            }\n\n            case 'm':\n            {\n                if (input_format != input_format_t::bjdata)\n                {\n                    break;\n                }\n                std::uint32_t len{};\n                return get_number(input_format, len) && get_string(input_format, len, result);\n            }\n\n            case 'M':\n            {\n                if (input_format != input_format_t::bjdata)\n                {\n                    break;\n                }\n                std::uint64_t len{};\n                return get_number(input_format, len) && get_string(input_format, len, result);\n            }\n\n            default:\n                break;\n        }\n        auto last_token = get_token_string();\n        std::string message;\n\n        if (input_format != input_format_t::bjdata)\n        {\n            message = \"expected length type specification (U, i, I, l, L); last byte: 0x\" + last_token;\n        }\n        else\n        {\n            message = \"expected length type specification (U, i, u, I, m, l, M, L); last byte: 0x\" + last_token;\n        }\n        return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, exception_message(input_format, message, \"string\"), nullptr));\n    }\n\n    /*!\n    @param[out] dim  an integer vector storing the ND array dimensions\n    @return whether reading ND array size vector is successful\n    */\n    bool get_ubjson_ndarray_size(std::vector<size_t>& dim)\n    {\n        std::pair<std::size_t, char_int_type> size_and_type;\n        size_t dimlen = 0;\n        bool no_ndarray = true;\n\n        if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_type(size_and_type, no_ndarray)))\n        {\n            return false;\n        }\n\n        if (size_and_type.first != npos)\n        {\n            if (size_and_type.second != 0)\n            {\n                if (size_and_type.second != 'N')\n                {\n                    for (std::size_t i = 0; i < size_and_type.first; ++i)\n                    {\n                        if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_value(dimlen, no_ndarray, size_and_type.second)))\n                        {\n                            return false;\n                        }\n                        dim.push_back(dimlen);\n                    }\n                }\n            }\n            else\n            {\n                for (std::size_t i = 0; i < size_and_type.first; ++i)\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_value(dimlen, no_ndarray)))\n                    {\n                        return false;\n                    }\n                    dim.push_back(dimlen);\n                }\n            }\n        }\n        else\n        {\n            while (current != ']')\n            {\n                if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_value(dimlen, no_ndarray, current)))\n                {\n                    return false;\n                }\n                dim.push_back(dimlen);\n                get_ignore_noop();\n            }\n        }\n        return true;\n    }\n\n    /*!\n    @param[out] result  determined size\n    @param[in,out] is_ndarray  for input, `true` means already inside an ndarray vector\n                               or ndarray dimension is not allowed; `false` means ndarray\n                               is allowed; for output, `true` means an ndarray is found;\n                               is_ndarray can only return `true` when its initial value\n                               is `false`\n    @param[in] prefix  type marker if already read, otherwise set to 0\n\n    @return whether size determination completed\n    */\n    bool get_ubjson_size_value(std::size_t& result, bool& is_ndarray, char_int_type prefix = 0)\n    {\n        if (prefix == 0)\n        {\n            prefix = get_ignore_noop();\n        }\n\n        switch (prefix)\n        {\n            case 'U':\n            {\n                std::uint8_t number{};\n                if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number)))\n                {\n                    return false;\n                }\n                result = static_cast<std::size_t>(number);\n                return true;\n            }\n\n            case 'i':\n            {\n                std::int8_t number{};\n                if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number)))\n                {\n                    return false;\n                }\n                if (number < 0)\n                {\n                    return sax->parse_error(chars_read, get_token_string(), parse_error::create(113, chars_read,\n                                            exception_message(input_format, \"count in an optimized container must be positive\", \"size\"), nullptr));\n                }\n                result = static_cast<std::size_t>(number); // NOLINT(bugprone-signed-char-misuse,cert-str34-c): number is not a char\n                return true;\n            }\n\n            case 'I':\n            {\n                std::int16_t number{};\n                if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number)))\n                {\n                    return false;\n                }\n                if (number < 0)\n                {\n                    return sax->parse_error(chars_read, get_token_string(), parse_error::create(113, chars_read,\n                                            exception_message(input_format, \"count in an optimized container must be positive\", \"size\"), nullptr));\n                }\n                result = static_cast<std::size_t>(number);\n                return true;\n            }\n\n            case 'l':\n            {\n                std::int32_t number{};\n                if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number)))\n                {\n                    return false;\n                }\n                if (number < 0)\n                {\n                    return sax->parse_error(chars_read, get_token_string(), parse_error::create(113, chars_read,\n                                            exception_message(input_format, \"count in an optimized container must be positive\", \"size\"), nullptr));\n                }\n                result = static_cast<std::size_t>(number);\n                return true;\n            }\n\n            case 'L':\n            {\n                std::int64_t number{};\n                if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number)))\n                {\n                    return false;\n                }\n                if (number < 0)\n                {\n                    return sax->parse_error(chars_read, get_token_string(), parse_error::create(113, chars_read,\n                                            exception_message(input_format, \"count in an optimized container must be positive\", \"size\"), nullptr));\n                }\n                if (!value_in_range_of<std::size_t>(number))\n                {\n                    return sax->parse_error(chars_read, get_token_string(), out_of_range::create(408,\n                                            exception_message(input_format, \"integer value overflow\", \"size\"), nullptr));\n                }\n                result = static_cast<std::size_t>(number);\n                return true;\n            }\n\n            case 'u':\n            {\n                if (input_format != input_format_t::bjdata)\n                {\n                    break;\n                }\n                std::uint16_t number{};\n                if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number)))\n                {\n                    return false;\n                }\n                result = static_cast<std::size_t>(number);\n                return true;\n            }\n\n            case 'm':\n            {\n                if (input_format != input_format_t::bjdata)\n                {\n                    break;\n                }\n                std::uint32_t number{};\n                if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number)))\n                {\n                    return false;\n                }\n                result = conditional_static_cast<std::size_t>(number);\n                return true;\n            }\n\n            case 'M':\n            {\n                if (input_format != input_format_t::bjdata)\n                {\n                    break;\n                }\n                std::uint64_t number{};\n                if (JSON_HEDLEY_UNLIKELY(!get_number(input_format, number)))\n                {\n                    return false;\n                }\n                if (!value_in_range_of<std::size_t>(number))\n                {\n                    return sax->parse_error(chars_read, get_token_string(), out_of_range::create(408,\n                                            exception_message(input_format, \"integer value overflow\", \"size\"), nullptr));\n                }\n                result = detail::conditional_static_cast<std::size_t>(number);\n                return true;\n            }\n\n            case '[':\n            {\n                if (input_format != input_format_t::bjdata)\n                {\n                    break;\n                }\n                if (is_ndarray) // ndarray dimensional vector can only contain integers, and can not embed another array\n                {\n                    return sax->parse_error(chars_read, get_token_string(), parse_error::create(113, chars_read, exception_message(input_format, \"ndarray dimentional vector is not allowed\", \"size\"), nullptr));\n                }\n                std::vector<size_t> dim;\n                if (JSON_HEDLEY_UNLIKELY(!get_ubjson_ndarray_size(dim)))\n                {\n                    return false;\n                }\n                if (dim.size() == 1 || (dim.size() == 2 && dim.at(0) == 1)) // return normal array size if 1D row vector\n                {\n                    result = dim.at(dim.size() - 1);\n                    return true;\n                }\n                if (!dim.empty())  // if ndarray, convert to an object in JData annotated array format\n                {\n                    for (auto i : dim) // test if any dimension in an ndarray is 0, if so, return a 1D empty container\n                    {\n                        if ( i == 0 )\n                        {\n                            result = 0;\n                            return true;\n                        }\n                    }\n\n                    string_t key = \"_ArraySize_\";\n                    if (JSON_HEDLEY_UNLIKELY(!sax->start_object(3) || !sax->key(key) || !sax->start_array(dim.size())))\n                    {\n                        return false;\n                    }\n                    result = 1;\n                    for (auto i : dim)\n                    {\n                        result *= i;\n                        if (result == 0 || result == npos) // because dim elements shall not have zeros, result = 0 means overflow happened; it also can't be npos as it is used to initialize size in get_ubjson_size_type()\n                        {\n                            return sax->parse_error(chars_read, get_token_string(), out_of_range::create(408, exception_message(input_format, \"excessive ndarray size caused overflow\", \"size\"), nullptr));\n                        }\n                        if (JSON_HEDLEY_UNLIKELY(!sax->number_unsigned(static_cast<number_unsigned_t>(i))))\n                        {\n                            return false;\n                        }\n                    }\n                    is_ndarray = true;\n                    return sax->end_array();\n                }\n                result = 0;\n                return true;\n            }\n\n            default:\n                break;\n        }\n        auto last_token = get_token_string();\n        std::string message;\n\n        if (input_format != input_format_t::bjdata)\n        {\n            message = \"expected length type specification (U, i, I, l, L) after '#'; last byte: 0x\" + last_token;\n        }\n        else\n        {\n            message = \"expected length type specification (U, i, u, I, m, l, M, L) after '#'; last byte: 0x\" + last_token;\n        }\n        return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, exception_message(input_format, message, \"size\"), nullptr));\n    }\n\n    /*!\n    @brief determine the type and size for a container\n\n    In the optimized UBJSON format, a type and a size can be provided to allow\n    for a more compact representation.\n\n    @param[out] result  pair of the size and the type\n    @param[in] inside_ndarray  whether the parser is parsing an ND array dimensional vector\n\n    @return whether pair creation completed\n    */\n    bool get_ubjson_size_type(std::pair<std::size_t, char_int_type>& result, bool inside_ndarray = false)\n    {\n        result.first = npos; // size\n        result.second = 0; // type\n        bool is_ndarray = false;\n\n        get_ignore_noop();\n\n        if (current == '$')\n        {\n            result.second = get();  // must not ignore 'N', because 'N' maybe the type\n            if (input_format == input_format_t::bjdata\n                    && JSON_HEDLEY_UNLIKELY(std::binary_search(bjd_optimized_type_markers.begin(), bjd_optimized_type_markers.end(), result.second)))\n            {\n                auto last_token = get_token_string();\n                return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read,\n                                        exception_message(input_format, concat(\"marker 0x\", last_token, \" is not a permitted optimized array type\"), \"type\"), nullptr));\n            }\n\n            if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, \"type\")))\n            {\n                return false;\n            }\n\n            get_ignore_noop();\n            if (JSON_HEDLEY_UNLIKELY(current != '#'))\n            {\n                if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, \"value\")))\n                {\n                    return false;\n                }\n                auto last_token = get_token_string();\n                return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read,\n                                        exception_message(input_format, concat(\"expected '#' after type information; last byte: 0x\", last_token), \"size\"), nullptr));\n            }\n\n            bool is_error = get_ubjson_size_value(result.first, is_ndarray);\n            if (input_format == input_format_t::bjdata && is_ndarray)\n            {\n                if (inside_ndarray)\n                {\n                    return sax->parse_error(chars_read, get_token_string(), parse_error::create(112, chars_read,\n                                            exception_message(input_format, \"ndarray can not be recursive\", \"size\"), nullptr));\n                }\n                result.second |= (1 << 8); // use bit 8 to indicate ndarray, all UBJSON and BJData markers should be ASCII letters\n            }\n            return is_error;\n        }\n\n        if (current == '#')\n        {\n            bool is_error = get_ubjson_size_value(result.first, is_ndarray);\n            if (input_format == input_format_t::bjdata && is_ndarray)\n            {\n                return sax->parse_error(chars_read, get_token_string(), parse_error::create(112, chars_read,\n                                        exception_message(input_format, \"ndarray requires both type and size\", \"size\"), nullptr));\n            }\n            return is_error;\n        }\n\n        return true;\n    }\n\n    /*!\n    @param prefix  the previously read or set type prefix\n    @return whether value creation completed\n    */\n    bool get_ubjson_value(const char_int_type prefix)\n    {\n        switch (prefix)\n        {\n            case std::char_traits<char_type>::eof():  // EOF\n                return unexpect_eof(input_format, \"value\");\n\n            case 'T':  // true\n                return sax->boolean(true);\n            case 'F':  // false\n                return sax->boolean(false);\n\n            case 'Z':  // null\n                return sax->null();\n\n            case 'U':\n            {\n                std::uint8_t number{};\n                return get_number(input_format, number) && sax->number_unsigned(number);\n            }\n\n            case 'i':\n            {\n                std::int8_t number{};\n                return get_number(input_format, number) && sax->number_integer(number);\n            }\n\n            case 'I':\n            {\n                std::int16_t number{};\n                return get_number(input_format, number) && sax->number_integer(number);\n            }\n\n            case 'l':\n            {\n                std::int32_t number{};\n                return get_number(input_format, number) && sax->number_integer(number);\n            }\n\n            case 'L':\n            {\n                std::int64_t number{};\n                return get_number(input_format, number) && sax->number_integer(number);\n            }\n\n            case 'u':\n            {\n                if (input_format != input_format_t::bjdata)\n                {\n                    break;\n                }\n                std::uint16_t number{};\n                return get_number(input_format, number) && sax->number_unsigned(number);\n            }\n\n            case 'm':\n            {\n                if (input_format != input_format_t::bjdata)\n                {\n                    break;\n                }\n                std::uint32_t number{};\n                return get_number(input_format, number) && sax->number_unsigned(number);\n            }\n\n            case 'M':\n            {\n                if (input_format != input_format_t::bjdata)\n                {\n                    break;\n                }\n                std::uint64_t number{};\n                return get_number(input_format, number) && sax->number_unsigned(number);\n            }\n\n            case 'h':\n            {\n                if (input_format != input_format_t::bjdata)\n                {\n                    break;\n                }\n                const auto byte1_raw = get();\n                if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, \"number\")))\n                {\n                    return false;\n                }\n                const auto byte2_raw = get();\n                if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, \"number\")))\n                {\n                    return false;\n                }\n\n                const auto byte1 = static_cast<unsigned char>(byte1_raw);\n                const auto byte2 = static_cast<unsigned char>(byte2_raw);\n\n                // code from RFC 7049, Appendix D, Figure 3:\n                // As half-precision floating-point numbers were only added\n                // to IEEE 754 in 2008, today's programming platforms often\n                // still only have limited support for them. It is very\n                // easy to include at least decoding support for them even\n                // without such support. An example of a small decoder for\n                // half-precision floating-point numbers in the C language\n                // is shown in Fig. 3.\n                const auto half = static_cast<unsigned int>((byte2 << 8u) + byte1);\n                const double val = [&half]\n                {\n                    const int exp = (half >> 10u) & 0x1Fu;\n                    const unsigned int mant = half & 0x3FFu;\n                    JSON_ASSERT(0 <= exp&& exp <= 32);\n                    JSON_ASSERT(mant <= 1024);\n                    switch (exp)\n                    {\n                        case 0:\n                            return std::ldexp(mant, -24);\n                        case 31:\n                            return (mant == 0)\n                            ? std::numeric_limits<double>::infinity()\n                            : std::numeric_limits<double>::quiet_NaN();\n                        default:\n                            return std::ldexp(mant + 1024, exp - 25);\n                    }\n                }();\n                return sax->number_float((half & 0x8000u) != 0\n                                         ? static_cast<number_float_t>(-val)\n                                         : static_cast<number_float_t>(val), \"\");\n            }\n\n            case 'd':\n            {\n                float number{};\n                return get_number(input_format, number) && sax->number_float(static_cast<number_float_t>(number), \"\");\n            }\n\n            case 'D':\n            {\n                double number{};\n                return get_number(input_format, number) && sax->number_float(static_cast<number_float_t>(number), \"\");\n            }\n\n            case 'H':\n            {\n                return get_ubjson_high_precision_number();\n            }\n\n            case 'C':  // char\n            {\n                get();\n                if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, \"char\")))\n                {\n                    return false;\n                }\n                if (JSON_HEDLEY_UNLIKELY(current > 127))\n                {\n                    auto last_token = get_token_string();\n                    return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read,\n                                            exception_message(input_format, concat(\"byte after 'C' must be in range 0x00..0x7F; last byte: 0x\", last_token), \"char\"), nullptr));\n                }\n                string_t s(1, static_cast<typename string_t::value_type>(current));\n                return sax->string(s);\n            }\n\n            case 'S':  // string\n            {\n                string_t s;\n                return get_ubjson_string(s) && sax->string(s);\n            }\n\n            case '[':  // array\n                return get_ubjson_array();\n\n            case '{':  // object\n                return get_ubjson_object();\n\n            default: // anything else\n                break;\n        }\n        auto last_token = get_token_string();\n        return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, exception_message(input_format, \"invalid byte: 0x\" + last_token, \"value\"), nullptr));\n    }\n\n    /*!\n    @return whether array creation completed\n    */\n    bool get_ubjson_array()\n    {\n        std::pair<std::size_t, char_int_type> size_and_type;\n        if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_type(size_and_type)))\n        {\n            return false;\n        }\n\n        // if bit-8 of size_and_type.second is set to 1, encode bjdata ndarray as an object in JData annotated array format (https://github.com/NeuroJSON/jdata):\n        // {\"_ArrayType_\" : \"typeid\", \"_ArraySize_\" : [n1, n2, ...], \"_ArrayData_\" : [v1, v2, ...]}\n\n        if (input_format == input_format_t::bjdata && size_and_type.first != npos && (size_and_type.second & (1 << 8)) != 0)\n        {\n            size_and_type.second &= ~(static_cast<char_int_type>(1) << 8);  // use bit 8 to indicate ndarray, here we remove the bit to restore the type marker\n            auto it = std::lower_bound(bjd_types_map.begin(), bjd_types_map.end(), size_and_type.second, [](const bjd_type & p, char_int_type t)\n            {\n                return p.first < t;\n            });\n            string_t key = \"_ArrayType_\";\n            if (JSON_HEDLEY_UNLIKELY(it == bjd_types_map.end() || it->first != size_and_type.second))\n            {\n                auto last_token = get_token_string();\n                return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read,\n                                        exception_message(input_format, \"invalid byte: 0x\" + last_token, \"type\"), nullptr));\n            }\n\n            string_t type = it->second; // sax->string() takes a reference\n            if (JSON_HEDLEY_UNLIKELY(!sax->key(key) || !sax->string(type)))\n            {\n                return false;\n            }\n\n            if (size_and_type.second == 'C')\n            {\n                size_and_type.second = 'U';\n            }\n\n            key = \"_ArrayData_\";\n            if (JSON_HEDLEY_UNLIKELY(!sax->key(key) || !sax->start_array(size_and_type.first) ))\n            {\n                return false;\n            }\n\n            for (std::size_t i = 0; i < size_and_type.first; ++i)\n            {\n                if (JSON_HEDLEY_UNLIKELY(!get_ubjson_value(size_and_type.second)))\n                {\n                    return false;\n                }\n            }\n\n            return (sax->end_array() && sax->end_object());\n        }\n\n        if (size_and_type.first != npos)\n        {\n            if (JSON_HEDLEY_UNLIKELY(!sax->start_array(size_and_type.first)))\n            {\n                return false;\n            }\n\n            if (size_and_type.second != 0)\n            {\n                if (size_and_type.second != 'N')\n                {\n                    for (std::size_t i = 0; i < size_and_type.first; ++i)\n                    {\n                        if (JSON_HEDLEY_UNLIKELY(!get_ubjson_value(size_and_type.second)))\n                        {\n                            return false;\n                        }\n                    }\n                }\n            }\n            else\n            {\n                for (std::size_t i = 0; i < size_and_type.first; ++i)\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!parse_ubjson_internal()))\n                    {\n                        return false;\n                    }\n                }\n            }\n        }\n        else\n        {\n            if (JSON_HEDLEY_UNLIKELY(!sax->start_array(static_cast<std::size_t>(-1))))\n            {\n                return false;\n            }\n\n            while (current != ']')\n            {\n                if (JSON_HEDLEY_UNLIKELY(!parse_ubjson_internal(false)))\n                {\n                    return false;\n                }\n                get_ignore_noop();\n            }\n        }\n\n        return sax->end_array();\n    }\n\n    /*!\n    @return whether object creation completed\n    */\n    bool get_ubjson_object()\n    {\n        std::pair<std::size_t, char_int_type> size_and_type;\n        if (JSON_HEDLEY_UNLIKELY(!get_ubjson_size_type(size_and_type)))\n        {\n            return false;\n        }\n\n        // do not accept ND-array size in objects in BJData\n        if (input_format == input_format_t::bjdata && size_and_type.first != npos && (size_and_type.second & (1 << 8)) != 0)\n        {\n            auto last_token = get_token_string();\n            return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read,\n                                    exception_message(input_format, \"BJData object does not support ND-array size in optimized format\", \"object\"), nullptr));\n        }\n\n        string_t key;\n        if (size_and_type.first != npos)\n        {\n            if (JSON_HEDLEY_UNLIKELY(!sax->start_object(size_and_type.first)))\n            {\n                return false;\n            }\n\n            if (size_and_type.second != 0)\n            {\n                for (std::size_t i = 0; i < size_and_type.first; ++i)\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!get_ubjson_string(key) || !sax->key(key)))\n                    {\n                        return false;\n                    }\n                    if (JSON_HEDLEY_UNLIKELY(!get_ubjson_value(size_and_type.second)))\n                    {\n                        return false;\n                    }\n                    key.clear();\n                }\n            }\n            else\n            {\n                for (std::size_t i = 0; i < size_and_type.first; ++i)\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!get_ubjson_string(key) || !sax->key(key)))\n                    {\n                        return false;\n                    }\n                    if (JSON_HEDLEY_UNLIKELY(!parse_ubjson_internal()))\n                    {\n                        return false;\n                    }\n                    key.clear();\n                }\n            }\n        }\n        else\n        {\n            if (JSON_HEDLEY_UNLIKELY(!sax->start_object(static_cast<std::size_t>(-1))))\n            {\n                return false;\n            }\n\n            while (current != '}')\n            {\n                if (JSON_HEDLEY_UNLIKELY(!get_ubjson_string(key, false) || !sax->key(key)))\n                {\n                    return false;\n                }\n                if (JSON_HEDLEY_UNLIKELY(!parse_ubjson_internal()))\n                {\n                    return false;\n                }\n                get_ignore_noop();\n                key.clear();\n            }\n        }\n\n        return sax->end_object();\n    }\n\n    // Note, no reader for UBJSON binary types is implemented because they do\n    // not exist\n\n    bool get_ubjson_high_precision_number()\n    {\n        // get size of following number string\n        std::size_t size{};\n        bool no_ndarray = true;\n        auto res = get_ubjson_size_value(size, no_ndarray);\n        if (JSON_HEDLEY_UNLIKELY(!res))\n        {\n            return res;\n        }\n\n        // get number string\n        std::vector<char> number_vector;\n        for (std::size_t i = 0; i < size; ++i)\n        {\n            get();\n            if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(input_format, \"number\")))\n            {\n                return false;\n            }\n            number_vector.push_back(static_cast<char>(current));\n        }\n\n        // parse number string\n        using ia_type = decltype(detail::input_adapter(number_vector));\n        auto number_lexer = detail::lexer<BasicJsonType, ia_type>(detail::input_adapter(number_vector), false);\n        const auto result_number = number_lexer.scan();\n        const auto number_string = number_lexer.get_token_string();\n        const auto result_remainder = number_lexer.scan();\n\n        using token_type = typename detail::lexer_base<BasicJsonType>::token_type;\n\n        if (JSON_HEDLEY_UNLIKELY(result_remainder != token_type::end_of_input))\n        {\n            return sax->parse_error(chars_read, number_string, parse_error::create(115, chars_read,\n                                    exception_message(input_format, concat(\"invalid number text: \", number_lexer.get_token_string()), \"high-precision number\"), nullptr));\n        }\n\n        switch (result_number)\n        {\n            case token_type::value_integer:\n                return sax->number_integer(number_lexer.get_number_integer());\n            case token_type::value_unsigned:\n                return sax->number_unsigned(number_lexer.get_number_unsigned());\n            case token_type::value_float:\n                return sax->number_float(number_lexer.get_number_float(), std::move(number_string));\n            case token_type::uninitialized:\n            case token_type::literal_true:\n            case token_type::literal_false:\n            case token_type::literal_null:\n            case token_type::value_string:\n            case token_type::begin_array:\n            case token_type::begin_object:\n            case token_type::end_array:\n            case token_type::end_object:\n            case token_type::name_separator:\n            case token_type::value_separator:\n            case token_type::parse_error:\n            case token_type::end_of_input:\n            case token_type::literal_or_value:\n            default:\n                return sax->parse_error(chars_read, number_string, parse_error::create(115, chars_read,\n                                        exception_message(input_format, concat(\"invalid number text: \", number_lexer.get_token_string()), \"high-precision number\"), nullptr));\n        }\n    }\n\n    ///////////////////////\n    // Utility functions //\n    ///////////////////////\n\n    /*!\n    @brief get next character from the input\n\n    This function provides the interface to the used input adapter. It does\n    not throw in case the input reached EOF, but returns a -'ve valued\n    `std::char_traits<char_type>::eof()` in that case.\n\n    @return character read from the input\n    */\n    char_int_type get()\n    {\n        ++chars_read;\n        return current = ia.get_character();\n    }\n\n    /*!\n    @return character read from the input after ignoring all 'N' entries\n    */\n    char_int_type get_ignore_noop()\n    {\n        do\n        {\n            get();\n        }\n        while (current == 'N');\n\n        return current;\n    }\n\n    /*\n    @brief read a number from the input\n\n    @tparam NumberType the type of the number\n    @param[in] format   the current format (for diagnostics)\n    @param[out] result  number of type @a NumberType\n\n    @return whether conversion completed\n\n    @note This function needs to respect the system's endianness, because\n          bytes in CBOR, MessagePack, and UBJSON are stored in network order\n          (big endian) and therefore need reordering on little endian systems.\n          On the other hand, BSON and BJData use little endian and should reorder\n          on big endian systems.\n    */\n    template<typename NumberType, bool InputIsLittleEndian = false>\n    bool get_number(const input_format_t format, NumberType& result)\n    {\n        // step 1: read input into array with system's byte order\n        std::array<std::uint8_t, sizeof(NumberType)> vec{};\n        for (std::size_t i = 0; i < sizeof(NumberType); ++i)\n        {\n            get();\n            if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(format, \"number\")))\n            {\n                return false;\n            }\n\n            // reverse byte order prior to conversion if necessary\n            if (is_little_endian != (InputIsLittleEndian || format == input_format_t::bjdata))\n            {\n                vec[sizeof(NumberType) - i - 1] = static_cast<std::uint8_t>(current);\n            }\n            else\n            {\n                vec[i] = static_cast<std::uint8_t>(current); // LCOV_EXCL_LINE\n            }\n        }\n\n        // step 2: convert array into number of type T and return\n        std::memcpy(&result, vec.data(), sizeof(NumberType));\n        return true;\n    }\n\n    /*!\n    @brief create a string by reading characters from the input\n\n    @tparam NumberType the type of the number\n    @param[in] format the current format (for diagnostics)\n    @param[in] len number of characters to read\n    @param[out] result string created by reading @a len bytes\n\n    @return whether string creation completed\n\n    @note We can not reserve @a len bytes for the result, because @a len\n          may be too large. Usually, @ref unexpect_eof() detects the end of\n          the input before we run out of string memory.\n    */\n    template<typename NumberType>\n    bool get_string(const input_format_t format,\n                    const NumberType len,\n                    string_t& result)\n    {\n        bool success = true;\n        for (NumberType i = 0; i < len; i++)\n        {\n            get();\n            if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(format, \"string\")))\n            {\n                success = false;\n                break;\n            }\n            result.push_back(static_cast<typename string_t::value_type>(current));\n        }\n        return success;\n    }\n\n    /*!\n    @brief create a byte array by reading bytes from the input\n\n    @tparam NumberType the type of the number\n    @param[in] format the current format (for diagnostics)\n    @param[in] len number of bytes to read\n    @param[out] result byte array created by reading @a len bytes\n\n    @return whether byte array creation completed\n\n    @note We can not reserve @a len bytes for the result, because @a len\n          may be too large. Usually, @ref unexpect_eof() detects the end of\n          the input before we run out of memory.\n    */\n    template<typename NumberType>\n    bool get_binary(const input_format_t format,\n                    const NumberType len,\n                    binary_t& result)\n    {\n        bool success = true;\n        for (NumberType i = 0; i < len; i++)\n        {\n            get();\n            if (JSON_HEDLEY_UNLIKELY(!unexpect_eof(format, \"binary\")))\n            {\n                success = false;\n                break;\n            }\n            result.push_back(static_cast<std::uint8_t>(current));\n        }\n        return success;\n    }\n\n    /*!\n    @param[in] format   the current format (for diagnostics)\n    @param[in] context  further context information (for diagnostics)\n    @return whether the last read character is not EOF\n    */\n    JSON_HEDLEY_NON_NULL(3)\n    bool unexpect_eof(const input_format_t format, const char* context) const\n    {\n        if (JSON_HEDLEY_UNLIKELY(current == std::char_traits<char_type>::eof()))\n        {\n            return sax->parse_error(chars_read, \"<end of file>\",\n                                    parse_error::create(110, chars_read, exception_message(format, \"unexpected end of input\", context), nullptr));\n        }\n        return true;\n    }\n\n    /*!\n    @return a string representation of the last read byte\n    */\n    std::string get_token_string() const\n    {\n        std::array<char, 3> cr{{}};\n        static_cast<void>((std::snprintf)(cr.data(), cr.size(), \"%.2hhX\", static_cast<unsigned char>(current))); // NOLINT(cppcoreguidelines-pro-type-vararg,hicpp-vararg)\n        return std::string{cr.data()};\n    }\n\n    /*!\n    @param[in] format   the current format\n    @param[in] detail   a detailed error message\n    @param[in] context  further context information\n    @return a message string to use in the parse_error exceptions\n    */\n    std::string exception_message(const input_format_t format,\n                                  const std::string& detail,\n                                  const std::string& context) const\n    {\n        std::string error_msg = \"syntax error while parsing \";\n\n        switch (format)\n        {\n            case input_format_t::cbor:\n                error_msg += \"CBOR\";\n                break;\n\n            case input_format_t::msgpack:\n                error_msg += \"MessagePack\";\n                break;\n\n            case input_format_t::ubjson:\n                error_msg += \"UBJSON\";\n                break;\n\n            case input_format_t::bson:\n                error_msg += \"BSON\";\n                break;\n\n            case input_format_t::bjdata:\n                error_msg += \"BJData\";\n                break;\n\n            case input_format_t::json: // LCOV_EXCL_LINE\n            default:            // LCOV_EXCL_LINE\n                JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE\n        }\n\n        return concat(error_msg, ' ', context, \": \", detail);\n    }\n\n  private:\n    static JSON_INLINE_VARIABLE constexpr std::size_t npos = static_cast<std::size_t>(-1);\n\n    /// input adapter\n    InputAdapterType ia;\n\n    /// the current character\n    char_int_type current = std::char_traits<char_type>::eof();\n\n    /// the number of characters read\n    std::size_t chars_read = 0;\n\n    /// whether we can assume little endianness\n    const bool is_little_endian = little_endianness();\n\n    /// input format\n    const input_format_t input_format = input_format_t::json;\n\n    /// the SAX parser\n    json_sax_t* sax = nullptr;\n\n    // excluded markers in bjdata optimized type\n#define JSON_BINARY_READER_MAKE_BJD_OPTIMIZED_TYPE_MARKERS_ \\\n    make_array<char_int_type>('F', 'H', 'N', 'S', 'T', 'Z', '[', '{')\n\n#define JSON_BINARY_READER_MAKE_BJD_TYPES_MAP_ \\\n    make_array<bjd_type>(                      \\\n    bjd_type{'C', \"char\"},                     \\\n    bjd_type{'D', \"double\"},                   \\\n    bjd_type{'I', \"int16\"},                    \\\n    bjd_type{'L', \"int64\"},                    \\\n    bjd_type{'M', \"uint64\"},                   \\\n    bjd_type{'U', \"uint8\"},                    \\\n    bjd_type{'d', \"single\"},                   \\\n    bjd_type{'i', \"int8\"},                     \\\n    bjd_type{'l', \"int32\"},                    \\\n    bjd_type{'m', \"uint32\"},                   \\\n    bjd_type{'u', \"uint16\"})\n\n  JSON_PRIVATE_UNLESS_TESTED:\n    // lookup tables\n    // NOLINTNEXTLINE(cppcoreguidelines-non-private-member-variables-in-classes)\n    const decltype(JSON_BINARY_READER_MAKE_BJD_OPTIMIZED_TYPE_MARKERS_) bjd_optimized_type_markers =\n        JSON_BINARY_READER_MAKE_BJD_OPTIMIZED_TYPE_MARKERS_;\n\n    using bjd_type = std::pair<char_int_type, string_t>;\n    // NOLINTNEXTLINE(cppcoreguidelines-non-private-member-variables-in-classes)\n    const decltype(JSON_BINARY_READER_MAKE_BJD_TYPES_MAP_) bjd_types_map =\n        JSON_BINARY_READER_MAKE_BJD_TYPES_MAP_;\n\n#undef JSON_BINARY_READER_MAKE_BJD_OPTIMIZED_TYPE_MARKERS_\n#undef JSON_BINARY_READER_MAKE_BJD_TYPES_MAP_\n};\n\n#ifndef JSON_HAS_CPP_17\n    template<typename BasicJsonType, typename InputAdapterType, typename SAX>\n    constexpr std::size_t binary_reader<BasicJsonType, InputAdapterType, SAX>::npos;\n#endif\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/input/input_adapters.hpp>\n\n// #include <nlohmann/detail/input/lexer.hpp>\n\n// #include <nlohmann/detail/input/parser.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <cmath> // isfinite\n#include <cstdint> // uint8_t\n#include <functional> // function\n#include <string> // string\n#include <utility> // move\n#include <vector> // vector\n\n// #include <nlohmann/detail/exceptions.hpp>\n\n// #include <nlohmann/detail/input/input_adapters.hpp>\n\n// #include <nlohmann/detail/input/json_sax.hpp>\n\n// #include <nlohmann/detail/input/lexer.hpp>\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/meta/is_sax.hpp>\n\n// #include <nlohmann/detail/string_concat.hpp>\n\n// #include <nlohmann/detail/value_t.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n////////////\n// parser //\n////////////\n\nenum class parse_event_t : std::uint8_t\n{\n    /// the parser read `{` and started to process a JSON object\n    object_start,\n    /// the parser read `}` and finished processing a JSON object\n    object_end,\n    /// the parser read `[` and started to process a JSON array\n    array_start,\n    /// the parser read `]` and finished processing a JSON array\n    array_end,\n    /// the parser read a key of a value in an object\n    key,\n    /// the parser finished reading a JSON value\n    value\n};\n\ntemplate<typename BasicJsonType>\nusing parser_callback_t =\n    std::function<bool(int /*depth*/, parse_event_t /*event*/, BasicJsonType& /*parsed*/)>;\n\n/*!\n@brief syntax analysis\n\nThis class implements a recursive descent parser.\n*/\ntemplate<typename BasicJsonType, typename InputAdapterType>\nclass parser\n{\n    using number_integer_t = typename BasicJsonType::number_integer_t;\n    using number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n    using string_t = typename BasicJsonType::string_t;\n    using lexer_t = lexer<BasicJsonType, InputAdapterType>;\n    using token_type = typename lexer_t::token_type;\n\n  public:\n    /// a parser reading from an input adapter\n    explicit parser(InputAdapterType&& adapter,\n                    const parser_callback_t<BasicJsonType> cb = nullptr,\n                    const bool allow_exceptions_ = true,\n                    const bool skip_comments = false)\n        : callback(cb)\n        , m_lexer(std::move(adapter), skip_comments)\n        , allow_exceptions(allow_exceptions_)\n    {\n        // read first token\n        get_token();\n    }\n\n    /*!\n    @brief public parser interface\n\n    @param[in] strict      whether to expect the last token to be EOF\n    @param[in,out] result  parsed JSON value\n\n    @throw parse_error.101 in case of an unexpected token\n    @throw parse_error.102 if to_unicode fails or surrogate error\n    @throw parse_error.103 if to_unicode fails\n    */\n    void parse(const bool strict, BasicJsonType& result)\n    {\n        if (callback)\n        {\n            json_sax_dom_callback_parser<BasicJsonType> sdp(result, callback, allow_exceptions);\n            sax_parse_internal(&sdp);\n\n            // in strict mode, input must be completely read\n            if (strict && (get_token() != token_type::end_of_input))\n            {\n                sdp.parse_error(m_lexer.get_position(),\n                                m_lexer.get_token_string(),\n                                parse_error::create(101, m_lexer.get_position(),\n                                                    exception_message(token_type::end_of_input, \"value\"), nullptr));\n            }\n\n            // in case of an error, return discarded value\n            if (sdp.is_errored())\n            {\n                result = value_t::discarded;\n                return;\n            }\n\n            // set top-level value to null if it was discarded by the callback\n            // function\n            if (result.is_discarded())\n            {\n                result = nullptr;\n            }\n        }\n        else\n        {\n            json_sax_dom_parser<BasicJsonType> sdp(result, allow_exceptions);\n            sax_parse_internal(&sdp);\n\n            // in strict mode, input must be completely read\n            if (strict && (get_token() != token_type::end_of_input))\n            {\n                sdp.parse_error(m_lexer.get_position(),\n                                m_lexer.get_token_string(),\n                                parse_error::create(101, m_lexer.get_position(), exception_message(token_type::end_of_input, \"value\"), nullptr));\n            }\n\n            // in case of an error, return discarded value\n            if (sdp.is_errored())\n            {\n                result = value_t::discarded;\n                return;\n            }\n        }\n\n        result.assert_invariant();\n    }\n\n    /*!\n    @brief public accept interface\n\n    @param[in] strict  whether to expect the last token to be EOF\n    @return whether the input is a proper JSON text\n    */\n    bool accept(const bool strict = true)\n    {\n        json_sax_acceptor<BasicJsonType> sax_acceptor;\n        return sax_parse(&sax_acceptor, strict);\n    }\n\n    template<typename SAX>\n    JSON_HEDLEY_NON_NULL(2)\n    bool sax_parse(SAX* sax, const bool strict = true)\n    {\n        (void)detail::is_sax_static_asserts<SAX, BasicJsonType> {};\n        const bool result = sax_parse_internal(sax);\n\n        // strict mode: next byte must be EOF\n        if (result && strict && (get_token() != token_type::end_of_input))\n        {\n            return sax->parse_error(m_lexer.get_position(),\n                                    m_lexer.get_token_string(),\n                                    parse_error::create(101, m_lexer.get_position(), exception_message(token_type::end_of_input, \"value\"), nullptr));\n        }\n\n        return result;\n    }\n\n  private:\n    template<typename SAX>\n    JSON_HEDLEY_NON_NULL(2)\n    bool sax_parse_internal(SAX* sax)\n    {\n        // stack to remember the hierarchy of structured values we are parsing\n        // true = array; false = object\n        std::vector<bool> states;\n        // value to avoid a goto (see comment where set to true)\n        bool skip_to_state_evaluation = false;\n\n        while (true)\n        {\n            if (!skip_to_state_evaluation)\n            {\n                // invariant: get_token() was called before each iteration\n                switch (last_token)\n                {\n                    case token_type::begin_object:\n                    {\n                        if (JSON_HEDLEY_UNLIKELY(!sax->start_object(static_cast<std::size_t>(-1))))\n                        {\n                            return false;\n                        }\n\n                        // closing } -> we are done\n                        if (get_token() == token_type::end_object)\n                        {\n                            if (JSON_HEDLEY_UNLIKELY(!sax->end_object()))\n                            {\n                                return false;\n                            }\n                            break;\n                        }\n\n                        // parse key\n                        if (JSON_HEDLEY_UNLIKELY(last_token != token_type::value_string))\n                        {\n                            return sax->parse_error(m_lexer.get_position(),\n                                                    m_lexer.get_token_string(),\n                                                    parse_error::create(101, m_lexer.get_position(), exception_message(token_type::value_string, \"object key\"), nullptr));\n                        }\n                        if (JSON_HEDLEY_UNLIKELY(!sax->key(m_lexer.get_string())))\n                        {\n                            return false;\n                        }\n\n                        // parse separator (:)\n                        if (JSON_HEDLEY_UNLIKELY(get_token() != token_type::name_separator))\n                        {\n                            return sax->parse_error(m_lexer.get_position(),\n                                                    m_lexer.get_token_string(),\n                                                    parse_error::create(101, m_lexer.get_position(), exception_message(token_type::name_separator, \"object separator\"), nullptr));\n                        }\n\n                        // remember we are now inside an object\n                        states.push_back(false);\n\n                        // parse values\n                        get_token();\n                        continue;\n                    }\n\n                    case token_type::begin_array:\n                    {\n                        if (JSON_HEDLEY_UNLIKELY(!sax->start_array(static_cast<std::size_t>(-1))))\n                        {\n                            return false;\n                        }\n\n                        // closing ] -> we are done\n                        if (get_token() == token_type::end_array)\n                        {\n                            if (JSON_HEDLEY_UNLIKELY(!sax->end_array()))\n                            {\n                                return false;\n                            }\n                            break;\n                        }\n\n                        // remember we are now inside an array\n                        states.push_back(true);\n\n                        // parse values (no need to call get_token)\n                        continue;\n                    }\n\n                    case token_type::value_float:\n                    {\n                        const auto res = m_lexer.get_number_float();\n\n                        if (JSON_HEDLEY_UNLIKELY(!std::isfinite(res)))\n                        {\n                            return sax->parse_error(m_lexer.get_position(),\n                                                    m_lexer.get_token_string(),\n                                                    out_of_range::create(406, concat(\"number overflow parsing '\", m_lexer.get_token_string(), '\\''), nullptr));\n                        }\n\n                        if (JSON_HEDLEY_UNLIKELY(!sax->number_float(res, m_lexer.get_string())))\n                        {\n                            return false;\n                        }\n\n                        break;\n                    }\n\n                    case token_type::literal_false:\n                    {\n                        if (JSON_HEDLEY_UNLIKELY(!sax->boolean(false)))\n                        {\n                            return false;\n                        }\n                        break;\n                    }\n\n                    case token_type::literal_null:\n                    {\n                        if (JSON_HEDLEY_UNLIKELY(!sax->null()))\n                        {\n                            return false;\n                        }\n                        break;\n                    }\n\n                    case token_type::literal_true:\n                    {\n                        if (JSON_HEDLEY_UNLIKELY(!sax->boolean(true)))\n                        {\n                            return false;\n                        }\n                        break;\n                    }\n\n                    case token_type::value_integer:\n                    {\n                        if (JSON_HEDLEY_UNLIKELY(!sax->number_integer(m_lexer.get_number_integer())))\n                        {\n                            return false;\n                        }\n                        break;\n                    }\n\n                    case token_type::value_string:\n                    {\n                        if (JSON_HEDLEY_UNLIKELY(!sax->string(m_lexer.get_string())))\n                        {\n                            return false;\n                        }\n                        break;\n                    }\n\n                    case token_type::value_unsigned:\n                    {\n                        if (JSON_HEDLEY_UNLIKELY(!sax->number_unsigned(m_lexer.get_number_unsigned())))\n                        {\n                            return false;\n                        }\n                        break;\n                    }\n\n                    case token_type::parse_error:\n                    {\n                        // using \"uninitialized\" to avoid \"expected\" message\n                        return sax->parse_error(m_lexer.get_position(),\n                                                m_lexer.get_token_string(),\n                                                parse_error::create(101, m_lexer.get_position(), exception_message(token_type::uninitialized, \"value\"), nullptr));\n                    }\n\n                    case token_type::uninitialized:\n                    case token_type::end_array:\n                    case token_type::end_object:\n                    case token_type::name_separator:\n                    case token_type::value_separator:\n                    case token_type::end_of_input:\n                    case token_type::literal_or_value:\n                    default: // the last token was unexpected\n                    {\n                        return sax->parse_error(m_lexer.get_position(),\n                                                m_lexer.get_token_string(),\n                                                parse_error::create(101, m_lexer.get_position(), exception_message(token_type::literal_or_value, \"value\"), nullptr));\n                    }\n                }\n            }\n            else\n            {\n                skip_to_state_evaluation = false;\n            }\n\n            // we reached this line after we successfully parsed a value\n            if (states.empty())\n            {\n                // empty stack: we reached the end of the hierarchy: done\n                return true;\n            }\n\n            if (states.back())  // array\n            {\n                // comma -> next value\n                if (get_token() == token_type::value_separator)\n                {\n                    // parse a new value\n                    get_token();\n                    continue;\n                }\n\n                // closing ]\n                if (JSON_HEDLEY_LIKELY(last_token == token_type::end_array))\n                {\n                    if (JSON_HEDLEY_UNLIKELY(!sax->end_array()))\n                    {\n                        return false;\n                    }\n\n                    // We are done with this array. Before we can parse a\n                    // new value, we need to evaluate the new state first.\n                    // By setting skip_to_state_evaluation to false, we\n                    // are effectively jumping to the beginning of this if.\n                    JSON_ASSERT(!states.empty());\n                    states.pop_back();\n                    skip_to_state_evaluation = true;\n                    continue;\n                }\n\n                return sax->parse_error(m_lexer.get_position(),\n                                        m_lexer.get_token_string(),\n                                        parse_error::create(101, m_lexer.get_position(), exception_message(token_type::end_array, \"array\"), nullptr));\n            }\n\n            // states.back() is false -> object\n\n            // comma -> next value\n            if (get_token() == token_type::value_separator)\n            {\n                // parse key\n                if (JSON_HEDLEY_UNLIKELY(get_token() != token_type::value_string))\n                {\n                    return sax->parse_error(m_lexer.get_position(),\n                                            m_lexer.get_token_string(),\n                                            parse_error::create(101, m_lexer.get_position(), exception_message(token_type::value_string, \"object key\"), nullptr));\n                }\n\n                if (JSON_HEDLEY_UNLIKELY(!sax->key(m_lexer.get_string())))\n                {\n                    return false;\n                }\n\n                // parse separator (:)\n                if (JSON_HEDLEY_UNLIKELY(get_token() != token_type::name_separator))\n                {\n                    return sax->parse_error(m_lexer.get_position(),\n                                            m_lexer.get_token_string(),\n                                            parse_error::create(101, m_lexer.get_position(), exception_message(token_type::name_separator, \"object separator\"), nullptr));\n                }\n\n                // parse values\n                get_token();\n                continue;\n            }\n\n            // closing }\n            if (JSON_HEDLEY_LIKELY(last_token == token_type::end_object))\n            {\n                if (JSON_HEDLEY_UNLIKELY(!sax->end_object()))\n                {\n                    return false;\n                }\n\n                // We are done with this object. Before we can parse a\n                // new value, we need to evaluate the new state first.\n                // By setting skip_to_state_evaluation to false, we\n                // are effectively jumping to the beginning of this if.\n                JSON_ASSERT(!states.empty());\n                states.pop_back();\n                skip_to_state_evaluation = true;\n                continue;\n            }\n\n            return sax->parse_error(m_lexer.get_position(),\n                                    m_lexer.get_token_string(),\n                                    parse_error::create(101, m_lexer.get_position(), exception_message(token_type::end_object, \"object\"), nullptr));\n        }\n    }\n\n    /// get next token from lexer\n    token_type get_token()\n    {\n        return last_token = m_lexer.scan();\n    }\n\n    std::string exception_message(const token_type expected, const std::string& context)\n    {\n        std::string error_msg = \"syntax error \";\n\n        if (!context.empty())\n        {\n            error_msg += concat(\"while parsing \", context, ' ');\n        }\n\n        error_msg += \"- \";\n\n        if (last_token == token_type::parse_error)\n        {\n            error_msg += concat(m_lexer.get_error_message(), \"; last read: '\",\n                                m_lexer.get_token_string(), '\\'');\n        }\n        else\n        {\n            error_msg += concat(\"unexpected \", lexer_t::token_type_name(last_token));\n        }\n\n        if (expected != token_type::uninitialized)\n        {\n            error_msg += concat(\"; expected \", lexer_t::token_type_name(expected));\n        }\n\n        return error_msg;\n    }\n\n  private:\n    /// callback function\n    const parser_callback_t<BasicJsonType> callback = nullptr;\n    /// the type of the last read token\n    token_type last_token = token_type::uninitialized;\n    /// the lexer\n    lexer_t m_lexer;\n    /// whether to throw exceptions in case of errors\n    const bool allow_exceptions = true;\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/iterators/internal_iterator.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n// #include <nlohmann/detail/iterators/primitive_iterator.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <cstddef> // ptrdiff_t\n#include <limits>  // numeric_limits\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n/*\n@brief an iterator for primitive JSON types\n\nThis class models an iterator for primitive JSON types (boolean, number,\nstring). It's only purpose is to allow the iterator/const_iterator classes\nto \"iterate\" over primitive values. Internally, the iterator is modeled by\na `difference_type` variable. Value begin_value (`0`) models the begin,\nend_value (`1`) models past the end.\n*/\nclass primitive_iterator_t\n{\n  private:\n    using difference_type = std::ptrdiff_t;\n    static constexpr difference_type begin_value = 0;\n    static constexpr difference_type end_value = begin_value + 1;\n\n  JSON_PRIVATE_UNLESS_TESTED:\n    /// iterator as signed integer type\n    difference_type m_it = (std::numeric_limits<std::ptrdiff_t>::min)();\n\n  public:\n    constexpr difference_type get_value() const noexcept\n    {\n        return m_it;\n    }\n\n    /// set iterator to a defined beginning\n    void set_begin() noexcept\n    {\n        m_it = begin_value;\n    }\n\n    /// set iterator to a defined past the end\n    void set_end() noexcept\n    {\n        m_it = end_value;\n    }\n\n    /// return whether the iterator can be dereferenced\n    constexpr bool is_begin() const noexcept\n    {\n        return m_it == begin_value;\n    }\n\n    /// return whether the iterator is at end\n    constexpr bool is_end() const noexcept\n    {\n        return m_it == end_value;\n    }\n\n    friend constexpr bool operator==(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept\n    {\n        return lhs.m_it == rhs.m_it;\n    }\n\n    friend constexpr bool operator<(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept\n    {\n        return lhs.m_it < rhs.m_it;\n    }\n\n    primitive_iterator_t operator+(difference_type n) noexcept\n    {\n        auto result = *this;\n        result += n;\n        return result;\n    }\n\n    friend constexpr difference_type operator-(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept\n    {\n        return lhs.m_it - rhs.m_it;\n    }\n\n    primitive_iterator_t& operator++() noexcept\n    {\n        ++m_it;\n        return *this;\n    }\n\n    primitive_iterator_t operator++(int)& noexcept // NOLINT(cert-dcl21-cpp)\n    {\n        auto result = *this;\n        ++m_it;\n        return result;\n    }\n\n    primitive_iterator_t& operator--() noexcept\n    {\n        --m_it;\n        return *this;\n    }\n\n    primitive_iterator_t operator--(int)& noexcept // NOLINT(cert-dcl21-cpp)\n    {\n        auto result = *this;\n        --m_it;\n        return result;\n    }\n\n    primitive_iterator_t& operator+=(difference_type n) noexcept\n    {\n        m_it += n;\n        return *this;\n    }\n\n    primitive_iterator_t& operator-=(difference_type n) noexcept\n    {\n        m_it -= n;\n        return *this;\n    }\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n/*!\n@brief an iterator value\n\n@note This structure could easily be a union, but MSVC currently does not allow\nunions members with complex constructors, see https://github.com/nlohmann/json/pull/105.\n*/\ntemplate<typename BasicJsonType> struct internal_iterator\n{\n    /// iterator for JSON objects\n    typename BasicJsonType::object_t::iterator object_iterator {};\n    /// iterator for JSON arrays\n    typename BasicJsonType::array_t::iterator array_iterator {};\n    /// generic iterator for all other types\n    primitive_iterator_t primitive_iterator {};\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/iterators/iter_impl.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <iterator> // iterator, random_access_iterator_tag, bidirectional_iterator_tag, advance, next\n#include <type_traits> // conditional, is_const, remove_const\n\n// #include <nlohmann/detail/exceptions.hpp>\n\n// #include <nlohmann/detail/iterators/internal_iterator.hpp>\n\n// #include <nlohmann/detail/iterators/primitive_iterator.hpp>\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/meta/cpp_future.hpp>\n\n// #include <nlohmann/detail/meta/type_traits.hpp>\n\n// #include <nlohmann/detail/value_t.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n// forward declare, to be able to friend it later on\ntemplate<typename IteratorType> class iteration_proxy;\ntemplate<typename IteratorType> class iteration_proxy_value;\n\n/*!\n@brief a template for a bidirectional iterator for the @ref basic_json class\nThis class implements a both iterators (iterator and const_iterator) for the\n@ref basic_json class.\n@note An iterator is called *initialized* when a pointer to a JSON value has\n      been set (e.g., by a constructor or a copy assignment). If the iterator is\n      default-constructed, it is *uninitialized* and most methods are undefined.\n      **The library uses assertions to detect calls on uninitialized iterators.**\n@requirement The class satisfies the following concept requirements:\n-\n[BidirectionalIterator](https://en.cppreference.com/w/cpp/named_req/BidirectionalIterator):\n  The iterator that can be moved can be moved in both directions (i.e.\n  incremented and decremented).\n@since version 1.0.0, simplified in version 2.0.9, change to bidirectional\n       iterators in version 3.0.0 (see https://github.com/nlohmann/json/issues/593)\n*/\ntemplate<typename BasicJsonType>\nclass iter_impl // NOLINT(cppcoreguidelines-special-member-functions,hicpp-special-member-functions)\n{\n    /// the iterator with BasicJsonType of different const-ness\n    using other_iter_impl = iter_impl<typename std::conditional<std::is_const<BasicJsonType>::value, typename std::remove_const<BasicJsonType>::type, const BasicJsonType>::type>;\n    /// allow basic_json to access private members\n    friend other_iter_impl;\n    friend BasicJsonType;\n    friend iteration_proxy<iter_impl>;\n    friend iteration_proxy_value<iter_impl>;\n\n    using object_t = typename BasicJsonType::object_t;\n    using array_t = typename BasicJsonType::array_t;\n    // make sure BasicJsonType is basic_json or const basic_json\n    static_assert(is_basic_json<typename std::remove_const<BasicJsonType>::type>::value,\n                  \"iter_impl only accepts (const) basic_json\");\n    // superficial check for the LegacyBidirectionalIterator named requirement\n    static_assert(std::is_base_of<std::bidirectional_iterator_tag, std::bidirectional_iterator_tag>::value\n                  &&  std::is_base_of<std::bidirectional_iterator_tag, typename std::iterator_traits<typename array_t::iterator>::iterator_category>::value,\n                  \"basic_json iterator assumes array and object type iterators satisfy the LegacyBidirectionalIterator named requirement.\");\n\n  public:\n    /// The std::iterator class template (used as a base class to provide typedefs) is deprecated in C++17.\n    /// The C++ Standard has never required user-defined iterators to derive from std::iterator.\n    /// A user-defined iterator should provide publicly accessible typedefs named\n    /// iterator_category, value_type, difference_type, pointer, and reference.\n    /// Note that value_type is required to be non-const, even for constant iterators.\n    using iterator_category = std::bidirectional_iterator_tag;\n\n    /// the type of the values when the iterator is dereferenced\n    using value_type = typename BasicJsonType::value_type;\n    /// a type to represent differences between iterators\n    using difference_type = typename BasicJsonType::difference_type;\n    /// defines a pointer to the type iterated over (value_type)\n    using pointer = typename std::conditional<std::is_const<BasicJsonType>::value,\n          typename BasicJsonType::const_pointer,\n          typename BasicJsonType::pointer>::type;\n    /// defines a reference to the type iterated over (value_type)\n    using reference =\n        typename std::conditional<std::is_const<BasicJsonType>::value,\n        typename BasicJsonType::const_reference,\n        typename BasicJsonType::reference>::type;\n\n    iter_impl() = default;\n    ~iter_impl() = default;\n    iter_impl(iter_impl&&) noexcept = default;\n    iter_impl& operator=(iter_impl&&) noexcept = default;\n\n    /*!\n    @brief constructor for a given JSON instance\n    @param[in] object  pointer to a JSON object for this iterator\n    @pre object != nullptr\n    @post The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    explicit iter_impl(pointer object) noexcept : m_object(object)\n    {\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n            {\n                m_it.object_iterator = typename object_t::iterator();\n                break;\n            }\n\n            case value_t::array:\n            {\n                m_it.array_iterator = typename array_t::iterator();\n                break;\n            }\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                m_it.primitive_iterator = primitive_iterator_t();\n                break;\n            }\n        }\n    }\n\n    /*!\n    @note The conventional copy constructor and copy assignment are implicitly\n          defined. Combined with the following converting constructor and\n          assignment, they support: (1) copy from iterator to iterator, (2)\n          copy from const iterator to const iterator, and (3) conversion from\n          iterator to const iterator. However conversion from const iterator\n          to iterator is not defined.\n    */\n\n    /*!\n    @brief const copy constructor\n    @param[in] other const iterator to copy from\n    @note This copy constructor had to be defined explicitly to circumvent a bug\n          occurring on msvc v19.0 compiler (VS 2015) debug build. For more\n          information refer to: https://github.com/nlohmann/json/issues/1608\n    */\n    iter_impl(const iter_impl<const BasicJsonType>& other) noexcept\n        : m_object(other.m_object), m_it(other.m_it)\n    {}\n\n    /*!\n    @brief converting assignment\n    @param[in] other const iterator to copy from\n    @return const/non-const iterator\n    @note It is not checked whether @a other is initialized.\n    */\n    iter_impl& operator=(const iter_impl<const BasicJsonType>& other) noexcept\n    {\n        if (&other != this)\n        {\n            m_object = other.m_object;\n            m_it = other.m_it;\n        }\n        return *this;\n    }\n\n    /*!\n    @brief converting constructor\n    @param[in] other  non-const iterator to copy from\n    @note It is not checked whether @a other is initialized.\n    */\n    iter_impl(const iter_impl<typename std::remove_const<BasicJsonType>::type>& other) noexcept\n        : m_object(other.m_object), m_it(other.m_it)\n    {}\n\n    /*!\n    @brief converting assignment\n    @param[in] other  non-const iterator to copy from\n    @return const/non-const iterator\n    @note It is not checked whether @a other is initialized.\n    */\n    iter_impl& operator=(const iter_impl<typename std::remove_const<BasicJsonType>::type>& other) noexcept // NOLINT(cert-oop54-cpp)\n    {\n        m_object = other.m_object;\n        m_it = other.m_it;\n        return *this;\n    }\n\n  JSON_PRIVATE_UNLESS_TESTED:\n    /*!\n    @brief set the iterator to the first value\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    void set_begin() noexcept\n    {\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n            {\n                m_it.object_iterator = m_object->m_value.object->begin();\n                break;\n            }\n\n            case value_t::array:\n            {\n                m_it.array_iterator = m_object->m_value.array->begin();\n                break;\n            }\n\n            case value_t::null:\n            {\n                // set to end so begin()==end() is true: null is empty\n                m_it.primitive_iterator.set_end();\n                break;\n            }\n\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                m_it.primitive_iterator.set_begin();\n                break;\n            }\n        }\n    }\n\n    /*!\n    @brief set the iterator past the last value\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    void set_end() noexcept\n    {\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n            {\n                m_it.object_iterator = m_object->m_value.object->end();\n                break;\n            }\n\n            case value_t::array:\n            {\n                m_it.array_iterator = m_object->m_value.array->end();\n                break;\n            }\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                m_it.primitive_iterator.set_end();\n                break;\n            }\n        }\n    }\n\n  public:\n    /*!\n    @brief return a reference to the value pointed to by the iterator\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    reference operator*() const\n    {\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n            {\n                JSON_ASSERT(m_it.object_iterator != m_object->m_value.object->end());\n                return m_it.object_iterator->second;\n            }\n\n            case value_t::array:\n            {\n                JSON_ASSERT(m_it.array_iterator != m_object->m_value.array->end());\n                return *m_it.array_iterator;\n            }\n\n            case value_t::null:\n                JSON_THROW(invalid_iterator::create(214, \"cannot get value\", m_object));\n\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                if (JSON_HEDLEY_LIKELY(m_it.primitive_iterator.is_begin()))\n                {\n                    return *m_object;\n                }\n\n                JSON_THROW(invalid_iterator::create(214, \"cannot get value\", m_object));\n            }\n        }\n    }\n\n    /*!\n    @brief dereference the iterator\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    pointer operator->() const\n    {\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n            {\n                JSON_ASSERT(m_it.object_iterator != m_object->m_value.object->end());\n                return &(m_it.object_iterator->second);\n            }\n\n            case value_t::array:\n            {\n                JSON_ASSERT(m_it.array_iterator != m_object->m_value.array->end());\n                return &*m_it.array_iterator;\n            }\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                if (JSON_HEDLEY_LIKELY(m_it.primitive_iterator.is_begin()))\n                {\n                    return m_object;\n                }\n\n                JSON_THROW(invalid_iterator::create(214, \"cannot get value\", m_object));\n            }\n        }\n    }\n\n    /*!\n    @brief post-increment (it++)\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    iter_impl operator++(int)& // NOLINT(cert-dcl21-cpp)\n    {\n        auto result = *this;\n        ++(*this);\n        return result;\n    }\n\n    /*!\n    @brief pre-increment (++it)\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    iter_impl& operator++()\n    {\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n            {\n                std::advance(m_it.object_iterator, 1);\n                break;\n            }\n\n            case value_t::array:\n            {\n                std::advance(m_it.array_iterator, 1);\n                break;\n            }\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                ++m_it.primitive_iterator;\n                break;\n            }\n        }\n\n        return *this;\n    }\n\n    /*!\n    @brief post-decrement (it--)\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    iter_impl operator--(int)& // NOLINT(cert-dcl21-cpp)\n    {\n        auto result = *this;\n        --(*this);\n        return result;\n    }\n\n    /*!\n    @brief pre-decrement (--it)\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    iter_impl& operator--()\n    {\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n            {\n                std::advance(m_it.object_iterator, -1);\n                break;\n            }\n\n            case value_t::array:\n            {\n                std::advance(m_it.array_iterator, -1);\n                break;\n            }\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                --m_it.primitive_iterator;\n                break;\n            }\n        }\n\n        return *this;\n    }\n\n    /*!\n    @brief comparison: equal\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    template < typename IterImpl, detail::enable_if_t < (std::is_same<IterImpl, iter_impl>::value || std::is_same<IterImpl, other_iter_impl>::value), std::nullptr_t > = nullptr >\n    bool operator==(const IterImpl& other) const\n    {\n        // if objects are not the same, the comparison is undefined\n        if (JSON_HEDLEY_UNLIKELY(m_object != other.m_object))\n        {\n            JSON_THROW(invalid_iterator::create(212, \"cannot compare iterators of different containers\", m_object));\n        }\n\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n                return (m_it.object_iterator == other.m_it.object_iterator);\n\n            case value_t::array:\n                return (m_it.array_iterator == other.m_it.array_iterator);\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n                return (m_it.primitive_iterator == other.m_it.primitive_iterator);\n        }\n    }\n\n    /*!\n    @brief comparison: not equal\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    template < typename IterImpl, detail::enable_if_t < (std::is_same<IterImpl, iter_impl>::value || std::is_same<IterImpl, other_iter_impl>::value), std::nullptr_t > = nullptr >\n    bool operator!=(const IterImpl& other) const\n    {\n        return !operator==(other);\n    }\n\n    /*!\n    @brief comparison: smaller\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    bool operator<(const iter_impl& other) const\n    {\n        // if objects are not the same, the comparison is undefined\n        if (JSON_HEDLEY_UNLIKELY(m_object != other.m_object))\n        {\n            JSON_THROW(invalid_iterator::create(212, \"cannot compare iterators of different containers\", m_object));\n        }\n\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n                JSON_THROW(invalid_iterator::create(213, \"cannot compare order of object iterators\", m_object));\n\n            case value_t::array:\n                return (m_it.array_iterator < other.m_it.array_iterator);\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n                return (m_it.primitive_iterator < other.m_it.primitive_iterator);\n        }\n    }\n\n    /*!\n    @brief comparison: less than or equal\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    bool operator<=(const iter_impl& other) const\n    {\n        return !other.operator < (*this);\n    }\n\n    /*!\n    @brief comparison: greater than\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    bool operator>(const iter_impl& other) const\n    {\n        return !operator<=(other);\n    }\n\n    /*!\n    @brief comparison: greater than or equal\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    bool operator>=(const iter_impl& other) const\n    {\n        return !operator<(other);\n    }\n\n    /*!\n    @brief add to iterator\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    iter_impl& operator+=(difference_type i)\n    {\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n                JSON_THROW(invalid_iterator::create(209, \"cannot use offsets with object iterators\", m_object));\n\n            case value_t::array:\n            {\n                std::advance(m_it.array_iterator, i);\n                break;\n            }\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                m_it.primitive_iterator += i;\n                break;\n            }\n        }\n\n        return *this;\n    }\n\n    /*!\n    @brief subtract from iterator\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    iter_impl& operator-=(difference_type i)\n    {\n        return operator+=(-i);\n    }\n\n    /*!\n    @brief add to iterator\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    iter_impl operator+(difference_type i) const\n    {\n        auto result = *this;\n        result += i;\n        return result;\n    }\n\n    /*!\n    @brief addition of distance and iterator\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    friend iter_impl operator+(difference_type i, const iter_impl& it)\n    {\n        auto result = it;\n        result += i;\n        return result;\n    }\n\n    /*!\n    @brief subtract from iterator\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    iter_impl operator-(difference_type i) const\n    {\n        auto result = *this;\n        result -= i;\n        return result;\n    }\n\n    /*!\n    @brief return difference\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    difference_type operator-(const iter_impl& other) const\n    {\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n                JSON_THROW(invalid_iterator::create(209, \"cannot use offsets with object iterators\", m_object));\n\n            case value_t::array:\n                return m_it.array_iterator - other.m_it.array_iterator;\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n                return m_it.primitive_iterator - other.m_it.primitive_iterator;\n        }\n    }\n\n    /*!\n    @brief access to successor\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    reference operator[](difference_type n) const\n    {\n        JSON_ASSERT(m_object != nullptr);\n\n        switch (m_object->m_type)\n        {\n            case value_t::object:\n                JSON_THROW(invalid_iterator::create(208, \"cannot use operator[] for object iterators\", m_object));\n\n            case value_t::array:\n                return *std::next(m_it.array_iterator, n);\n\n            case value_t::null:\n                JSON_THROW(invalid_iterator::create(214, \"cannot get value\", m_object));\n\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                if (JSON_HEDLEY_LIKELY(m_it.primitive_iterator.get_value() == -n))\n                {\n                    return *m_object;\n                }\n\n                JSON_THROW(invalid_iterator::create(214, \"cannot get value\", m_object));\n            }\n        }\n    }\n\n    /*!\n    @brief return the key of an object iterator\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    const typename object_t::key_type& key() const\n    {\n        JSON_ASSERT(m_object != nullptr);\n\n        if (JSON_HEDLEY_LIKELY(m_object->is_object()))\n        {\n            return m_it.object_iterator->first;\n        }\n\n        JSON_THROW(invalid_iterator::create(207, \"cannot use key() for non-object iterators\", m_object));\n    }\n\n    /*!\n    @brief return the value of an iterator\n    @pre The iterator is initialized; i.e. `m_object != nullptr`.\n    */\n    reference value() const\n    {\n        return operator*();\n    }\n\n  JSON_PRIVATE_UNLESS_TESTED:\n    /// associated JSON instance\n    pointer m_object = nullptr;\n    /// the actual iterator of the associated instance\n    internal_iterator<typename std::remove_const<BasicJsonType>::type> m_it {};\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/iterators/iteration_proxy.hpp>\n\n// #include <nlohmann/detail/iterators/json_reverse_iterator.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <cstddef> // ptrdiff_t\n#include <iterator> // reverse_iterator\n#include <utility> // declval\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n//////////////////////\n// reverse_iterator //\n//////////////////////\n\n/*!\n@brief a template for a reverse iterator class\n\n@tparam Base the base iterator type to reverse. Valid types are @ref\niterator (to create @ref reverse_iterator) and @ref const_iterator (to\ncreate @ref const_reverse_iterator).\n\n@requirement The class satisfies the following concept requirements:\n-\n[BidirectionalIterator](https://en.cppreference.com/w/cpp/named_req/BidirectionalIterator):\n  The iterator that can be moved can be moved in both directions (i.e.\n  incremented and decremented).\n- [OutputIterator](https://en.cppreference.com/w/cpp/named_req/OutputIterator):\n  It is possible to write to the pointed-to element (only if @a Base is\n  @ref iterator).\n\n@since version 1.0.0\n*/\ntemplate<typename Base>\nclass json_reverse_iterator : public std::reverse_iterator<Base>\n{\n  public:\n    using difference_type = std::ptrdiff_t;\n    /// shortcut to the reverse iterator adapter\n    using base_iterator = std::reverse_iterator<Base>;\n    /// the reference type for the pointed-to element\n    using reference = typename Base::reference;\n\n    /// create reverse iterator from iterator\n    explicit json_reverse_iterator(const typename base_iterator::iterator_type& it) noexcept\n        : base_iterator(it) {}\n\n    /// create reverse iterator from base class\n    explicit json_reverse_iterator(const base_iterator& it) noexcept : base_iterator(it) {}\n\n    /// post-increment (it++)\n    json_reverse_iterator operator++(int)& // NOLINT(cert-dcl21-cpp)\n    {\n        return static_cast<json_reverse_iterator>(base_iterator::operator++(1));\n    }\n\n    /// pre-increment (++it)\n    json_reverse_iterator& operator++()\n    {\n        return static_cast<json_reverse_iterator&>(base_iterator::operator++());\n    }\n\n    /// post-decrement (it--)\n    json_reverse_iterator operator--(int)& // NOLINT(cert-dcl21-cpp)\n    {\n        return static_cast<json_reverse_iterator>(base_iterator::operator--(1));\n    }\n\n    /// pre-decrement (--it)\n    json_reverse_iterator& operator--()\n    {\n        return static_cast<json_reverse_iterator&>(base_iterator::operator--());\n    }\n\n    /// add to iterator\n    json_reverse_iterator& operator+=(difference_type i)\n    {\n        return static_cast<json_reverse_iterator&>(base_iterator::operator+=(i));\n    }\n\n    /// add to iterator\n    json_reverse_iterator operator+(difference_type i) const\n    {\n        return static_cast<json_reverse_iterator>(base_iterator::operator+(i));\n    }\n\n    /// subtract from iterator\n    json_reverse_iterator operator-(difference_type i) const\n    {\n        return static_cast<json_reverse_iterator>(base_iterator::operator-(i));\n    }\n\n    /// return difference\n    difference_type operator-(const json_reverse_iterator& other) const\n    {\n        return base_iterator(*this) - base_iterator(other);\n    }\n\n    /// access to successor\n    reference operator[](difference_type n) const\n    {\n        return *(this->operator+(n));\n    }\n\n    /// return the key of an object iterator\n    auto key() const -> decltype(std::declval<Base>().key())\n    {\n        auto it = --this->base();\n        return it.key();\n    }\n\n    /// return the value of an iterator\n    reference value() const\n    {\n        auto it = --this->base();\n        return it.operator * ();\n    }\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/iterators/primitive_iterator.hpp>\n\n// #include <nlohmann/detail/json_pointer.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <algorithm> // all_of\n#include <cctype> // isdigit\n#include <cerrno> // errno, ERANGE\n#include <cstdlib> // strtoull\n#ifndef JSON_NO_IO\n    #include <iosfwd> // ostream\n#endif  // JSON_NO_IO\n#include <limits> // max\n#include <numeric> // accumulate\n#include <string> // string\n#include <utility> // move\n#include <vector> // vector\n\n// #include <nlohmann/detail/exceptions.hpp>\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/string_concat.hpp>\n\n// #include <nlohmann/detail/string_escape.hpp>\n\n// #include <nlohmann/detail/value_t.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\n\n/// @brief JSON Pointer defines a string syntax for identifying a specific value within a JSON document\n/// @sa https://json.nlohmann.me/api/json_pointer/\ntemplate<typename RefStringType>\nclass json_pointer\n{\n    // allow basic_json to access private members\n    NLOHMANN_BASIC_JSON_TPL_DECLARATION\n    friend class basic_json;\n\n    template<typename>\n    friend class json_pointer;\n\n    template<typename T>\n    struct string_t_helper\n    {\n        using type = T;\n    };\n\n    NLOHMANN_BASIC_JSON_TPL_DECLARATION\n    struct string_t_helper<NLOHMANN_BASIC_JSON_TPL>\n    {\n        using type = StringType;\n    };\n\n  public:\n    // for backwards compatibility accept BasicJsonType\n    using string_t = typename string_t_helper<RefStringType>::type;\n\n    /// @brief create JSON pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/json_pointer/\n    explicit json_pointer(const string_t& s = \"\")\n        : reference_tokens(split(s))\n    {}\n\n    /// @brief return a string representation of the JSON pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/to_string/\n    string_t to_string() const\n    {\n        return std::accumulate(reference_tokens.begin(), reference_tokens.end(),\n                               string_t{},\n                               [](const string_t& a, const string_t& b)\n        {\n            return detail::concat(a, '/', detail::escape(b));\n        });\n    }\n\n    /// @brief return a string representation of the JSON pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_string/\n    JSON_HEDLEY_DEPRECATED_FOR(3.11.0, to_string())\n    operator string_t() const\n    {\n        return to_string();\n    }\n\n#ifndef JSON_NO_IO\n    /// @brief write string representation of the JSON pointer to stream\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ltlt/\n    friend std::ostream& operator<<(std::ostream& o, const json_pointer& ptr)\n    {\n        o << ptr.to_string();\n        return o;\n    }\n#endif\n\n    /// @brief append another JSON pointer at the end of this JSON pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_slasheq/\n    json_pointer& operator/=(const json_pointer& ptr)\n    {\n        reference_tokens.insert(reference_tokens.end(),\n                                ptr.reference_tokens.begin(),\n                                ptr.reference_tokens.end());\n        return *this;\n    }\n\n    /// @brief append an unescaped reference token at the end of this JSON pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_slasheq/\n    json_pointer& operator/=(string_t token)\n    {\n        push_back(std::move(token));\n        return *this;\n    }\n\n    /// @brief append an array index at the end of this JSON pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_slasheq/\n    json_pointer& operator/=(std::size_t array_idx)\n    {\n        return *this /= std::to_string(array_idx);\n    }\n\n    /// @brief create a new JSON pointer by appending the right JSON pointer at the end of the left JSON pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_slash/\n    friend json_pointer operator/(const json_pointer& lhs,\n                                  const json_pointer& rhs)\n    {\n        return json_pointer(lhs) /= rhs;\n    }\n\n    /// @brief create a new JSON pointer by appending the unescaped token at the end of the JSON pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_slash/\n    friend json_pointer operator/(const json_pointer& lhs, string_t token) // NOLINT(performance-unnecessary-value-param)\n    {\n        return json_pointer(lhs) /= std::move(token);\n    }\n\n    /// @brief create a new JSON pointer by appending the array-index-token at the end of the JSON pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_slash/\n    friend json_pointer operator/(const json_pointer& lhs, std::size_t array_idx)\n    {\n        return json_pointer(lhs) /= array_idx;\n    }\n\n    /// @brief returns the parent of this JSON pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/parent_pointer/\n    json_pointer parent_pointer() const\n    {\n        if (empty())\n        {\n            return *this;\n        }\n\n        json_pointer res = *this;\n        res.pop_back();\n        return res;\n    }\n\n    /// @brief remove last reference token\n    /// @sa https://json.nlohmann.me/api/json_pointer/pop_back/\n    void pop_back()\n    {\n        if (JSON_HEDLEY_UNLIKELY(empty()))\n        {\n            JSON_THROW(detail::out_of_range::create(405, \"JSON pointer has no parent\", nullptr));\n        }\n\n        reference_tokens.pop_back();\n    }\n\n    /// @brief return last reference token\n    /// @sa https://json.nlohmann.me/api/json_pointer/back/\n    const string_t& back() const\n    {\n        if (JSON_HEDLEY_UNLIKELY(empty()))\n        {\n            JSON_THROW(detail::out_of_range::create(405, \"JSON pointer has no parent\", nullptr));\n        }\n\n        return reference_tokens.back();\n    }\n\n    /// @brief append an unescaped token at the end of the reference pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/push_back/\n    void push_back(const string_t& token)\n    {\n        reference_tokens.push_back(token);\n    }\n\n    /// @brief append an unescaped token at the end of the reference pointer\n    /// @sa https://json.nlohmann.me/api/json_pointer/push_back/\n    void push_back(string_t&& token)\n    {\n        reference_tokens.push_back(std::move(token));\n    }\n\n    /// @brief return whether pointer points to the root document\n    /// @sa https://json.nlohmann.me/api/json_pointer/empty/\n    bool empty() const noexcept\n    {\n        return reference_tokens.empty();\n    }\n\n  private:\n    /*!\n    @param[in] s  reference token to be converted into an array index\n\n    @return integer representation of @a s\n\n    @throw parse_error.106  if an array index begins with '0'\n    @throw parse_error.109  if an array index begins not with a digit\n    @throw out_of_range.404 if string @a s could not be converted to an integer\n    @throw out_of_range.410 if an array index exceeds size_type\n    */\n    template<typename BasicJsonType>\n    static typename BasicJsonType::size_type array_index(const string_t& s)\n    {\n        using size_type = typename BasicJsonType::size_type;\n\n        // error condition (cf. RFC 6901, Sect. 4)\n        if (JSON_HEDLEY_UNLIKELY(s.size() > 1 && s[0] == '0'))\n        {\n            JSON_THROW(detail::parse_error::create(106, 0, detail::concat(\"array index '\", s, \"' must not begin with '0'\"), nullptr));\n        }\n\n        // error condition (cf. RFC 6901, Sect. 4)\n        if (JSON_HEDLEY_UNLIKELY(s.size() > 1 && !(s[0] >= '1' && s[0] <= '9')))\n        {\n            JSON_THROW(detail::parse_error::create(109, 0, detail::concat(\"array index '\", s, \"' is not a number\"), nullptr));\n        }\n\n        const char* p = s.c_str();\n        char* p_end = nullptr;\n        errno = 0; // strtoull doesn't reset errno\n        unsigned long long res = std::strtoull(p, &p_end, 10); // NOLINT(runtime/int)\n        if (p == p_end // invalid input or empty string\n                || errno == ERANGE // out of range\n                || JSON_HEDLEY_UNLIKELY(static_cast<std::size_t>(p_end - p) != s.size())) // incomplete read\n        {\n            JSON_THROW(detail::out_of_range::create(404, detail::concat(\"unresolved reference token '\", s, \"'\"), nullptr));\n        }\n\n        // only triggered on special platforms (like 32bit), see also\n        // https://github.com/nlohmann/json/pull/2203\n        if (res >= static_cast<unsigned long long>((std::numeric_limits<size_type>::max)()))  // NOLINT(runtime/int)\n        {\n            JSON_THROW(detail::out_of_range::create(410, detail::concat(\"array index \", s, \" exceeds size_type\"), nullptr));   // LCOV_EXCL_LINE\n        }\n\n        return static_cast<size_type>(res);\n    }\n\n  JSON_PRIVATE_UNLESS_TESTED:\n    json_pointer top() const\n    {\n        if (JSON_HEDLEY_UNLIKELY(empty()))\n        {\n            JSON_THROW(detail::out_of_range::create(405, \"JSON pointer has no parent\", nullptr));\n        }\n\n        json_pointer result = *this;\n        result.reference_tokens = {reference_tokens[0]};\n        return result;\n    }\n\n  private:\n    /*!\n    @brief create and return a reference to the pointed to value\n\n    @complexity Linear in the number of reference tokens.\n\n    @throw parse_error.109 if array index is not a number\n    @throw type_error.313 if value cannot be unflattened\n    */\n    template<typename BasicJsonType>\n    BasicJsonType& get_and_create(BasicJsonType& j) const\n    {\n        auto* result = &j;\n\n        // in case no reference tokens exist, return a reference to the JSON value\n        // j which will be overwritten by a primitive value\n        for (const auto& reference_token : reference_tokens)\n        {\n            switch (result->type())\n            {\n                case detail::value_t::null:\n                {\n                    if (reference_token == \"0\")\n                    {\n                        // start a new array if reference token is 0\n                        result = &result->operator[](0);\n                    }\n                    else\n                    {\n                        // start a new object otherwise\n                        result = &result->operator[](reference_token);\n                    }\n                    break;\n                }\n\n                case detail::value_t::object:\n                {\n                    // create an entry in the object\n                    result = &result->operator[](reference_token);\n                    break;\n                }\n\n                case detail::value_t::array:\n                {\n                    // create an entry in the array\n                    result = &result->operator[](array_index<BasicJsonType>(reference_token));\n                    break;\n                }\n\n                /*\n                The following code is only reached if there exists a reference\n                token _and_ the current value is primitive. In this case, we have\n                an error situation, because primitive values may only occur as\n                single value; that is, with an empty list of reference tokens.\n                */\n                case detail::value_t::string:\n                case detail::value_t::boolean:\n                case detail::value_t::number_integer:\n                case detail::value_t::number_unsigned:\n                case detail::value_t::number_float:\n                case detail::value_t::binary:\n                case detail::value_t::discarded:\n                default:\n                    JSON_THROW(detail::type_error::create(313, \"invalid value to unflatten\", &j));\n            }\n        }\n\n        return *result;\n    }\n\n    /*!\n    @brief return a reference to the pointed to value\n\n    @note This version does not throw if a value is not present, but tries to\n          create nested values instead. For instance, calling this function\n          with pointer `\"/this/that\"` on a null value is equivalent to calling\n          `operator[](\"this\").operator[](\"that\")` on that value, effectively\n          changing the null value to an object.\n\n    @param[in] ptr  a JSON value\n\n    @return reference to the JSON value pointed to by the JSON pointer\n\n    @complexity Linear in the length of the JSON pointer.\n\n    @throw parse_error.106   if an array index begins with '0'\n    @throw parse_error.109   if an array index was not a number\n    @throw out_of_range.404  if the JSON pointer can not be resolved\n    */\n    template<typename BasicJsonType>\n    BasicJsonType& get_unchecked(BasicJsonType* ptr) const\n    {\n        for (const auto& reference_token : reference_tokens)\n        {\n            // convert null values to arrays or objects before continuing\n            if (ptr->is_null())\n            {\n                // check if reference token is a number\n                const bool nums =\n                    std::all_of(reference_token.begin(), reference_token.end(),\n                                [](const unsigned char x)\n                {\n                    return std::isdigit(x);\n                });\n\n                // change value to array for numbers or \"-\" or to object otherwise\n                *ptr = (nums || reference_token == \"-\")\n                       ? detail::value_t::array\n                       : detail::value_t::object;\n            }\n\n            switch (ptr->type())\n            {\n                case detail::value_t::object:\n                {\n                    // use unchecked object access\n                    ptr = &ptr->operator[](reference_token);\n                    break;\n                }\n\n                case detail::value_t::array:\n                {\n                    if (reference_token == \"-\")\n                    {\n                        // explicitly treat \"-\" as index beyond the end\n                        ptr = &ptr->operator[](ptr->m_value.array->size());\n                    }\n                    else\n                    {\n                        // convert array index to number; unchecked access\n                        ptr = &ptr->operator[](array_index<BasicJsonType>(reference_token));\n                    }\n                    break;\n                }\n\n                case detail::value_t::null:\n                case detail::value_t::string:\n                case detail::value_t::boolean:\n                case detail::value_t::number_integer:\n                case detail::value_t::number_unsigned:\n                case detail::value_t::number_float:\n                case detail::value_t::binary:\n                case detail::value_t::discarded:\n                default:\n                    JSON_THROW(detail::out_of_range::create(404, detail::concat(\"unresolved reference token '\", reference_token, \"'\"), ptr));\n            }\n        }\n\n        return *ptr;\n    }\n\n    /*!\n    @throw parse_error.106   if an array index begins with '0'\n    @throw parse_error.109   if an array index was not a number\n    @throw out_of_range.402  if the array index '-' is used\n    @throw out_of_range.404  if the JSON pointer can not be resolved\n    */\n    template<typename BasicJsonType>\n    BasicJsonType& get_checked(BasicJsonType* ptr) const\n    {\n        for (const auto& reference_token : reference_tokens)\n        {\n            switch (ptr->type())\n            {\n                case detail::value_t::object:\n                {\n                    // note: at performs range check\n                    ptr = &ptr->at(reference_token);\n                    break;\n                }\n\n                case detail::value_t::array:\n                {\n                    if (JSON_HEDLEY_UNLIKELY(reference_token == \"-\"))\n                    {\n                        // \"-\" always fails the range check\n                        JSON_THROW(detail::out_of_range::create(402, detail::concat(\n                                \"array index '-' (\", std::to_string(ptr->m_value.array->size()),\n                                \") is out of range\"), ptr));\n                    }\n\n                    // note: at performs range check\n                    ptr = &ptr->at(array_index<BasicJsonType>(reference_token));\n                    break;\n                }\n\n                case detail::value_t::null:\n                case detail::value_t::string:\n                case detail::value_t::boolean:\n                case detail::value_t::number_integer:\n                case detail::value_t::number_unsigned:\n                case detail::value_t::number_float:\n                case detail::value_t::binary:\n                case detail::value_t::discarded:\n                default:\n                    JSON_THROW(detail::out_of_range::create(404, detail::concat(\"unresolved reference token '\", reference_token, \"'\"), ptr));\n            }\n        }\n\n        return *ptr;\n    }\n\n    /*!\n    @brief return a const reference to the pointed to value\n\n    @param[in] ptr  a JSON value\n\n    @return const reference to the JSON value pointed to by the JSON\n    pointer\n\n    @throw parse_error.106   if an array index begins with '0'\n    @throw parse_error.109   if an array index was not a number\n    @throw out_of_range.402  if the array index '-' is used\n    @throw out_of_range.404  if the JSON pointer can not be resolved\n    */\n    template<typename BasicJsonType>\n    const BasicJsonType& get_unchecked(const BasicJsonType* ptr) const\n    {\n        for (const auto& reference_token : reference_tokens)\n        {\n            switch (ptr->type())\n            {\n                case detail::value_t::object:\n                {\n                    // use unchecked object access\n                    ptr = &ptr->operator[](reference_token);\n                    break;\n                }\n\n                case detail::value_t::array:\n                {\n                    if (JSON_HEDLEY_UNLIKELY(reference_token == \"-\"))\n                    {\n                        // \"-\" cannot be used for const access\n                        JSON_THROW(detail::out_of_range::create(402, detail::concat(\"array index '-' (\", std::to_string(ptr->m_value.array->size()), \") is out of range\"), ptr));\n                    }\n\n                    // use unchecked array access\n                    ptr = &ptr->operator[](array_index<BasicJsonType>(reference_token));\n                    break;\n                }\n\n                case detail::value_t::null:\n                case detail::value_t::string:\n                case detail::value_t::boolean:\n                case detail::value_t::number_integer:\n                case detail::value_t::number_unsigned:\n                case detail::value_t::number_float:\n                case detail::value_t::binary:\n                case detail::value_t::discarded:\n                default:\n                    JSON_THROW(detail::out_of_range::create(404, detail::concat(\"unresolved reference token '\", reference_token, \"'\"), ptr));\n            }\n        }\n\n        return *ptr;\n    }\n\n    /*!\n    @throw parse_error.106   if an array index begins with '0'\n    @throw parse_error.109   if an array index was not a number\n    @throw out_of_range.402  if the array index '-' is used\n    @throw out_of_range.404  if the JSON pointer can not be resolved\n    */\n    template<typename BasicJsonType>\n    const BasicJsonType& get_checked(const BasicJsonType* ptr) const\n    {\n        for (const auto& reference_token : reference_tokens)\n        {\n            switch (ptr->type())\n            {\n                case detail::value_t::object:\n                {\n                    // note: at performs range check\n                    ptr = &ptr->at(reference_token);\n                    break;\n                }\n\n                case detail::value_t::array:\n                {\n                    if (JSON_HEDLEY_UNLIKELY(reference_token == \"-\"))\n                    {\n                        // \"-\" always fails the range check\n                        JSON_THROW(detail::out_of_range::create(402, detail::concat(\n                                \"array index '-' (\", std::to_string(ptr->m_value.array->size()),\n                                \") is out of range\"), ptr));\n                    }\n\n                    // note: at performs range check\n                    ptr = &ptr->at(array_index<BasicJsonType>(reference_token));\n                    break;\n                }\n\n                case detail::value_t::null:\n                case detail::value_t::string:\n                case detail::value_t::boolean:\n                case detail::value_t::number_integer:\n                case detail::value_t::number_unsigned:\n                case detail::value_t::number_float:\n                case detail::value_t::binary:\n                case detail::value_t::discarded:\n                default:\n                    JSON_THROW(detail::out_of_range::create(404, detail::concat(\"unresolved reference token '\", reference_token, \"'\"), ptr));\n            }\n        }\n\n        return *ptr;\n    }\n\n    /*!\n    @throw parse_error.106   if an array index begins with '0'\n    @throw parse_error.109   if an array index was not a number\n    */\n    template<typename BasicJsonType>\n    bool contains(const BasicJsonType* ptr) const\n    {\n        for (const auto& reference_token : reference_tokens)\n        {\n            switch (ptr->type())\n            {\n                case detail::value_t::object:\n                {\n                    if (!ptr->contains(reference_token))\n                    {\n                        // we did not find the key in the object\n                        return false;\n                    }\n\n                    ptr = &ptr->operator[](reference_token);\n                    break;\n                }\n\n                case detail::value_t::array:\n                {\n                    if (JSON_HEDLEY_UNLIKELY(reference_token == \"-\"))\n                    {\n                        // \"-\" always fails the range check\n                        return false;\n                    }\n                    if (JSON_HEDLEY_UNLIKELY(reference_token.size() == 1 && !(\"0\" <= reference_token && reference_token <= \"9\")))\n                    {\n                        // invalid char\n                        return false;\n                    }\n                    if (JSON_HEDLEY_UNLIKELY(reference_token.size() > 1))\n                    {\n                        if (JSON_HEDLEY_UNLIKELY(!('1' <= reference_token[0] && reference_token[0] <= '9')))\n                        {\n                            // first char should be between '1' and '9'\n                            return false;\n                        }\n                        for (std::size_t i = 1; i < reference_token.size(); i++)\n                        {\n                            if (JSON_HEDLEY_UNLIKELY(!('0' <= reference_token[i] && reference_token[i] <= '9')))\n                            {\n                                // other char should be between '0' and '9'\n                                return false;\n                            }\n                        }\n                    }\n\n                    const auto idx = array_index<BasicJsonType>(reference_token);\n                    if (idx >= ptr->size())\n                    {\n                        // index out of range\n                        return false;\n                    }\n\n                    ptr = &ptr->operator[](idx);\n                    break;\n                }\n\n                case detail::value_t::null:\n                case detail::value_t::string:\n                case detail::value_t::boolean:\n                case detail::value_t::number_integer:\n                case detail::value_t::number_unsigned:\n                case detail::value_t::number_float:\n                case detail::value_t::binary:\n                case detail::value_t::discarded:\n                default:\n                {\n                    // we do not expect primitive values if there is still a\n                    // reference token to process\n                    return false;\n                }\n            }\n        }\n\n        // no reference token left means we found a primitive value\n        return true;\n    }\n\n    /*!\n    @brief split the string input to reference tokens\n\n    @note This function is only called by the json_pointer constructor.\n          All exceptions below are documented there.\n\n    @throw parse_error.107  if the pointer is not empty or begins with '/'\n    @throw parse_error.108  if character '~' is not followed by '0' or '1'\n    */\n    static std::vector<string_t> split(const string_t& reference_string)\n    {\n        std::vector<string_t> result;\n\n        // special case: empty reference string -> no reference tokens\n        if (reference_string.empty())\n        {\n            return result;\n        }\n\n        // check if nonempty reference string begins with slash\n        if (JSON_HEDLEY_UNLIKELY(reference_string[0] != '/'))\n        {\n            JSON_THROW(detail::parse_error::create(107, 1, detail::concat(\"JSON pointer must be empty or begin with '/' - was: '\", reference_string, \"'\"), nullptr));\n        }\n\n        // extract the reference tokens:\n        // - slash: position of the last read slash (or end of string)\n        // - start: position after the previous slash\n        for (\n            // search for the first slash after the first character\n            std::size_t slash = reference_string.find_first_of('/', 1),\n            // set the beginning of the first reference token\n            start = 1;\n            // we can stop if start == 0 (if slash == string_t::npos)\n            start != 0;\n            // set the beginning of the next reference token\n            // (will eventually be 0 if slash == string_t::npos)\n            start = (slash == string_t::npos) ? 0 : slash + 1,\n            // find next slash\n            slash = reference_string.find_first_of('/', start))\n        {\n            // use the text between the beginning of the reference token\n            // (start) and the last slash (slash).\n            auto reference_token = reference_string.substr(start, slash - start);\n\n            // check reference tokens are properly escaped\n            for (std::size_t pos = reference_token.find_first_of('~');\n                    pos != string_t::npos;\n                    pos = reference_token.find_first_of('~', pos + 1))\n            {\n                JSON_ASSERT(reference_token[pos] == '~');\n\n                // ~ must be followed by 0 or 1\n                if (JSON_HEDLEY_UNLIKELY(pos == reference_token.size() - 1 ||\n                                         (reference_token[pos + 1] != '0' &&\n                                          reference_token[pos + 1] != '1')))\n                {\n                    JSON_THROW(detail::parse_error::create(108, 0, \"escape character '~' must be followed with '0' or '1'\", nullptr));\n                }\n            }\n\n            // finally, store the reference token\n            detail::unescape(reference_token);\n            result.push_back(reference_token);\n        }\n\n        return result;\n    }\n\n  private:\n    /*!\n    @param[in] reference_string  the reference string to the current value\n    @param[in] value             the value to consider\n    @param[in,out] result        the result object to insert values to\n\n    @note Empty objects or arrays are flattened to `null`.\n    */\n    template<typename BasicJsonType>\n    static void flatten(const string_t& reference_string,\n                        const BasicJsonType& value,\n                        BasicJsonType& result)\n    {\n        switch (value.type())\n        {\n            case detail::value_t::array:\n            {\n                if (value.m_value.array->empty())\n                {\n                    // flatten empty array as null\n                    result[reference_string] = nullptr;\n                }\n                else\n                {\n                    // iterate array and use index as reference string\n                    for (std::size_t i = 0; i < value.m_value.array->size(); ++i)\n                    {\n                        flatten(detail::concat(reference_string, '/', std::to_string(i)),\n                                value.m_value.array->operator[](i), result);\n                    }\n                }\n                break;\n            }\n\n            case detail::value_t::object:\n            {\n                if (value.m_value.object->empty())\n                {\n                    // flatten empty object as null\n                    result[reference_string] = nullptr;\n                }\n                else\n                {\n                    // iterate object and use keys as reference string\n                    for (const auto& element : *value.m_value.object)\n                    {\n                        flatten(detail::concat(reference_string, '/', detail::escape(element.first)), element.second, result);\n                    }\n                }\n                break;\n            }\n\n            case detail::value_t::null:\n            case detail::value_t::string:\n            case detail::value_t::boolean:\n            case detail::value_t::number_integer:\n            case detail::value_t::number_unsigned:\n            case detail::value_t::number_float:\n            case detail::value_t::binary:\n            case detail::value_t::discarded:\n            default:\n            {\n                // add primitive value with its reference string\n                result[reference_string] = value;\n                break;\n            }\n        }\n    }\n\n    /*!\n    @param[in] value  flattened JSON\n\n    @return unflattened JSON\n\n    @throw parse_error.109 if array index is not a number\n    @throw type_error.314  if value is not an object\n    @throw type_error.315  if object values are not primitive\n    @throw type_error.313  if value cannot be unflattened\n    */\n    template<typename BasicJsonType>\n    static BasicJsonType\n    unflatten(const BasicJsonType& value)\n    {\n        if (JSON_HEDLEY_UNLIKELY(!value.is_object()))\n        {\n            JSON_THROW(detail::type_error::create(314, \"only objects can be unflattened\", &value));\n        }\n\n        BasicJsonType result;\n\n        // iterate the JSON object values\n        for (const auto& element : *value.m_value.object)\n        {\n            if (JSON_HEDLEY_UNLIKELY(!element.second.is_primitive()))\n            {\n                JSON_THROW(detail::type_error::create(315, \"values in object must be primitive\", &element.second));\n            }\n\n            // assign value to reference pointed to by JSON pointer; Note that if\n            // the JSON pointer is \"\" (i.e., points to the whole value), function\n            // get_and_create returns a reference to result itself. An assignment\n            // will then create a primitive value.\n            json_pointer(element.first).get_and_create(result) = element.second;\n        }\n\n        return result;\n    }\n\n    // can't use conversion operator because of ambiguity\n    json_pointer<string_t> convert() const&\n    {\n        json_pointer<string_t> result;\n        result.reference_tokens = reference_tokens;\n        return result;\n    }\n\n    json_pointer<string_t> convert()&&\n    {\n        json_pointer<string_t> result;\n        result.reference_tokens = std::move(reference_tokens);\n        return result;\n    }\n\n  public:\n#if JSON_HAS_THREE_WAY_COMPARISON\n    /// @brief compares two JSON pointers for equality\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_eq/\n    template<typename RefStringTypeRhs>\n    bool operator==(const json_pointer<RefStringTypeRhs>& rhs) const noexcept\n    {\n        return reference_tokens == rhs.reference_tokens;\n    }\n\n    /// @brief compares JSON pointer and string for equality\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_eq/\n    JSON_HEDLEY_DEPRECATED_FOR(3.11.2, operator==(json_pointer))\n    bool operator==(const string_t& rhs) const\n    {\n        return *this == json_pointer(rhs);\n    }\n\n    /// @brief 3-way compares two JSON pointers\n    template<typename RefStringTypeRhs>\n    std::strong_ordering operator<=>(const json_pointer<RefStringTypeRhs>& rhs) const noexcept // *NOPAD*\n    {\n        return  reference_tokens <=> rhs.reference_tokens; // *NOPAD*\n    }\n#else\n    /// @brief compares two JSON pointers for equality\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_eq/\n    template<typename RefStringTypeLhs, typename RefStringTypeRhs>\n    // NOLINTNEXTLINE(readability-redundant-declaration)\n    friend bool operator==(const json_pointer<RefStringTypeLhs>& lhs,\n                           const json_pointer<RefStringTypeRhs>& rhs) noexcept;\n\n    /// @brief compares JSON pointer and string for equality\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_eq/\n    template<typename RefStringTypeLhs, typename StringType>\n    // NOLINTNEXTLINE(readability-redundant-declaration)\n    friend bool operator==(const json_pointer<RefStringTypeLhs>& lhs,\n                           const StringType& rhs);\n\n    /// @brief compares string and JSON pointer for equality\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_eq/\n    template<typename RefStringTypeRhs, typename StringType>\n    // NOLINTNEXTLINE(readability-redundant-declaration)\n    friend bool operator==(const StringType& lhs,\n                           const json_pointer<RefStringTypeRhs>& rhs);\n\n    /// @brief compares two JSON pointers for inequality\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_ne/\n    template<typename RefStringTypeLhs, typename RefStringTypeRhs>\n    // NOLINTNEXTLINE(readability-redundant-declaration)\n    friend bool operator!=(const json_pointer<RefStringTypeLhs>& lhs,\n                           const json_pointer<RefStringTypeRhs>& rhs) noexcept;\n\n    /// @brief compares JSON pointer and string for inequality\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_ne/\n    template<typename RefStringTypeLhs, typename StringType>\n    // NOLINTNEXTLINE(readability-redundant-declaration)\n    friend bool operator!=(const json_pointer<RefStringTypeLhs>& lhs,\n                           const StringType& rhs);\n\n    /// @brief compares string and JSON pointer for inequality\n    /// @sa https://json.nlohmann.me/api/json_pointer/operator_ne/\n    template<typename RefStringTypeRhs, typename StringType>\n    // NOLINTNEXTLINE(readability-redundant-declaration)\n    friend bool operator!=(const StringType& lhs,\n                           const json_pointer<RefStringTypeRhs>& rhs);\n\n    /// @brief compares two JSON pointer for less-than\n    template<typename RefStringTypeLhs, typename RefStringTypeRhs>\n    // NOLINTNEXTLINE(readability-redundant-declaration)\n    friend bool operator<(const json_pointer<RefStringTypeLhs>& lhs,\n                          const json_pointer<RefStringTypeRhs>& rhs) noexcept;\n#endif\n\n  private:\n    /// the reference tokens\n    std::vector<string_t> reference_tokens;\n};\n\n#if !JSON_HAS_THREE_WAY_COMPARISON\n// functions cannot be defined inside class due to ODR violations\ntemplate<typename RefStringTypeLhs, typename RefStringTypeRhs>\ninline bool operator==(const json_pointer<RefStringTypeLhs>& lhs,\n                       const json_pointer<RefStringTypeRhs>& rhs) noexcept\n{\n    return lhs.reference_tokens == rhs.reference_tokens;\n}\n\ntemplate<typename RefStringTypeLhs,\n         typename StringType = typename json_pointer<RefStringTypeLhs>::string_t>\nJSON_HEDLEY_DEPRECATED_FOR(3.11.2, operator==(json_pointer, json_pointer))\ninline bool operator==(const json_pointer<RefStringTypeLhs>& lhs,\n                       const StringType& rhs)\n{\n    return lhs == json_pointer<RefStringTypeLhs>(rhs);\n}\n\ntemplate<typename RefStringTypeRhs,\n         typename StringType = typename json_pointer<RefStringTypeRhs>::string_t>\nJSON_HEDLEY_DEPRECATED_FOR(3.11.2, operator==(json_pointer, json_pointer))\ninline bool operator==(const StringType& lhs,\n                       const json_pointer<RefStringTypeRhs>& rhs)\n{\n    return json_pointer<RefStringTypeRhs>(lhs) == rhs;\n}\n\ntemplate<typename RefStringTypeLhs, typename RefStringTypeRhs>\ninline bool operator!=(const json_pointer<RefStringTypeLhs>& lhs,\n                       const json_pointer<RefStringTypeRhs>& rhs) noexcept\n{\n    return !(lhs == rhs);\n}\n\ntemplate<typename RefStringTypeLhs,\n         typename StringType = typename json_pointer<RefStringTypeLhs>::string_t>\nJSON_HEDLEY_DEPRECATED_FOR(3.11.2, operator!=(json_pointer, json_pointer))\ninline bool operator!=(const json_pointer<RefStringTypeLhs>& lhs,\n                       const StringType& rhs)\n{\n    return !(lhs == rhs);\n}\n\ntemplate<typename RefStringTypeRhs,\n         typename StringType = typename json_pointer<RefStringTypeRhs>::string_t>\nJSON_HEDLEY_DEPRECATED_FOR(3.11.2, operator!=(json_pointer, json_pointer))\ninline bool operator!=(const StringType& lhs,\n                       const json_pointer<RefStringTypeRhs>& rhs)\n{\n    return !(lhs == rhs);\n}\n\ntemplate<typename RefStringTypeLhs, typename RefStringTypeRhs>\ninline bool operator<(const json_pointer<RefStringTypeLhs>& lhs,\n                      const json_pointer<RefStringTypeRhs>& rhs) noexcept\n{\n    return lhs.reference_tokens < rhs.reference_tokens;\n}\n#endif\n\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/json_ref.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <initializer_list>\n#include <utility>\n\n// #include <nlohmann/detail/abi_macros.hpp>\n\n// #include <nlohmann/detail/meta/type_traits.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\ntemplate<typename BasicJsonType>\nclass json_ref\n{\n  public:\n    using value_type = BasicJsonType;\n\n    json_ref(value_type&& value)\n        : owned_value(std::move(value))\n    {}\n\n    json_ref(const value_type& value)\n        : value_ref(&value)\n    {}\n\n    json_ref(std::initializer_list<json_ref> init)\n        : owned_value(init)\n    {}\n\n    template <\n        class... Args,\n        enable_if_t<std::is_constructible<value_type, Args...>::value, int> = 0 >\n    json_ref(Args && ... args)\n        : owned_value(std::forward<Args>(args)...)\n    {}\n\n    // class should be movable only\n    json_ref(json_ref&&) noexcept = default;\n    json_ref(const json_ref&) = delete;\n    json_ref& operator=(const json_ref&) = delete;\n    json_ref& operator=(json_ref&&) = delete;\n    ~json_ref() = default;\n\n    value_type moved_or_copied() const\n    {\n        if (value_ref == nullptr)\n        {\n            return std::move(owned_value);\n        }\n        return *value_ref;\n    }\n\n    value_type const& operator*() const\n    {\n        return value_ref ? *value_ref : owned_value;\n    }\n\n    value_type const* operator->() const\n    {\n        return &** this;\n    }\n\n  private:\n    mutable value_type owned_value = nullptr;\n    value_type const* value_ref = nullptr;\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/string_concat.hpp>\n\n// #include <nlohmann/detail/string_escape.hpp>\n\n// #include <nlohmann/detail/meta/cpp_future.hpp>\n\n// #include <nlohmann/detail/meta/type_traits.hpp>\n\n// #include <nlohmann/detail/output/binary_writer.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <algorithm> // reverse\n#include <array> // array\n#include <map> // map\n#include <cmath> // isnan, isinf\n#include <cstdint> // uint8_t, uint16_t, uint32_t, uint64_t\n#include <cstring> // memcpy\n#include <limits> // numeric_limits\n#include <string> // string\n#include <utility> // move\n#include <vector> // vector\n\n// #include <nlohmann/detail/input/binary_reader.hpp>\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/output/output_adapters.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <algorithm> // copy\n#include <cstddef> // size_t\n#include <iterator> // back_inserter\n#include <memory> // shared_ptr, make_shared\n#include <string> // basic_string\n#include <vector> // vector\n\n#ifndef JSON_NO_IO\n    #include <ios>      // streamsize\n    #include <ostream>  // basic_ostream\n#endif  // JSON_NO_IO\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n/// abstract output adapter interface\ntemplate<typename CharType> struct output_adapter_protocol\n{\n    virtual void write_character(CharType c) = 0;\n    virtual void write_characters(const CharType* s, std::size_t length) = 0;\n    virtual ~output_adapter_protocol() = default;\n\n    output_adapter_protocol() = default;\n    output_adapter_protocol(const output_adapter_protocol&) = default;\n    output_adapter_protocol(output_adapter_protocol&&) noexcept = default;\n    output_adapter_protocol& operator=(const output_adapter_protocol&) = default;\n    output_adapter_protocol& operator=(output_adapter_protocol&&) noexcept = default;\n};\n\n/// a type to simplify interfaces\ntemplate<typename CharType>\nusing output_adapter_t = std::shared_ptr<output_adapter_protocol<CharType>>;\n\n/// output adapter for byte vectors\ntemplate<typename CharType, typename AllocatorType = std::allocator<CharType>>\nclass output_vector_adapter : public output_adapter_protocol<CharType>\n{\n  public:\n    explicit output_vector_adapter(std::vector<CharType, AllocatorType>& vec) noexcept\n        : v(vec)\n    {}\n\n    void write_character(CharType c) override\n    {\n        v.push_back(c);\n    }\n\n    JSON_HEDLEY_NON_NULL(2)\n    void write_characters(const CharType* s, std::size_t length) override\n    {\n        v.insert(v.end(), s, s + length);\n    }\n\n  private:\n    std::vector<CharType, AllocatorType>& v;\n};\n\n#ifndef JSON_NO_IO\n/// output adapter for output streams\ntemplate<typename CharType>\nclass output_stream_adapter : public output_adapter_protocol<CharType>\n{\n  public:\n    explicit output_stream_adapter(std::basic_ostream<CharType>& s) noexcept\n        : stream(s)\n    {}\n\n    void write_character(CharType c) override\n    {\n        stream.put(c);\n    }\n\n    JSON_HEDLEY_NON_NULL(2)\n    void write_characters(const CharType* s, std::size_t length) override\n    {\n        stream.write(s, static_cast<std::streamsize>(length));\n    }\n\n  private:\n    std::basic_ostream<CharType>& stream;\n};\n#endif  // JSON_NO_IO\n\n/// output adapter for basic_string\ntemplate<typename CharType, typename StringType = std::basic_string<CharType>>\nclass output_string_adapter : public output_adapter_protocol<CharType>\n{\n  public:\n    explicit output_string_adapter(StringType& s) noexcept\n        : str(s)\n    {}\n\n    void write_character(CharType c) override\n    {\n        str.push_back(c);\n    }\n\n    JSON_HEDLEY_NON_NULL(2)\n    void write_characters(const CharType* s, std::size_t length) override\n    {\n        str.append(s, length);\n    }\n\n  private:\n    StringType& str;\n};\n\ntemplate<typename CharType, typename StringType = std::basic_string<CharType>>\nclass output_adapter\n{\n  public:\n    template<typename AllocatorType = std::allocator<CharType>>\n    output_adapter(std::vector<CharType, AllocatorType>& vec)\n        : oa(std::make_shared<output_vector_adapter<CharType, AllocatorType>>(vec)) {}\n\n#ifndef JSON_NO_IO\n    output_adapter(std::basic_ostream<CharType>& s)\n        : oa(std::make_shared<output_stream_adapter<CharType>>(s)) {}\n#endif  // JSON_NO_IO\n\n    output_adapter(StringType& s)\n        : oa(std::make_shared<output_string_adapter<CharType, StringType>>(s)) {}\n\n    operator output_adapter_t<CharType>()\n    {\n        return oa;\n    }\n\n  private:\n    output_adapter_t<CharType> oa = nullptr;\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/string_concat.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n///////////////////\n// binary writer //\n///////////////////\n\n/*!\n@brief serialization to CBOR and MessagePack values\n*/\ntemplate<typename BasicJsonType, typename CharType>\nclass binary_writer\n{\n    using string_t = typename BasicJsonType::string_t;\n    using binary_t = typename BasicJsonType::binary_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n\n  public:\n    /*!\n    @brief create a binary writer\n\n    @param[in] adapter  output adapter to write to\n    */\n    explicit binary_writer(output_adapter_t<CharType> adapter) : oa(std::move(adapter))\n    {\n        JSON_ASSERT(oa);\n    }\n\n    /*!\n    @param[in] j  JSON value to serialize\n    @pre       j.type() == value_t::object\n    */\n    void write_bson(const BasicJsonType& j)\n    {\n        switch (j.type())\n        {\n            case value_t::object:\n            {\n                write_bson_object(*j.m_value.object);\n                break;\n            }\n\n            case value_t::null:\n            case value_t::array:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                JSON_THROW(type_error::create(317, concat(\"to serialize to BSON, top-level type must be object, but is \", j.type_name()), &j));\n            }\n        }\n    }\n\n    /*!\n    @param[in] j  JSON value to serialize\n    */\n    void write_cbor(const BasicJsonType& j)\n    {\n        switch (j.type())\n        {\n            case value_t::null:\n            {\n                oa->write_character(to_char_type(0xF6));\n                break;\n            }\n\n            case value_t::boolean:\n            {\n                oa->write_character(j.m_value.boolean\n                                    ? to_char_type(0xF5)\n                                    : to_char_type(0xF4));\n                break;\n            }\n\n            case value_t::number_integer:\n            {\n                if (j.m_value.number_integer >= 0)\n                {\n                    // CBOR does not differentiate between positive signed\n                    // integers and unsigned integers. Therefore, we used the\n                    // code from the value_t::number_unsigned case here.\n                    if (j.m_value.number_integer <= 0x17)\n                    {\n                        write_number(static_cast<std::uint8_t>(j.m_value.number_integer));\n                    }\n                    else if (j.m_value.number_integer <= (std::numeric_limits<std::uint8_t>::max)())\n                    {\n                        oa->write_character(to_char_type(0x18));\n                        write_number(static_cast<std::uint8_t>(j.m_value.number_integer));\n                    }\n                    else if (j.m_value.number_integer <= (std::numeric_limits<std::uint16_t>::max)())\n                    {\n                        oa->write_character(to_char_type(0x19));\n                        write_number(static_cast<std::uint16_t>(j.m_value.number_integer));\n                    }\n                    else if (j.m_value.number_integer <= (std::numeric_limits<std::uint32_t>::max)())\n                    {\n                        oa->write_character(to_char_type(0x1A));\n                        write_number(static_cast<std::uint32_t>(j.m_value.number_integer));\n                    }\n                    else\n                    {\n                        oa->write_character(to_char_type(0x1B));\n                        write_number(static_cast<std::uint64_t>(j.m_value.number_integer));\n                    }\n                }\n                else\n                {\n                    // The conversions below encode the sign in the first\n                    // byte, and the value is converted to a positive number.\n                    const auto positive_number = -1 - j.m_value.number_integer;\n                    if (j.m_value.number_integer >= -24)\n                    {\n                        write_number(static_cast<std::uint8_t>(0x20 + positive_number));\n                    }\n                    else if (positive_number <= (std::numeric_limits<std::uint8_t>::max)())\n                    {\n                        oa->write_character(to_char_type(0x38));\n                        write_number(static_cast<std::uint8_t>(positive_number));\n                    }\n                    else if (positive_number <= (std::numeric_limits<std::uint16_t>::max)())\n                    {\n                        oa->write_character(to_char_type(0x39));\n                        write_number(static_cast<std::uint16_t>(positive_number));\n                    }\n                    else if (positive_number <= (std::numeric_limits<std::uint32_t>::max)())\n                    {\n                        oa->write_character(to_char_type(0x3A));\n                        write_number(static_cast<std::uint32_t>(positive_number));\n                    }\n                    else\n                    {\n                        oa->write_character(to_char_type(0x3B));\n                        write_number(static_cast<std::uint64_t>(positive_number));\n                    }\n                }\n                break;\n            }\n\n            case value_t::number_unsigned:\n            {\n                if (j.m_value.number_unsigned <= 0x17)\n                {\n                    write_number(static_cast<std::uint8_t>(j.m_value.number_unsigned));\n                }\n                else if (j.m_value.number_unsigned <= (std::numeric_limits<std::uint8_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x18));\n                    write_number(static_cast<std::uint8_t>(j.m_value.number_unsigned));\n                }\n                else if (j.m_value.number_unsigned <= (std::numeric_limits<std::uint16_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x19));\n                    write_number(static_cast<std::uint16_t>(j.m_value.number_unsigned));\n                }\n                else if (j.m_value.number_unsigned <= (std::numeric_limits<std::uint32_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x1A));\n                    write_number(static_cast<std::uint32_t>(j.m_value.number_unsigned));\n                }\n                else\n                {\n                    oa->write_character(to_char_type(0x1B));\n                    write_number(static_cast<std::uint64_t>(j.m_value.number_unsigned));\n                }\n                break;\n            }\n\n            case value_t::number_float:\n            {\n                if (std::isnan(j.m_value.number_float))\n                {\n                    // NaN is 0xf97e00 in CBOR\n                    oa->write_character(to_char_type(0xF9));\n                    oa->write_character(to_char_type(0x7E));\n                    oa->write_character(to_char_type(0x00));\n                }\n                else if (std::isinf(j.m_value.number_float))\n                {\n                    // Infinity is 0xf97c00, -Infinity is 0xf9fc00\n                    oa->write_character(to_char_type(0xf9));\n                    oa->write_character(j.m_value.number_float > 0 ? to_char_type(0x7C) : to_char_type(0xFC));\n                    oa->write_character(to_char_type(0x00));\n                }\n                else\n                {\n                    write_compact_float(j.m_value.number_float, detail::input_format_t::cbor);\n                }\n                break;\n            }\n\n            case value_t::string:\n            {\n                // step 1: write control byte and the string length\n                const auto N = j.m_value.string->size();\n                if (N <= 0x17)\n                {\n                    write_number(static_cast<std::uint8_t>(0x60 + N));\n                }\n                else if (N <= (std::numeric_limits<std::uint8_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x78));\n                    write_number(static_cast<std::uint8_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint16_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x79));\n                    write_number(static_cast<std::uint16_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint32_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x7A));\n                    write_number(static_cast<std::uint32_t>(N));\n                }\n                // LCOV_EXCL_START\n                else if (N <= (std::numeric_limits<std::uint64_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x7B));\n                    write_number(static_cast<std::uint64_t>(N));\n                }\n                // LCOV_EXCL_STOP\n\n                // step 2: write the string\n                oa->write_characters(\n                    reinterpret_cast<const CharType*>(j.m_value.string->c_str()),\n                    j.m_value.string->size());\n                break;\n            }\n\n            case value_t::array:\n            {\n                // step 1: write control byte and the array size\n                const auto N = j.m_value.array->size();\n                if (N <= 0x17)\n                {\n                    write_number(static_cast<std::uint8_t>(0x80 + N));\n                }\n                else if (N <= (std::numeric_limits<std::uint8_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x98));\n                    write_number(static_cast<std::uint8_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint16_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x99));\n                    write_number(static_cast<std::uint16_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint32_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x9A));\n                    write_number(static_cast<std::uint32_t>(N));\n                }\n                // LCOV_EXCL_START\n                else if (N <= (std::numeric_limits<std::uint64_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x9B));\n                    write_number(static_cast<std::uint64_t>(N));\n                }\n                // LCOV_EXCL_STOP\n\n                // step 2: write each element\n                for (const auto& el : *j.m_value.array)\n                {\n                    write_cbor(el);\n                }\n                break;\n            }\n\n            case value_t::binary:\n            {\n                if (j.m_value.binary->has_subtype())\n                {\n                    if (j.m_value.binary->subtype() <= (std::numeric_limits<std::uint8_t>::max)())\n                    {\n                        write_number(static_cast<std::uint8_t>(0xd8));\n                        write_number(static_cast<std::uint8_t>(j.m_value.binary->subtype()));\n                    }\n                    else if (j.m_value.binary->subtype() <= (std::numeric_limits<std::uint16_t>::max)())\n                    {\n                        write_number(static_cast<std::uint8_t>(0xd9));\n                        write_number(static_cast<std::uint16_t>(j.m_value.binary->subtype()));\n                    }\n                    else if (j.m_value.binary->subtype() <= (std::numeric_limits<std::uint32_t>::max)())\n                    {\n                        write_number(static_cast<std::uint8_t>(0xda));\n                        write_number(static_cast<std::uint32_t>(j.m_value.binary->subtype()));\n                    }\n                    else if (j.m_value.binary->subtype() <= (std::numeric_limits<std::uint64_t>::max)())\n                    {\n                        write_number(static_cast<std::uint8_t>(0xdb));\n                        write_number(static_cast<std::uint64_t>(j.m_value.binary->subtype()));\n                    }\n                }\n\n                // step 1: write control byte and the binary array size\n                const auto N = j.m_value.binary->size();\n                if (N <= 0x17)\n                {\n                    write_number(static_cast<std::uint8_t>(0x40 + N));\n                }\n                else if (N <= (std::numeric_limits<std::uint8_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x58));\n                    write_number(static_cast<std::uint8_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint16_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x59));\n                    write_number(static_cast<std::uint16_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint32_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x5A));\n                    write_number(static_cast<std::uint32_t>(N));\n                }\n                // LCOV_EXCL_START\n                else if (N <= (std::numeric_limits<std::uint64_t>::max)())\n                {\n                    oa->write_character(to_char_type(0x5B));\n                    write_number(static_cast<std::uint64_t>(N));\n                }\n                // LCOV_EXCL_STOP\n\n                // step 2: write each element\n                oa->write_characters(\n                    reinterpret_cast<const CharType*>(j.m_value.binary->data()),\n                    N);\n\n                break;\n            }\n\n            case value_t::object:\n            {\n                // step 1: write control byte and the object size\n                const auto N = j.m_value.object->size();\n                if (N <= 0x17)\n                {\n                    write_number(static_cast<std::uint8_t>(0xA0 + N));\n                }\n                else if (N <= (std::numeric_limits<std::uint8_t>::max)())\n                {\n                    oa->write_character(to_char_type(0xB8));\n                    write_number(static_cast<std::uint8_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint16_t>::max)())\n                {\n                    oa->write_character(to_char_type(0xB9));\n                    write_number(static_cast<std::uint16_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint32_t>::max)())\n                {\n                    oa->write_character(to_char_type(0xBA));\n                    write_number(static_cast<std::uint32_t>(N));\n                }\n                // LCOV_EXCL_START\n                else if (N <= (std::numeric_limits<std::uint64_t>::max)())\n                {\n                    oa->write_character(to_char_type(0xBB));\n                    write_number(static_cast<std::uint64_t>(N));\n                }\n                // LCOV_EXCL_STOP\n\n                // step 2: write each element\n                for (const auto& el : *j.m_value.object)\n                {\n                    write_cbor(el.first);\n                    write_cbor(el.second);\n                }\n                break;\n            }\n\n            case value_t::discarded:\n            default:\n                break;\n        }\n    }\n\n    /*!\n    @param[in] j  JSON value to serialize\n    */\n    void write_msgpack(const BasicJsonType& j)\n    {\n        switch (j.type())\n        {\n            case value_t::null: // nil\n            {\n                oa->write_character(to_char_type(0xC0));\n                break;\n            }\n\n            case value_t::boolean: // true and false\n            {\n                oa->write_character(j.m_value.boolean\n                                    ? to_char_type(0xC3)\n                                    : to_char_type(0xC2));\n                break;\n            }\n\n            case value_t::number_integer:\n            {\n                if (j.m_value.number_integer >= 0)\n                {\n                    // MessagePack does not differentiate between positive\n                    // signed integers and unsigned integers. Therefore, we used\n                    // the code from the value_t::number_unsigned case here.\n                    if (j.m_value.number_unsigned < 128)\n                    {\n                        // positive fixnum\n                        write_number(static_cast<std::uint8_t>(j.m_value.number_integer));\n                    }\n                    else if (j.m_value.number_unsigned <= (std::numeric_limits<std::uint8_t>::max)())\n                    {\n                        // uint 8\n                        oa->write_character(to_char_type(0xCC));\n                        write_number(static_cast<std::uint8_t>(j.m_value.number_integer));\n                    }\n                    else if (j.m_value.number_unsigned <= (std::numeric_limits<std::uint16_t>::max)())\n                    {\n                        // uint 16\n                        oa->write_character(to_char_type(0xCD));\n                        write_number(static_cast<std::uint16_t>(j.m_value.number_integer));\n                    }\n                    else if (j.m_value.number_unsigned <= (std::numeric_limits<std::uint32_t>::max)())\n                    {\n                        // uint 32\n                        oa->write_character(to_char_type(0xCE));\n                        write_number(static_cast<std::uint32_t>(j.m_value.number_integer));\n                    }\n                    else if (j.m_value.number_unsigned <= (std::numeric_limits<std::uint64_t>::max)())\n                    {\n                        // uint 64\n                        oa->write_character(to_char_type(0xCF));\n                        write_number(static_cast<std::uint64_t>(j.m_value.number_integer));\n                    }\n                }\n                else\n                {\n                    if (j.m_value.number_integer >= -32)\n                    {\n                        // negative fixnum\n                        write_number(static_cast<std::int8_t>(j.m_value.number_integer));\n                    }\n                    else if (j.m_value.number_integer >= (std::numeric_limits<std::int8_t>::min)() &&\n                             j.m_value.number_integer <= (std::numeric_limits<std::int8_t>::max)())\n                    {\n                        // int 8\n                        oa->write_character(to_char_type(0xD0));\n                        write_number(static_cast<std::int8_t>(j.m_value.number_integer));\n                    }\n                    else if (j.m_value.number_integer >= (std::numeric_limits<std::int16_t>::min)() &&\n                             j.m_value.number_integer <= (std::numeric_limits<std::int16_t>::max)())\n                    {\n                        // int 16\n                        oa->write_character(to_char_type(0xD1));\n                        write_number(static_cast<std::int16_t>(j.m_value.number_integer));\n                    }\n                    else if (j.m_value.number_integer >= (std::numeric_limits<std::int32_t>::min)() &&\n                             j.m_value.number_integer <= (std::numeric_limits<std::int32_t>::max)())\n                    {\n                        // int 32\n                        oa->write_character(to_char_type(0xD2));\n                        write_number(static_cast<std::int32_t>(j.m_value.number_integer));\n                    }\n                    else if (j.m_value.number_integer >= (std::numeric_limits<std::int64_t>::min)() &&\n                             j.m_value.number_integer <= (std::numeric_limits<std::int64_t>::max)())\n                    {\n                        // int 64\n                        oa->write_character(to_char_type(0xD3));\n                        write_number(static_cast<std::int64_t>(j.m_value.number_integer));\n                    }\n                }\n                break;\n            }\n\n            case value_t::number_unsigned:\n            {\n                if (j.m_value.number_unsigned < 128)\n                {\n                    // positive fixnum\n                    write_number(static_cast<std::uint8_t>(j.m_value.number_integer));\n                }\n                else if (j.m_value.number_unsigned <= (std::numeric_limits<std::uint8_t>::max)())\n                {\n                    // uint 8\n                    oa->write_character(to_char_type(0xCC));\n                    write_number(static_cast<std::uint8_t>(j.m_value.number_integer));\n                }\n                else if (j.m_value.number_unsigned <= (std::numeric_limits<std::uint16_t>::max)())\n                {\n                    // uint 16\n                    oa->write_character(to_char_type(0xCD));\n                    write_number(static_cast<std::uint16_t>(j.m_value.number_integer));\n                }\n                else if (j.m_value.number_unsigned <= (std::numeric_limits<std::uint32_t>::max)())\n                {\n                    // uint 32\n                    oa->write_character(to_char_type(0xCE));\n                    write_number(static_cast<std::uint32_t>(j.m_value.number_integer));\n                }\n                else if (j.m_value.number_unsigned <= (std::numeric_limits<std::uint64_t>::max)())\n                {\n                    // uint 64\n                    oa->write_character(to_char_type(0xCF));\n                    write_number(static_cast<std::uint64_t>(j.m_value.number_integer));\n                }\n                break;\n            }\n\n            case value_t::number_float:\n            {\n                write_compact_float(j.m_value.number_float, detail::input_format_t::msgpack);\n                break;\n            }\n\n            case value_t::string:\n            {\n                // step 1: write control byte and the string length\n                const auto N = j.m_value.string->size();\n                if (N <= 31)\n                {\n                    // fixstr\n                    write_number(static_cast<std::uint8_t>(0xA0 | N));\n                }\n                else if (N <= (std::numeric_limits<std::uint8_t>::max)())\n                {\n                    // str 8\n                    oa->write_character(to_char_type(0xD9));\n                    write_number(static_cast<std::uint8_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint16_t>::max)())\n                {\n                    // str 16\n                    oa->write_character(to_char_type(0xDA));\n                    write_number(static_cast<std::uint16_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint32_t>::max)())\n                {\n                    // str 32\n                    oa->write_character(to_char_type(0xDB));\n                    write_number(static_cast<std::uint32_t>(N));\n                }\n\n                // step 2: write the string\n                oa->write_characters(\n                    reinterpret_cast<const CharType*>(j.m_value.string->c_str()),\n                    j.m_value.string->size());\n                break;\n            }\n\n            case value_t::array:\n            {\n                // step 1: write control byte and the array size\n                const auto N = j.m_value.array->size();\n                if (N <= 15)\n                {\n                    // fixarray\n                    write_number(static_cast<std::uint8_t>(0x90 | N));\n                }\n                else if (N <= (std::numeric_limits<std::uint16_t>::max)())\n                {\n                    // array 16\n                    oa->write_character(to_char_type(0xDC));\n                    write_number(static_cast<std::uint16_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint32_t>::max)())\n                {\n                    // array 32\n                    oa->write_character(to_char_type(0xDD));\n                    write_number(static_cast<std::uint32_t>(N));\n                }\n\n                // step 2: write each element\n                for (const auto& el : *j.m_value.array)\n                {\n                    write_msgpack(el);\n                }\n                break;\n            }\n\n            case value_t::binary:\n            {\n                // step 0: determine if the binary type has a set subtype to\n                // determine whether or not to use the ext or fixext types\n                const bool use_ext = j.m_value.binary->has_subtype();\n\n                // step 1: write control byte and the byte string length\n                const auto N = j.m_value.binary->size();\n                if (N <= (std::numeric_limits<std::uint8_t>::max)())\n                {\n                    std::uint8_t output_type{};\n                    bool fixed = true;\n                    if (use_ext)\n                    {\n                        switch (N)\n                        {\n                            case 1:\n                                output_type = 0xD4; // fixext 1\n                                break;\n                            case 2:\n                                output_type = 0xD5; // fixext 2\n                                break;\n                            case 4:\n                                output_type = 0xD6; // fixext 4\n                                break;\n                            case 8:\n                                output_type = 0xD7; // fixext 8\n                                break;\n                            case 16:\n                                output_type = 0xD8; // fixext 16\n                                break;\n                            default:\n                                output_type = 0xC7; // ext 8\n                                fixed = false;\n                                break;\n                        }\n\n                    }\n                    else\n                    {\n                        output_type = 0xC4; // bin 8\n                        fixed = false;\n                    }\n\n                    oa->write_character(to_char_type(output_type));\n                    if (!fixed)\n                    {\n                        write_number(static_cast<std::uint8_t>(N));\n                    }\n                }\n                else if (N <= (std::numeric_limits<std::uint16_t>::max)())\n                {\n                    std::uint8_t output_type = use_ext\n                                               ? 0xC8 // ext 16\n                                               : 0xC5; // bin 16\n\n                    oa->write_character(to_char_type(output_type));\n                    write_number(static_cast<std::uint16_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint32_t>::max)())\n                {\n                    std::uint8_t output_type = use_ext\n                                               ? 0xC9 // ext 32\n                                               : 0xC6; // bin 32\n\n                    oa->write_character(to_char_type(output_type));\n                    write_number(static_cast<std::uint32_t>(N));\n                }\n\n                // step 1.5: if this is an ext type, write the subtype\n                if (use_ext)\n                {\n                    write_number(static_cast<std::int8_t>(j.m_value.binary->subtype()));\n                }\n\n                // step 2: write the byte string\n                oa->write_characters(\n                    reinterpret_cast<const CharType*>(j.m_value.binary->data()),\n                    N);\n\n                break;\n            }\n\n            case value_t::object:\n            {\n                // step 1: write control byte and the object size\n                const auto N = j.m_value.object->size();\n                if (N <= 15)\n                {\n                    // fixmap\n                    write_number(static_cast<std::uint8_t>(0x80 | (N & 0xF)));\n                }\n                else if (N <= (std::numeric_limits<std::uint16_t>::max)())\n                {\n                    // map 16\n                    oa->write_character(to_char_type(0xDE));\n                    write_number(static_cast<std::uint16_t>(N));\n                }\n                else if (N <= (std::numeric_limits<std::uint32_t>::max)())\n                {\n                    // map 32\n                    oa->write_character(to_char_type(0xDF));\n                    write_number(static_cast<std::uint32_t>(N));\n                }\n\n                // step 2: write each element\n                for (const auto& el : *j.m_value.object)\n                {\n                    write_msgpack(el.first);\n                    write_msgpack(el.second);\n                }\n                break;\n            }\n\n            case value_t::discarded:\n            default:\n                break;\n        }\n    }\n\n    /*!\n    @param[in] j  JSON value to serialize\n    @param[in] use_count   whether to use '#' prefixes (optimized format)\n    @param[in] use_type    whether to use '$' prefixes (optimized format)\n    @param[in] add_prefix  whether prefixes need to be used for this value\n    @param[in] use_bjdata  whether write in BJData format, default is false\n    */\n    void write_ubjson(const BasicJsonType& j, const bool use_count,\n                      const bool use_type, const bool add_prefix = true,\n                      const bool use_bjdata = false)\n    {\n        switch (j.type())\n        {\n            case value_t::null:\n            {\n                if (add_prefix)\n                {\n                    oa->write_character(to_char_type('Z'));\n                }\n                break;\n            }\n\n            case value_t::boolean:\n            {\n                if (add_prefix)\n                {\n                    oa->write_character(j.m_value.boolean\n                                        ? to_char_type('T')\n                                        : to_char_type('F'));\n                }\n                break;\n            }\n\n            case value_t::number_integer:\n            {\n                write_number_with_ubjson_prefix(j.m_value.number_integer, add_prefix, use_bjdata);\n                break;\n            }\n\n            case value_t::number_unsigned:\n            {\n                write_number_with_ubjson_prefix(j.m_value.number_unsigned, add_prefix, use_bjdata);\n                break;\n            }\n\n            case value_t::number_float:\n            {\n                write_number_with_ubjson_prefix(j.m_value.number_float, add_prefix, use_bjdata);\n                break;\n            }\n\n            case value_t::string:\n            {\n                if (add_prefix)\n                {\n                    oa->write_character(to_char_type('S'));\n                }\n                write_number_with_ubjson_prefix(j.m_value.string->size(), true, use_bjdata);\n                oa->write_characters(\n                    reinterpret_cast<const CharType*>(j.m_value.string->c_str()),\n                    j.m_value.string->size());\n                break;\n            }\n\n            case value_t::array:\n            {\n                if (add_prefix)\n                {\n                    oa->write_character(to_char_type('['));\n                }\n\n                bool prefix_required = true;\n                if (use_type && !j.m_value.array->empty())\n                {\n                    JSON_ASSERT(use_count);\n                    const CharType first_prefix = ubjson_prefix(j.front(), use_bjdata);\n                    const bool same_prefix = std::all_of(j.begin() + 1, j.end(),\n                                                         [this, first_prefix, use_bjdata](const BasicJsonType & v)\n                    {\n                        return ubjson_prefix(v, use_bjdata) == first_prefix;\n                    });\n\n                    std::vector<CharType> bjdx = {'[', '{', 'S', 'H', 'T', 'F', 'N', 'Z'}; // excluded markers in bjdata optimized type\n\n                    if (same_prefix && !(use_bjdata && std::find(bjdx.begin(), bjdx.end(), first_prefix) != bjdx.end()))\n                    {\n                        prefix_required = false;\n                        oa->write_character(to_char_type('$'));\n                        oa->write_character(first_prefix);\n                    }\n                }\n\n                if (use_count)\n                {\n                    oa->write_character(to_char_type('#'));\n                    write_number_with_ubjson_prefix(j.m_value.array->size(), true, use_bjdata);\n                }\n\n                for (const auto& el : *j.m_value.array)\n                {\n                    write_ubjson(el, use_count, use_type, prefix_required, use_bjdata);\n                }\n\n                if (!use_count)\n                {\n                    oa->write_character(to_char_type(']'));\n                }\n\n                break;\n            }\n\n            case value_t::binary:\n            {\n                if (add_prefix)\n                {\n                    oa->write_character(to_char_type('['));\n                }\n\n                if (use_type && !j.m_value.binary->empty())\n                {\n                    JSON_ASSERT(use_count);\n                    oa->write_character(to_char_type('$'));\n                    oa->write_character('U');\n                }\n\n                if (use_count)\n                {\n                    oa->write_character(to_char_type('#'));\n                    write_number_with_ubjson_prefix(j.m_value.binary->size(), true, use_bjdata);\n                }\n\n                if (use_type)\n                {\n                    oa->write_characters(\n                        reinterpret_cast<const CharType*>(j.m_value.binary->data()),\n                        j.m_value.binary->size());\n                }\n                else\n                {\n                    for (size_t i = 0; i < j.m_value.binary->size(); ++i)\n                    {\n                        oa->write_character(to_char_type('U'));\n                        oa->write_character(j.m_value.binary->data()[i]);\n                    }\n                }\n\n                if (!use_count)\n                {\n                    oa->write_character(to_char_type(']'));\n                }\n\n                break;\n            }\n\n            case value_t::object:\n            {\n                if (use_bjdata && j.m_value.object->size() == 3 && j.m_value.object->find(\"_ArrayType_\") != j.m_value.object->end() && j.m_value.object->find(\"_ArraySize_\") != j.m_value.object->end() && j.m_value.object->find(\"_ArrayData_\") != j.m_value.object->end())\n                {\n                    if (!write_bjdata_ndarray(*j.m_value.object, use_count, use_type))  // decode bjdata ndarray in the JData format (https://github.com/NeuroJSON/jdata)\n                    {\n                        break;\n                    }\n                }\n\n                if (add_prefix)\n                {\n                    oa->write_character(to_char_type('{'));\n                }\n\n                bool prefix_required = true;\n                if (use_type && !j.m_value.object->empty())\n                {\n                    JSON_ASSERT(use_count);\n                    const CharType first_prefix = ubjson_prefix(j.front(), use_bjdata);\n                    const bool same_prefix = std::all_of(j.begin(), j.end(),\n                                                         [this, first_prefix, use_bjdata](const BasicJsonType & v)\n                    {\n                        return ubjson_prefix(v, use_bjdata) == first_prefix;\n                    });\n\n                    std::vector<CharType> bjdx = {'[', '{', 'S', 'H', 'T', 'F', 'N', 'Z'}; // excluded markers in bjdata optimized type\n\n                    if (same_prefix && !(use_bjdata && std::find(bjdx.begin(), bjdx.end(), first_prefix) != bjdx.end()))\n                    {\n                        prefix_required = false;\n                        oa->write_character(to_char_type('$'));\n                        oa->write_character(first_prefix);\n                    }\n                }\n\n                if (use_count)\n                {\n                    oa->write_character(to_char_type('#'));\n                    write_number_with_ubjson_prefix(j.m_value.object->size(), true, use_bjdata);\n                }\n\n                for (const auto& el : *j.m_value.object)\n                {\n                    write_number_with_ubjson_prefix(el.first.size(), true, use_bjdata);\n                    oa->write_characters(\n                        reinterpret_cast<const CharType*>(el.first.c_str()),\n                        el.first.size());\n                    write_ubjson(el.second, use_count, use_type, prefix_required, use_bjdata);\n                }\n\n                if (!use_count)\n                {\n                    oa->write_character(to_char_type('}'));\n                }\n\n                break;\n            }\n\n            case value_t::discarded:\n            default:\n                break;\n        }\n    }\n\n  private:\n    //////////\n    // BSON //\n    //////////\n\n    /*!\n    @return The size of a BSON document entry header, including the id marker\n            and the entry name size (and its null-terminator).\n    */\n    static std::size_t calc_bson_entry_header_size(const string_t& name, const BasicJsonType& j)\n    {\n        const auto it = name.find(static_cast<typename string_t::value_type>(0));\n        if (JSON_HEDLEY_UNLIKELY(it != BasicJsonType::string_t::npos))\n        {\n            JSON_THROW(out_of_range::create(409, concat(\"BSON key cannot contain code point U+0000 (at byte \", std::to_string(it), \")\"), &j));\n            static_cast<void>(j);\n        }\n\n        return /*id*/ 1ul + name.size() + /*zero-terminator*/1u;\n    }\n\n    /*!\n    @brief Writes the given @a element_type and @a name to the output adapter\n    */\n    void write_bson_entry_header(const string_t& name,\n                                 const std::uint8_t element_type)\n    {\n        oa->write_character(to_char_type(element_type)); // boolean\n        oa->write_characters(\n            reinterpret_cast<const CharType*>(name.c_str()),\n            name.size() + 1u);\n    }\n\n    /*!\n    @brief Writes a BSON element with key @a name and boolean value @a value\n    */\n    void write_bson_boolean(const string_t& name,\n                            const bool value)\n    {\n        write_bson_entry_header(name, 0x08);\n        oa->write_character(value ? to_char_type(0x01) : to_char_type(0x00));\n    }\n\n    /*!\n    @brief Writes a BSON element with key @a name and double value @a value\n    */\n    void write_bson_double(const string_t& name,\n                           const double value)\n    {\n        write_bson_entry_header(name, 0x01);\n        write_number<double>(value, true);\n    }\n\n    /*!\n    @return The size of the BSON-encoded string in @a value\n    */\n    static std::size_t calc_bson_string_size(const string_t& value)\n    {\n        return sizeof(std::int32_t) + value.size() + 1ul;\n    }\n\n    /*!\n    @brief Writes a BSON element with key @a name and string value @a value\n    */\n    void write_bson_string(const string_t& name,\n                           const string_t& value)\n    {\n        write_bson_entry_header(name, 0x02);\n\n        write_number<std::int32_t>(static_cast<std::int32_t>(value.size() + 1ul), true);\n        oa->write_characters(\n            reinterpret_cast<const CharType*>(value.c_str()),\n            value.size() + 1);\n    }\n\n    /*!\n    @brief Writes a BSON element with key @a name and null value\n    */\n    void write_bson_null(const string_t& name)\n    {\n        write_bson_entry_header(name, 0x0A);\n    }\n\n    /*!\n    @return The size of the BSON-encoded integer @a value\n    */\n    static std::size_t calc_bson_integer_size(const std::int64_t value)\n    {\n        return (std::numeric_limits<std::int32_t>::min)() <= value && value <= (std::numeric_limits<std::int32_t>::max)()\n               ? sizeof(std::int32_t)\n               : sizeof(std::int64_t);\n    }\n\n    /*!\n    @brief Writes a BSON element with key @a name and integer @a value\n    */\n    void write_bson_integer(const string_t& name,\n                            const std::int64_t value)\n    {\n        if ((std::numeric_limits<std::int32_t>::min)() <= value && value <= (std::numeric_limits<std::int32_t>::max)())\n        {\n            write_bson_entry_header(name, 0x10); // int32\n            write_number<std::int32_t>(static_cast<std::int32_t>(value), true);\n        }\n        else\n        {\n            write_bson_entry_header(name, 0x12); // int64\n            write_number<std::int64_t>(static_cast<std::int64_t>(value), true);\n        }\n    }\n\n    /*!\n    @return The size of the BSON-encoded unsigned integer in @a j\n    */\n    static constexpr std::size_t calc_bson_unsigned_size(const std::uint64_t value) noexcept\n    {\n        return (value <= static_cast<std::uint64_t>((std::numeric_limits<std::int32_t>::max)()))\n               ? sizeof(std::int32_t)\n               : sizeof(std::int64_t);\n    }\n\n    /*!\n    @brief Writes a BSON element with key @a name and unsigned @a value\n    */\n    void write_bson_unsigned(const string_t& name,\n                             const BasicJsonType& j)\n    {\n        if (j.m_value.number_unsigned <= static_cast<std::uint64_t>((std::numeric_limits<std::int32_t>::max)()))\n        {\n            write_bson_entry_header(name, 0x10 /* int32 */);\n            write_number<std::int32_t>(static_cast<std::int32_t>(j.m_value.number_unsigned), true);\n        }\n        else if (j.m_value.number_unsigned <= static_cast<std::uint64_t>((std::numeric_limits<std::int64_t>::max)()))\n        {\n            write_bson_entry_header(name, 0x12 /* int64 */);\n            write_number<std::int64_t>(static_cast<std::int64_t>(j.m_value.number_unsigned), true);\n        }\n        else\n        {\n            JSON_THROW(out_of_range::create(407, concat(\"integer number \", std::to_string(j.m_value.number_unsigned), \" cannot be represented by BSON as it does not fit int64\"), &j));\n        }\n    }\n\n    /*!\n    @brief Writes a BSON element with key @a name and object @a value\n    */\n    void write_bson_object_entry(const string_t& name,\n                                 const typename BasicJsonType::object_t& value)\n    {\n        write_bson_entry_header(name, 0x03); // object\n        write_bson_object(value);\n    }\n\n    /*!\n    @return The size of the BSON-encoded array @a value\n    */\n    static std::size_t calc_bson_array_size(const typename BasicJsonType::array_t& value)\n    {\n        std::size_t array_index = 0ul;\n\n        const std::size_t embedded_document_size = std::accumulate(std::begin(value), std::end(value), static_cast<std::size_t>(0), [&array_index](std::size_t result, const typename BasicJsonType::array_t::value_type & el)\n        {\n            return result + calc_bson_element_size(std::to_string(array_index++), el);\n        });\n\n        return sizeof(std::int32_t) + embedded_document_size + 1ul;\n    }\n\n    /*!\n    @return The size of the BSON-encoded binary array @a value\n    */\n    static std::size_t calc_bson_binary_size(const typename BasicJsonType::binary_t& value)\n    {\n        return sizeof(std::int32_t) + value.size() + 1ul;\n    }\n\n    /*!\n    @brief Writes a BSON element with key @a name and array @a value\n    */\n    void write_bson_array(const string_t& name,\n                          const typename BasicJsonType::array_t& value)\n    {\n        write_bson_entry_header(name, 0x04); // array\n        write_number<std::int32_t>(static_cast<std::int32_t>(calc_bson_array_size(value)), true);\n\n        std::size_t array_index = 0ul;\n\n        for (const auto& el : value)\n        {\n            write_bson_element(std::to_string(array_index++), el);\n        }\n\n        oa->write_character(to_char_type(0x00));\n    }\n\n    /*!\n    @brief Writes a BSON element with key @a name and binary value @a value\n    */\n    void write_bson_binary(const string_t& name,\n                           const binary_t& value)\n    {\n        write_bson_entry_header(name, 0x05);\n\n        write_number<std::int32_t>(static_cast<std::int32_t>(value.size()), true);\n        write_number(value.has_subtype() ? static_cast<std::uint8_t>(value.subtype()) : static_cast<std::uint8_t>(0x00));\n\n        oa->write_characters(reinterpret_cast<const CharType*>(value.data()), value.size());\n    }\n\n    /*!\n    @brief Calculates the size necessary to serialize the JSON value @a j with its @a name\n    @return The calculated size for the BSON document entry for @a j with the given @a name.\n    */\n    static std::size_t calc_bson_element_size(const string_t& name,\n            const BasicJsonType& j)\n    {\n        const auto header_size = calc_bson_entry_header_size(name, j);\n        switch (j.type())\n        {\n            case value_t::object:\n                return header_size + calc_bson_object_size(*j.m_value.object);\n\n            case value_t::array:\n                return header_size + calc_bson_array_size(*j.m_value.array);\n\n            case value_t::binary:\n                return header_size + calc_bson_binary_size(*j.m_value.binary);\n\n            case value_t::boolean:\n                return header_size + 1ul;\n\n            case value_t::number_float:\n                return header_size + 8ul;\n\n            case value_t::number_integer:\n                return header_size + calc_bson_integer_size(j.m_value.number_integer);\n\n            case value_t::number_unsigned:\n                return header_size + calc_bson_unsigned_size(j.m_value.number_unsigned);\n\n            case value_t::string:\n                return header_size + calc_bson_string_size(*j.m_value.string);\n\n            case value_t::null:\n                return header_size + 0ul;\n\n            // LCOV_EXCL_START\n            case value_t::discarded:\n            default:\n                JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert)\n                return 0ul;\n                // LCOV_EXCL_STOP\n        }\n    }\n\n    /*!\n    @brief Serializes the JSON value @a j to BSON and associates it with the\n           key @a name.\n    @param name The name to associate with the JSON entity @a j within the\n                current BSON document\n    */\n    void write_bson_element(const string_t& name,\n                            const BasicJsonType& j)\n    {\n        switch (j.type())\n        {\n            case value_t::object:\n                return write_bson_object_entry(name, *j.m_value.object);\n\n            case value_t::array:\n                return write_bson_array(name, *j.m_value.array);\n\n            case value_t::binary:\n                return write_bson_binary(name, *j.m_value.binary);\n\n            case value_t::boolean:\n                return write_bson_boolean(name, j.m_value.boolean);\n\n            case value_t::number_float:\n                return write_bson_double(name, j.m_value.number_float);\n\n            case value_t::number_integer:\n                return write_bson_integer(name, j.m_value.number_integer);\n\n            case value_t::number_unsigned:\n                return write_bson_unsigned(name, j);\n\n            case value_t::string:\n                return write_bson_string(name, *j.m_value.string);\n\n            case value_t::null:\n                return write_bson_null(name);\n\n            // LCOV_EXCL_START\n            case value_t::discarded:\n            default:\n                JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert)\n                return;\n                // LCOV_EXCL_STOP\n        }\n    }\n\n    /*!\n    @brief Calculates the size of the BSON serialization of the given\n           JSON-object @a j.\n    @param[in] value  JSON value to serialize\n    @pre       value.type() == value_t::object\n    */\n    static std::size_t calc_bson_object_size(const typename BasicJsonType::object_t& value)\n    {\n        std::size_t document_size = std::accumulate(value.begin(), value.end(), static_cast<std::size_t>(0),\n                                    [](size_t result, const typename BasicJsonType::object_t::value_type & el)\n        {\n            return result += calc_bson_element_size(el.first, el.second);\n        });\n\n        return sizeof(std::int32_t) + document_size + 1ul;\n    }\n\n    /*!\n    @param[in] value  JSON value to serialize\n    @pre       value.type() == value_t::object\n    */\n    void write_bson_object(const typename BasicJsonType::object_t& value)\n    {\n        write_number<std::int32_t>(static_cast<std::int32_t>(calc_bson_object_size(value)), true);\n\n        for (const auto& el : value)\n        {\n            write_bson_element(el.first, el.second);\n        }\n\n        oa->write_character(to_char_type(0x00));\n    }\n\n    //////////\n    // CBOR //\n    //////////\n\n    static constexpr CharType get_cbor_float_prefix(float /*unused*/)\n    {\n        return to_char_type(0xFA);  // Single-Precision Float\n    }\n\n    static constexpr CharType get_cbor_float_prefix(double /*unused*/)\n    {\n        return to_char_type(0xFB);  // Double-Precision Float\n    }\n\n    /////////////\n    // MsgPack //\n    /////////////\n\n    static constexpr CharType get_msgpack_float_prefix(float /*unused*/)\n    {\n        return to_char_type(0xCA);  // float 32\n    }\n\n    static constexpr CharType get_msgpack_float_prefix(double /*unused*/)\n    {\n        return to_char_type(0xCB);  // float 64\n    }\n\n    ////////////\n    // UBJSON //\n    ////////////\n\n    // UBJSON: write number (floating point)\n    template<typename NumberType, typename std::enable_if<\n                 std::is_floating_point<NumberType>::value, int>::type = 0>\n    void write_number_with_ubjson_prefix(const NumberType n,\n                                         const bool add_prefix,\n                                         const bool use_bjdata)\n    {\n        if (add_prefix)\n        {\n            oa->write_character(get_ubjson_float_prefix(n));\n        }\n        write_number(n, use_bjdata);\n    }\n\n    // UBJSON: write number (unsigned integer)\n    template<typename NumberType, typename std::enable_if<\n                 std::is_unsigned<NumberType>::value, int>::type = 0>\n    void write_number_with_ubjson_prefix(const NumberType n,\n                                         const bool add_prefix,\n                                         const bool use_bjdata)\n    {\n        if (n <= static_cast<std::uint64_t>((std::numeric_limits<std::int8_t>::max)()))\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('i'));  // int8\n            }\n            write_number(static_cast<std::uint8_t>(n), use_bjdata);\n        }\n        else if (n <= (std::numeric_limits<std::uint8_t>::max)())\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('U'));  // uint8\n            }\n            write_number(static_cast<std::uint8_t>(n), use_bjdata);\n        }\n        else if (n <= static_cast<std::uint64_t>((std::numeric_limits<std::int16_t>::max)()))\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('I'));  // int16\n            }\n            write_number(static_cast<std::int16_t>(n), use_bjdata);\n        }\n        else if (use_bjdata && n <= static_cast<uint64_t>((std::numeric_limits<uint16_t>::max)()))\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('u'));  // uint16 - bjdata only\n            }\n            write_number(static_cast<std::uint16_t>(n), use_bjdata);\n        }\n        else if (n <= static_cast<std::uint64_t>((std::numeric_limits<std::int32_t>::max)()))\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('l'));  // int32\n            }\n            write_number(static_cast<std::int32_t>(n), use_bjdata);\n        }\n        else if (use_bjdata && n <= static_cast<uint64_t>((std::numeric_limits<uint32_t>::max)()))\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('m'));  // uint32 - bjdata only\n            }\n            write_number(static_cast<std::uint32_t>(n), use_bjdata);\n        }\n        else if (n <= static_cast<std::uint64_t>((std::numeric_limits<std::int64_t>::max)()))\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('L'));  // int64\n            }\n            write_number(static_cast<std::int64_t>(n), use_bjdata);\n        }\n        else if (use_bjdata && n <= (std::numeric_limits<uint64_t>::max)())\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('M'));  // uint64 - bjdata only\n            }\n            write_number(static_cast<std::uint64_t>(n), use_bjdata);\n        }\n        else\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('H'));  // high-precision number\n            }\n\n            const auto number = BasicJsonType(n).dump();\n            write_number_with_ubjson_prefix(number.size(), true, use_bjdata);\n            for (std::size_t i = 0; i < number.size(); ++i)\n            {\n                oa->write_character(to_char_type(static_cast<std::uint8_t>(number[i])));\n            }\n        }\n    }\n\n    // UBJSON: write number (signed integer)\n    template < typename NumberType, typename std::enable_if <\n                   std::is_signed<NumberType>::value&&\n                   !std::is_floating_point<NumberType>::value, int >::type = 0 >\n    void write_number_with_ubjson_prefix(const NumberType n,\n                                         const bool add_prefix,\n                                         const bool use_bjdata)\n    {\n        if ((std::numeric_limits<std::int8_t>::min)() <= n && n <= (std::numeric_limits<std::int8_t>::max)())\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('i'));  // int8\n            }\n            write_number(static_cast<std::int8_t>(n), use_bjdata);\n        }\n        else if (static_cast<std::int64_t>((std::numeric_limits<std::uint8_t>::min)()) <= n && n <= static_cast<std::int64_t>((std::numeric_limits<std::uint8_t>::max)()))\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('U'));  // uint8\n            }\n            write_number(static_cast<std::uint8_t>(n), use_bjdata);\n        }\n        else if ((std::numeric_limits<std::int16_t>::min)() <= n && n <= (std::numeric_limits<std::int16_t>::max)())\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('I'));  // int16\n            }\n            write_number(static_cast<std::int16_t>(n), use_bjdata);\n        }\n        else if (use_bjdata && (static_cast<std::int64_t>((std::numeric_limits<std::uint16_t>::min)()) <= n && n <= static_cast<std::int64_t>((std::numeric_limits<std::uint16_t>::max)())))\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('u'));  // uint16 - bjdata only\n            }\n            write_number(static_cast<uint16_t>(n), use_bjdata);\n        }\n        else if ((std::numeric_limits<std::int32_t>::min)() <= n && n <= (std::numeric_limits<std::int32_t>::max)())\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('l'));  // int32\n            }\n            write_number(static_cast<std::int32_t>(n), use_bjdata);\n        }\n        else if (use_bjdata && (static_cast<std::int64_t>((std::numeric_limits<std::uint32_t>::min)()) <= n && n <= static_cast<std::int64_t>((std::numeric_limits<std::uint32_t>::max)())))\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('m'));  // uint32 - bjdata only\n            }\n            write_number(static_cast<uint32_t>(n), use_bjdata);\n        }\n        else if ((std::numeric_limits<std::int64_t>::min)() <= n && n <= (std::numeric_limits<std::int64_t>::max)())\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('L'));  // int64\n            }\n            write_number(static_cast<std::int64_t>(n), use_bjdata);\n        }\n        // LCOV_EXCL_START\n        else\n        {\n            if (add_prefix)\n            {\n                oa->write_character(to_char_type('H'));  // high-precision number\n            }\n\n            const auto number = BasicJsonType(n).dump();\n            write_number_with_ubjson_prefix(number.size(), true, use_bjdata);\n            for (std::size_t i = 0; i < number.size(); ++i)\n            {\n                oa->write_character(to_char_type(static_cast<std::uint8_t>(number[i])));\n            }\n        }\n        // LCOV_EXCL_STOP\n    }\n\n    /*!\n    @brief determine the type prefix of container values\n    */\n    CharType ubjson_prefix(const BasicJsonType& j, const bool use_bjdata) const noexcept\n    {\n        switch (j.type())\n        {\n            case value_t::null:\n                return 'Z';\n\n            case value_t::boolean:\n                return j.m_value.boolean ? 'T' : 'F';\n\n            case value_t::number_integer:\n            {\n                if ((std::numeric_limits<std::int8_t>::min)() <= j.m_value.number_integer && j.m_value.number_integer <= (std::numeric_limits<std::int8_t>::max)())\n                {\n                    return 'i';\n                }\n                if ((std::numeric_limits<std::uint8_t>::min)() <= j.m_value.number_integer && j.m_value.number_integer <= (std::numeric_limits<std::uint8_t>::max)())\n                {\n                    return 'U';\n                }\n                if ((std::numeric_limits<std::int16_t>::min)() <= j.m_value.number_integer && j.m_value.number_integer <= (std::numeric_limits<std::int16_t>::max)())\n                {\n                    return 'I';\n                }\n                if (use_bjdata && ((std::numeric_limits<std::uint16_t>::min)() <= j.m_value.number_integer && j.m_value.number_integer <= (std::numeric_limits<std::uint16_t>::max)()))\n                {\n                    return 'u';\n                }\n                if ((std::numeric_limits<std::int32_t>::min)() <= j.m_value.number_integer && j.m_value.number_integer <= (std::numeric_limits<std::int32_t>::max)())\n                {\n                    return 'l';\n                }\n                if (use_bjdata && ((std::numeric_limits<std::uint32_t>::min)() <= j.m_value.number_integer && j.m_value.number_integer <= (std::numeric_limits<std::uint32_t>::max)()))\n                {\n                    return 'm';\n                }\n                if ((std::numeric_limits<std::int64_t>::min)() <= j.m_value.number_integer && j.m_value.number_integer <= (std::numeric_limits<std::int64_t>::max)())\n                {\n                    return 'L';\n                }\n                // anything else is treated as high-precision number\n                return 'H'; // LCOV_EXCL_LINE\n            }\n\n            case value_t::number_unsigned:\n            {\n                if (j.m_value.number_unsigned <= static_cast<std::uint64_t>((std::numeric_limits<std::int8_t>::max)()))\n                {\n                    return 'i';\n                }\n                if (j.m_value.number_unsigned <= static_cast<std::uint64_t>((std::numeric_limits<std::uint8_t>::max)()))\n                {\n                    return 'U';\n                }\n                if (j.m_value.number_unsigned <= static_cast<std::uint64_t>((std::numeric_limits<std::int16_t>::max)()))\n                {\n                    return 'I';\n                }\n                if (use_bjdata && j.m_value.number_unsigned <= static_cast<std::uint64_t>((std::numeric_limits<std::uint16_t>::max)()))\n                {\n                    return 'u';\n                }\n                if (j.m_value.number_unsigned <= static_cast<std::uint64_t>((std::numeric_limits<std::int32_t>::max)()))\n                {\n                    return 'l';\n                }\n                if (use_bjdata && j.m_value.number_unsigned <= static_cast<std::uint64_t>((std::numeric_limits<std::uint32_t>::max)()))\n                {\n                    return 'm';\n                }\n                if (j.m_value.number_unsigned <= static_cast<std::uint64_t>((std::numeric_limits<std::int64_t>::max)()))\n                {\n                    return 'L';\n                }\n                if (use_bjdata && j.m_value.number_unsigned <= (std::numeric_limits<std::uint64_t>::max)())\n                {\n                    return 'M';\n                }\n                // anything else is treated as high-precision number\n                return 'H'; // LCOV_EXCL_LINE\n            }\n\n            case value_t::number_float:\n                return get_ubjson_float_prefix(j.m_value.number_float);\n\n            case value_t::string:\n                return 'S';\n\n            case value_t::array: // fallthrough\n            case value_t::binary:\n                return '[';\n\n            case value_t::object:\n                return '{';\n\n            case value_t::discarded:\n            default:  // discarded values\n                return 'N';\n        }\n    }\n\n    static constexpr CharType get_ubjson_float_prefix(float /*unused*/)\n    {\n        return 'd';  // float 32\n    }\n\n    static constexpr CharType get_ubjson_float_prefix(double /*unused*/)\n    {\n        return 'D';  // float 64\n    }\n\n    /*!\n    @return false if the object is successfully converted to a bjdata ndarray, true if the type or size is invalid\n    */\n    bool write_bjdata_ndarray(const typename BasicJsonType::object_t& value, const bool use_count, const bool use_type)\n    {\n        std::map<string_t, CharType> bjdtype = {{\"uint8\", 'U'},  {\"int8\", 'i'},  {\"uint16\", 'u'}, {\"int16\", 'I'},\n            {\"uint32\", 'm'}, {\"int32\", 'l'}, {\"uint64\", 'M'}, {\"int64\", 'L'}, {\"single\", 'd'}, {\"double\", 'D'}, {\"char\", 'C'}\n        };\n\n        string_t key = \"_ArrayType_\";\n        auto it = bjdtype.find(static_cast<string_t>(value.at(key)));\n        if (it == bjdtype.end())\n        {\n            return true;\n        }\n        CharType dtype = it->second;\n\n        key = \"_ArraySize_\";\n        std::size_t len = (value.at(key).empty() ? 0 : 1);\n        for (const auto& el : value.at(key))\n        {\n            len *= static_cast<std::size_t>(el.m_value.number_unsigned);\n        }\n\n        key = \"_ArrayData_\";\n        if (value.at(key).size() != len)\n        {\n            return true;\n        }\n\n        oa->write_character('[');\n        oa->write_character('$');\n        oa->write_character(dtype);\n        oa->write_character('#');\n\n        key = \"_ArraySize_\";\n        write_ubjson(value.at(key), use_count, use_type, true,  true);\n\n        key = \"_ArrayData_\";\n        if (dtype == 'U' || dtype == 'C')\n        {\n            for (const auto& el : value.at(key))\n            {\n                write_number(static_cast<std::uint8_t>(el.m_value.number_unsigned), true);\n            }\n        }\n        else if (dtype == 'i')\n        {\n            for (const auto& el : value.at(key))\n            {\n                write_number(static_cast<std::int8_t>(el.m_value.number_integer), true);\n            }\n        }\n        else if (dtype == 'u')\n        {\n            for (const auto& el : value.at(key))\n            {\n                write_number(static_cast<std::uint16_t>(el.m_value.number_unsigned), true);\n            }\n        }\n        else if (dtype == 'I')\n        {\n            for (const auto& el : value.at(key))\n            {\n                write_number(static_cast<std::int16_t>(el.m_value.number_integer), true);\n            }\n        }\n        else if (dtype == 'm')\n        {\n            for (const auto& el : value.at(key))\n            {\n                write_number(static_cast<std::uint32_t>(el.m_value.number_unsigned), true);\n            }\n        }\n        else if (dtype == 'l')\n        {\n            for (const auto& el : value.at(key))\n            {\n                write_number(static_cast<std::int32_t>(el.m_value.number_integer), true);\n            }\n        }\n        else if (dtype == 'M')\n        {\n            for (const auto& el : value.at(key))\n            {\n                write_number(static_cast<std::uint64_t>(el.m_value.number_unsigned), true);\n            }\n        }\n        else if (dtype == 'L')\n        {\n            for (const auto& el : value.at(key))\n            {\n                write_number(static_cast<std::int64_t>(el.m_value.number_integer), true);\n            }\n        }\n        else if (dtype == 'd')\n        {\n            for (const auto& el : value.at(key))\n            {\n                write_number(static_cast<float>(el.m_value.number_float), true);\n            }\n        }\n        else if (dtype == 'D')\n        {\n            for (const auto& el : value.at(key))\n            {\n                write_number(static_cast<double>(el.m_value.number_float), true);\n            }\n        }\n        return false;\n    }\n\n    ///////////////////////\n    // Utility functions //\n    ///////////////////////\n\n    /*\n    @brief write a number to output input\n    @param[in] n number of type @a NumberType\n    @param[in] OutputIsLittleEndian Set to true if output data is\n                                 required to be little endian\n    @tparam NumberType the type of the number\n\n    @note This function needs to respect the system's endianness, because bytes\n          in CBOR, MessagePack, and UBJSON are stored in network order (big\n          endian) and therefore need reordering on little endian systems.\n          On the other hand, BSON and BJData use little endian and should reorder\n          on big endian systems.\n    */\n    template<typename NumberType>\n    void write_number(const NumberType n, const bool OutputIsLittleEndian = false)\n    {\n        // step 1: write number to array of length NumberType\n        std::array<CharType, sizeof(NumberType)> vec{};\n        std::memcpy(vec.data(), &n, sizeof(NumberType));\n\n        // step 2: write array to output (with possible reordering)\n        if (is_little_endian != OutputIsLittleEndian)\n        {\n            // reverse byte order prior to conversion if necessary\n            std::reverse(vec.begin(), vec.end());\n        }\n\n        oa->write_characters(vec.data(), sizeof(NumberType));\n    }\n\n    void write_compact_float(const number_float_t n, detail::input_format_t format)\n    {\n#ifdef __GNUC__\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wfloat-equal\"\n#endif\n        if (static_cast<double>(n) >= static_cast<double>(std::numeric_limits<float>::lowest()) &&\n                static_cast<double>(n) <= static_cast<double>((std::numeric_limits<float>::max)()) &&\n                static_cast<double>(static_cast<float>(n)) == static_cast<double>(n))\n        {\n            oa->write_character(format == detail::input_format_t::cbor\n                                ? get_cbor_float_prefix(static_cast<float>(n))\n                                : get_msgpack_float_prefix(static_cast<float>(n)));\n            write_number(static_cast<float>(n));\n        }\n        else\n        {\n            oa->write_character(format == detail::input_format_t::cbor\n                                ? get_cbor_float_prefix(n)\n                                : get_msgpack_float_prefix(n));\n            write_number(n);\n        }\n#ifdef __GNUC__\n#pragma GCC diagnostic pop\n#endif\n    }\n\n  public:\n    // The following to_char_type functions are implement the conversion\n    // between uint8_t and CharType. In case CharType is not unsigned,\n    // such a conversion is required to allow values greater than 128.\n    // See <https://github.com/nlohmann/json/issues/1286> for a discussion.\n    template < typename C = CharType,\n               enable_if_t < std::is_signed<C>::value && std::is_signed<char>::value > * = nullptr >\n    static constexpr CharType to_char_type(std::uint8_t x) noexcept\n    {\n        return *reinterpret_cast<char*>(&x);\n    }\n\n    template < typename C = CharType,\n               enable_if_t < std::is_signed<C>::value && std::is_unsigned<char>::value > * = nullptr >\n    static CharType to_char_type(std::uint8_t x) noexcept\n    {\n        static_assert(sizeof(std::uint8_t) == sizeof(CharType), \"size of CharType must be equal to std::uint8_t\");\n        static_assert(std::is_trivial<CharType>::value, \"CharType must be trivial\");\n        CharType result;\n        std::memcpy(&result, &x, sizeof(x));\n        return result;\n    }\n\n    template<typename C = CharType,\n             enable_if_t<std::is_unsigned<C>::value>* = nullptr>\n    static constexpr CharType to_char_type(std::uint8_t x) noexcept\n    {\n        return x;\n    }\n\n    template < typename InputCharType, typename C = CharType,\n               enable_if_t <\n                   std::is_signed<C>::value &&\n                   std::is_signed<char>::value &&\n                   std::is_same<char, typename std::remove_cv<InputCharType>::type>::value\n                   > * = nullptr >\n    static constexpr CharType to_char_type(InputCharType x) noexcept\n    {\n        return x;\n    }\n\n  private:\n    /// whether we can assume little endianness\n    const bool is_little_endian = little_endianness();\n\n    /// the output\n    output_adapter_t<CharType> oa = nullptr;\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/output/output_adapters.hpp>\n\n// #include <nlohmann/detail/output/serializer.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2008-2009 Björn Hoehrmann <bjoern@hoehrmann.de>\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <algorithm> // reverse, remove, fill, find, none_of\n#include <array> // array\n#include <clocale> // localeconv, lconv\n#include <cmath> // labs, isfinite, isnan, signbit\n#include <cstddef> // size_t, ptrdiff_t\n#include <cstdint> // uint8_t\n#include <cstdio> // snprintf\n#include <limits> // numeric_limits\n#include <string> // string, char_traits\n#include <iomanip> // setfill, setw\n#include <type_traits> // is_same\n#include <utility> // move\n\n// #include <nlohmann/detail/conversions/to_chars.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2009 Florian Loitsch <https://florian.loitsch.com/>\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <array> // array\n#include <cmath>   // signbit, isfinite\n#include <cstdint> // intN_t, uintN_t\n#include <cstring> // memcpy, memmove\n#include <limits> // numeric_limits\n#include <type_traits> // conditional\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n/*!\n@brief implements the Grisu2 algorithm for binary to decimal floating-point\nconversion.\n\nThis implementation is a slightly modified version of the reference\nimplementation which may be obtained from\nhttp://florian.loitsch.com/publications (bench.tar.gz).\n\nThe code is distributed under the MIT license, Copyright (c) 2009 Florian Loitsch.\n\nFor a detailed description of the algorithm see:\n\n[1] Loitsch, \"Printing Floating-Point Numbers Quickly and Accurately with\n    Integers\", Proceedings of the ACM SIGPLAN 2010 Conference on Programming\n    Language Design and Implementation, PLDI 2010\n[2] Burger, Dybvig, \"Printing Floating-Point Numbers Quickly and Accurately\",\n    Proceedings of the ACM SIGPLAN 1996 Conference on Programming Language\n    Design and Implementation, PLDI 1996\n*/\nnamespace dtoa_impl\n{\n\ntemplate<typename Target, typename Source>\nTarget reinterpret_bits(const Source source)\n{\n    static_assert(sizeof(Target) == sizeof(Source), \"size mismatch\");\n\n    Target target;\n    std::memcpy(&target, &source, sizeof(Source));\n    return target;\n}\n\nstruct diyfp // f * 2^e\n{\n    static constexpr int kPrecision = 64; // = q\n\n    std::uint64_t f = 0;\n    int e = 0;\n\n    constexpr diyfp(std::uint64_t f_, int e_) noexcept : f(f_), e(e_) {}\n\n    /*!\n    @brief returns x - y\n    @pre x.e == y.e and x.f >= y.f\n    */\n    static diyfp sub(const diyfp& x, const diyfp& y) noexcept\n    {\n        JSON_ASSERT(x.e == y.e);\n        JSON_ASSERT(x.f >= y.f);\n\n        return {x.f - y.f, x.e};\n    }\n\n    /*!\n    @brief returns x * y\n    @note The result is rounded. (Only the upper q bits are returned.)\n    */\n    static diyfp mul(const diyfp& x, const diyfp& y) noexcept\n    {\n        static_assert(kPrecision == 64, \"internal error\");\n\n        // Computes:\n        //  f = round((x.f * y.f) / 2^q)\n        //  e = x.e + y.e + q\n\n        // Emulate the 64-bit * 64-bit multiplication:\n        //\n        // p = u * v\n        //   = (u_lo + 2^32 u_hi) (v_lo + 2^32 v_hi)\n        //   = (u_lo v_lo         ) + 2^32 ((u_lo v_hi         ) + (u_hi v_lo         )) + 2^64 (u_hi v_hi         )\n        //   = (p0                ) + 2^32 ((p1                ) + (p2                )) + 2^64 (p3                )\n        //   = (p0_lo + 2^32 p0_hi) + 2^32 ((p1_lo + 2^32 p1_hi) + (p2_lo + 2^32 p2_hi)) + 2^64 (p3                )\n        //   = (p0_lo             ) + 2^32 (p0_hi + p1_lo + p2_lo                      ) + 2^64 (p1_hi + p2_hi + p3)\n        //   = (p0_lo             ) + 2^32 (Q                                          ) + 2^64 (H                 )\n        //   = (p0_lo             ) + 2^32 (Q_lo + 2^32 Q_hi                           ) + 2^64 (H                 )\n        //\n        // (Since Q might be larger than 2^32 - 1)\n        //\n        //   = (p0_lo + 2^32 Q_lo) + 2^64 (Q_hi + H)\n        //\n        // (Q_hi + H does not overflow a 64-bit int)\n        //\n        //   = p_lo + 2^64 p_hi\n\n        const std::uint64_t u_lo = x.f & 0xFFFFFFFFu;\n        const std::uint64_t u_hi = x.f >> 32u;\n        const std::uint64_t v_lo = y.f & 0xFFFFFFFFu;\n        const std::uint64_t v_hi = y.f >> 32u;\n\n        const std::uint64_t p0 = u_lo * v_lo;\n        const std::uint64_t p1 = u_lo * v_hi;\n        const std::uint64_t p2 = u_hi * v_lo;\n        const std::uint64_t p3 = u_hi * v_hi;\n\n        const std::uint64_t p0_hi = p0 >> 32u;\n        const std::uint64_t p1_lo = p1 & 0xFFFFFFFFu;\n        const std::uint64_t p1_hi = p1 >> 32u;\n        const std::uint64_t p2_lo = p2 & 0xFFFFFFFFu;\n        const std::uint64_t p2_hi = p2 >> 32u;\n\n        std::uint64_t Q = p0_hi + p1_lo + p2_lo;\n\n        // The full product might now be computed as\n        //\n        // p_hi = p3 + p2_hi + p1_hi + (Q >> 32)\n        // p_lo = p0_lo + (Q << 32)\n        //\n        // But in this particular case here, the full p_lo is not required.\n        // Effectively we only need to add the highest bit in p_lo to p_hi (and\n        // Q_hi + 1 does not overflow).\n\n        Q += std::uint64_t{1} << (64u - 32u - 1u); // round, ties up\n\n        const std::uint64_t h = p3 + p2_hi + p1_hi + (Q >> 32u);\n\n        return {h, x.e + y.e + 64};\n    }\n\n    /*!\n    @brief normalize x such that the significand is >= 2^(q-1)\n    @pre x.f != 0\n    */\n    static diyfp normalize(diyfp x) noexcept\n    {\n        JSON_ASSERT(x.f != 0);\n\n        while ((x.f >> 63u) == 0)\n        {\n            x.f <<= 1u;\n            x.e--;\n        }\n\n        return x;\n    }\n\n    /*!\n    @brief normalize x such that the result has the exponent E\n    @pre e >= x.e and the upper e - x.e bits of x.f must be zero.\n    */\n    static diyfp normalize_to(const diyfp& x, const int target_exponent) noexcept\n    {\n        const int delta = x.e - target_exponent;\n\n        JSON_ASSERT(delta >= 0);\n        JSON_ASSERT(((x.f << delta) >> delta) == x.f);\n\n        return {x.f << delta, target_exponent};\n    }\n};\n\nstruct boundaries\n{\n    diyfp w;\n    diyfp minus;\n    diyfp plus;\n};\n\n/*!\nCompute the (normalized) diyfp representing the input number 'value' and its\nboundaries.\n\n@pre value must be finite and positive\n*/\ntemplate<typename FloatType>\nboundaries compute_boundaries(FloatType value)\n{\n    JSON_ASSERT(std::isfinite(value));\n    JSON_ASSERT(value > 0);\n\n    // Convert the IEEE representation into a diyfp.\n    //\n    // If v is denormal:\n    //      value = 0.F * 2^(1 - bias) = (          F) * 2^(1 - bias - (p-1))\n    // If v is normalized:\n    //      value = 1.F * 2^(E - bias) = (2^(p-1) + F) * 2^(E - bias - (p-1))\n\n    static_assert(std::numeric_limits<FloatType>::is_iec559,\n                  \"internal error: dtoa_short requires an IEEE-754 floating-point implementation\");\n\n    constexpr int      kPrecision = std::numeric_limits<FloatType>::digits; // = p (includes the hidden bit)\n    constexpr int      kBias      = std::numeric_limits<FloatType>::max_exponent - 1 + (kPrecision - 1);\n    constexpr int      kMinExp    = 1 - kBias;\n    constexpr std::uint64_t kHiddenBit = std::uint64_t{1} << (kPrecision - 1); // = 2^(p-1)\n\n    using bits_type = typename std::conditional<kPrecision == 24, std::uint32_t, std::uint64_t >::type;\n\n    const auto bits = static_cast<std::uint64_t>(reinterpret_bits<bits_type>(value));\n    const std::uint64_t E = bits >> (kPrecision - 1);\n    const std::uint64_t F = bits & (kHiddenBit - 1);\n\n    const bool is_denormal = E == 0;\n    const diyfp v = is_denormal\n                    ? diyfp(F, kMinExp)\n                    : diyfp(F + kHiddenBit, static_cast<int>(E) - kBias);\n\n    // Compute the boundaries m- and m+ of the floating-point value\n    // v = f * 2^e.\n    //\n    // Determine v- and v+, the floating-point predecessor and successor if v,\n    // respectively.\n    //\n    //      v- = v - 2^e        if f != 2^(p-1) or e == e_min                (A)\n    //         = v - 2^(e-1)    if f == 2^(p-1) and e > e_min                (B)\n    //\n    //      v+ = v + 2^e\n    //\n    // Let m- = (v- + v) / 2 and m+ = (v + v+) / 2. All real numbers _strictly_\n    // between m- and m+ round to v, regardless of how the input rounding\n    // algorithm breaks ties.\n    //\n    //      ---+-------------+-------------+-------------+-------------+---  (A)\n    //         v-            m-            v             m+            v+\n    //\n    //      -----------------+------+------+-------------+-------------+---  (B)\n    //                       v-     m-     v             m+            v+\n\n    const bool lower_boundary_is_closer = F == 0 && E > 1;\n    const diyfp m_plus = diyfp(2 * v.f + 1, v.e - 1);\n    const diyfp m_minus = lower_boundary_is_closer\n                          ? diyfp(4 * v.f - 1, v.e - 2)  // (B)\n                          : diyfp(2 * v.f - 1, v.e - 1); // (A)\n\n    // Determine the normalized w+ = m+.\n    const diyfp w_plus = diyfp::normalize(m_plus);\n\n    // Determine w- = m- such that e_(w-) = e_(w+).\n    const diyfp w_minus = diyfp::normalize_to(m_minus, w_plus.e);\n\n    return {diyfp::normalize(v), w_minus, w_plus};\n}\n\n// Given normalized diyfp w, Grisu needs to find a (normalized) cached\n// power-of-ten c, such that the exponent of the product c * w = f * 2^e lies\n// within a certain range [alpha, gamma] (Definition 3.2 from [1])\n//\n//      alpha <= e = e_c + e_w + q <= gamma\n//\n// or\n//\n//      f_c * f_w * 2^alpha <= f_c 2^(e_c) * f_w 2^(e_w) * 2^q\n//                          <= f_c * f_w * 2^gamma\n//\n// Since c and w are normalized, i.e. 2^(q-1) <= f < 2^q, this implies\n//\n//      2^(q-1) * 2^(q-1) * 2^alpha <= c * w * 2^q < 2^q * 2^q * 2^gamma\n//\n// or\n//\n//      2^(q - 2 + alpha) <= c * w < 2^(q + gamma)\n//\n// The choice of (alpha,gamma) determines the size of the table and the form of\n// the digit generation procedure. Using (alpha,gamma)=(-60,-32) works out well\n// in practice:\n//\n// The idea is to cut the number c * w = f * 2^e into two parts, which can be\n// processed independently: An integral part p1, and a fractional part p2:\n//\n//      f * 2^e = ( (f div 2^-e) * 2^-e + (f mod 2^-e) ) * 2^e\n//              = (f div 2^-e) + (f mod 2^-e) * 2^e\n//              = p1 + p2 * 2^e\n//\n// The conversion of p1 into decimal form requires a series of divisions and\n// modulos by (a power of) 10. These operations are faster for 32-bit than for\n// 64-bit integers, so p1 should ideally fit into a 32-bit integer. This can be\n// achieved by choosing\n//\n//      -e >= 32   or   e <= -32 := gamma\n//\n// In order to convert the fractional part\n//\n//      p2 * 2^e = p2 / 2^-e = d[-1] / 10^1 + d[-2] / 10^2 + ...\n//\n// into decimal form, the fraction is repeatedly multiplied by 10 and the digits\n// d[-i] are extracted in order:\n//\n//      (10 * p2) div 2^-e = d[-1]\n//      (10 * p2) mod 2^-e = d[-2] / 10^1 + ...\n//\n// The multiplication by 10 must not overflow. It is sufficient to choose\n//\n//      10 * p2 < 16 * p2 = 2^4 * p2 <= 2^64.\n//\n// Since p2 = f mod 2^-e < 2^-e,\n//\n//      -e <= 60   or   e >= -60 := alpha\n\nconstexpr int kAlpha = -60;\nconstexpr int kGamma = -32;\n\nstruct cached_power // c = f * 2^e ~= 10^k\n{\n    std::uint64_t f;\n    int e;\n    int k;\n};\n\n/*!\nFor a normalized diyfp w = f * 2^e, this function returns a (normalized) cached\npower-of-ten c = f_c * 2^e_c, such that the exponent of the product w * c\nsatisfies (Definition 3.2 from [1])\n\n     alpha <= e_c + e + q <= gamma.\n*/\ninline cached_power get_cached_power_for_binary_exponent(int e)\n{\n    // Now\n    //\n    //      alpha <= e_c + e + q <= gamma                                    (1)\n    //      ==> f_c * 2^alpha <= c * 2^e * 2^q\n    //\n    // and since the c's are normalized, 2^(q-1) <= f_c,\n    //\n    //      ==> 2^(q - 1 + alpha) <= c * 2^(e + q)\n    //      ==> 2^(alpha - e - 1) <= c\n    //\n    // If c were an exact power of ten, i.e. c = 10^k, one may determine k as\n    //\n    //      k = ceil( log_10( 2^(alpha - e - 1) ) )\n    //        = ceil( (alpha - e - 1) * log_10(2) )\n    //\n    // From the paper:\n    // \"In theory the result of the procedure could be wrong since c is rounded,\n    //  and the computation itself is approximated [...]. In practice, however,\n    //  this simple function is sufficient.\"\n    //\n    // For IEEE double precision floating-point numbers converted into\n    // normalized diyfp's w = f * 2^e, with q = 64,\n    //\n    //      e >= -1022      (min IEEE exponent)\n    //           -52        (p - 1)\n    //           -52        (p - 1, possibly normalize denormal IEEE numbers)\n    //           -11        (normalize the diyfp)\n    //         = -1137\n    //\n    // and\n    //\n    //      e <= +1023      (max IEEE exponent)\n    //           -52        (p - 1)\n    //           -11        (normalize the diyfp)\n    //         = 960\n    //\n    // This binary exponent range [-1137,960] results in a decimal exponent\n    // range [-307,324]. One does not need to store a cached power for each\n    // k in this range. For each such k it suffices to find a cached power\n    // such that the exponent of the product lies in [alpha,gamma].\n    // This implies that the difference of the decimal exponents of adjacent\n    // table entries must be less than or equal to\n    //\n    //      floor( (gamma - alpha) * log_10(2) ) = 8.\n    //\n    // (A smaller distance gamma-alpha would require a larger table.)\n\n    // NB:\n    // Actually this function returns c, such that -60 <= e_c + e + 64 <= -34.\n\n    constexpr int kCachedPowersMinDecExp = -300;\n    constexpr int kCachedPowersDecStep = 8;\n\n    static constexpr std::array<cached_power, 79> kCachedPowers =\n    {\n        {\n            { 0xAB70FE17C79AC6CA, -1060, -300 },\n            { 0xFF77B1FCBEBCDC4F, -1034, -292 },\n            { 0xBE5691EF416BD60C, -1007, -284 },\n            { 0x8DD01FAD907FFC3C,  -980, -276 },\n            { 0xD3515C2831559A83,  -954, -268 },\n            { 0x9D71AC8FADA6C9B5,  -927, -260 },\n            { 0xEA9C227723EE8BCB,  -901, -252 },\n            { 0xAECC49914078536D,  -874, -244 },\n            { 0x823C12795DB6CE57,  -847, -236 },\n            { 0xC21094364DFB5637,  -821, -228 },\n            { 0x9096EA6F3848984F,  -794, -220 },\n            { 0xD77485CB25823AC7,  -768, -212 },\n            { 0xA086CFCD97BF97F4,  -741, -204 },\n            { 0xEF340A98172AACE5,  -715, -196 },\n            { 0xB23867FB2A35B28E,  -688, -188 },\n            { 0x84C8D4DFD2C63F3B,  -661, -180 },\n            { 0xC5DD44271AD3CDBA,  -635, -172 },\n            { 0x936B9FCEBB25C996,  -608, -164 },\n            { 0xDBAC6C247D62A584,  -582, -156 },\n            { 0xA3AB66580D5FDAF6,  -555, -148 },\n            { 0xF3E2F893DEC3F126,  -529, -140 },\n            { 0xB5B5ADA8AAFF80B8,  -502, -132 },\n            { 0x87625F056C7C4A8B,  -475, -124 },\n            { 0xC9BCFF6034C13053,  -449, -116 },\n            { 0x964E858C91BA2655,  -422, -108 },\n            { 0xDFF9772470297EBD,  -396, -100 },\n            { 0xA6DFBD9FB8E5B88F,  -369,  -92 },\n            { 0xF8A95FCF88747D94,  -343,  -84 },\n            { 0xB94470938FA89BCF,  -316,  -76 },\n            { 0x8A08F0F8BF0F156B,  -289,  -68 },\n            { 0xCDB02555653131B6,  -263,  -60 },\n            { 0x993FE2C6D07B7FAC,  -236,  -52 },\n            { 0xE45C10C42A2B3B06,  -210,  -44 },\n            { 0xAA242499697392D3,  -183,  -36 },\n            { 0xFD87B5F28300CA0E,  -157,  -28 },\n            { 0xBCE5086492111AEB,  -130,  -20 },\n            { 0x8CBCCC096F5088CC,  -103,  -12 },\n            { 0xD1B71758E219652C,   -77,   -4 },\n            { 0x9C40000000000000,   -50,    4 },\n            { 0xE8D4A51000000000,   -24,   12 },\n            { 0xAD78EBC5AC620000,     3,   20 },\n            { 0x813F3978F8940984,    30,   28 },\n            { 0xC097CE7BC90715B3,    56,   36 },\n            { 0x8F7E32CE7BEA5C70,    83,   44 },\n            { 0xD5D238A4ABE98068,   109,   52 },\n            { 0x9F4F2726179A2245,   136,   60 },\n            { 0xED63A231D4C4FB27,   162,   68 },\n            { 0xB0DE65388CC8ADA8,   189,   76 },\n            { 0x83C7088E1AAB65DB,   216,   84 },\n            { 0xC45D1DF942711D9A,   242,   92 },\n            { 0x924D692CA61BE758,   269,  100 },\n            { 0xDA01EE641A708DEA,   295,  108 },\n            { 0xA26DA3999AEF774A,   322,  116 },\n            { 0xF209787BB47D6B85,   348,  124 },\n            { 0xB454E4A179DD1877,   375,  132 },\n            { 0x865B86925B9BC5C2,   402,  140 },\n            { 0xC83553C5C8965D3D,   428,  148 },\n            { 0x952AB45CFA97A0B3,   455,  156 },\n            { 0xDE469FBD99A05FE3,   481,  164 },\n            { 0xA59BC234DB398C25,   508,  172 },\n            { 0xF6C69A72A3989F5C,   534,  180 },\n            { 0xB7DCBF5354E9BECE,   561,  188 },\n            { 0x88FCF317F22241E2,   588,  196 },\n            { 0xCC20CE9BD35C78A5,   614,  204 },\n            { 0x98165AF37B2153DF,   641,  212 },\n            { 0xE2A0B5DC971F303A,   667,  220 },\n            { 0xA8D9D1535CE3B396,   694,  228 },\n            { 0xFB9B7CD9A4A7443C,   720,  236 },\n            { 0xBB764C4CA7A44410,   747,  244 },\n            { 0x8BAB8EEFB6409C1A,   774,  252 },\n            { 0xD01FEF10A657842C,   800,  260 },\n            { 0x9B10A4E5E9913129,   827,  268 },\n            { 0xE7109BFBA19C0C9D,   853,  276 },\n            { 0xAC2820D9623BF429,   880,  284 },\n            { 0x80444B5E7AA7CF85,   907,  292 },\n            { 0xBF21E44003ACDD2D,   933,  300 },\n            { 0x8E679C2F5E44FF8F,   960,  308 },\n            { 0xD433179D9C8CB841,   986,  316 },\n            { 0x9E19DB92B4E31BA9,  1013,  324 },\n        }\n    };\n\n    // This computation gives exactly the same results for k as\n    //      k = ceil((kAlpha - e - 1) * 0.30102999566398114)\n    // for |e| <= 1500, but doesn't require floating-point operations.\n    // NB: log_10(2) ~= 78913 / 2^18\n    JSON_ASSERT(e >= -1500);\n    JSON_ASSERT(e <=  1500);\n    const int f = kAlpha - e - 1;\n    const int k = (f * 78913) / (1 << 18) + static_cast<int>(f > 0);\n\n    const int index = (-kCachedPowersMinDecExp + k + (kCachedPowersDecStep - 1)) / kCachedPowersDecStep;\n    JSON_ASSERT(index >= 0);\n    JSON_ASSERT(static_cast<std::size_t>(index) < kCachedPowers.size());\n\n    const cached_power cached = kCachedPowers[static_cast<std::size_t>(index)];\n    JSON_ASSERT(kAlpha <= cached.e + e + 64);\n    JSON_ASSERT(kGamma >= cached.e + e + 64);\n\n    return cached;\n}\n\n/*!\nFor n != 0, returns k, such that pow10 := 10^(k-1) <= n < 10^k.\nFor n == 0, returns 1 and sets pow10 := 1.\n*/\ninline int find_largest_pow10(const std::uint32_t n, std::uint32_t& pow10)\n{\n    // LCOV_EXCL_START\n    if (n >= 1000000000)\n    {\n        pow10 = 1000000000;\n        return 10;\n    }\n    // LCOV_EXCL_STOP\n    if (n >= 100000000)\n    {\n        pow10 = 100000000;\n        return  9;\n    }\n    if (n >= 10000000)\n    {\n        pow10 = 10000000;\n        return  8;\n    }\n    if (n >= 1000000)\n    {\n        pow10 = 1000000;\n        return  7;\n    }\n    if (n >= 100000)\n    {\n        pow10 = 100000;\n        return  6;\n    }\n    if (n >= 10000)\n    {\n        pow10 = 10000;\n        return  5;\n    }\n    if (n >= 1000)\n    {\n        pow10 = 1000;\n        return  4;\n    }\n    if (n >= 100)\n    {\n        pow10 = 100;\n        return  3;\n    }\n    if (n >= 10)\n    {\n        pow10 = 10;\n        return  2;\n    }\n\n    pow10 = 1;\n    return 1;\n}\n\ninline void grisu2_round(char* buf, int len, std::uint64_t dist, std::uint64_t delta,\n                         std::uint64_t rest, std::uint64_t ten_k)\n{\n    JSON_ASSERT(len >= 1);\n    JSON_ASSERT(dist <= delta);\n    JSON_ASSERT(rest <= delta);\n    JSON_ASSERT(ten_k > 0);\n\n    //               <--------------------------- delta ---->\n    //                                  <---- dist --------->\n    // --------------[------------------+-------------------]--------------\n    //               M-                 w                   M+\n    //\n    //                                  ten_k\n    //                                <------>\n    //                                       <---- rest ---->\n    // --------------[------------------+----+--------------]--------------\n    //                                  w    V\n    //                                       = buf * 10^k\n    //\n    // ten_k represents a unit-in-the-last-place in the decimal representation\n    // stored in buf.\n    // Decrement buf by ten_k while this takes buf closer to w.\n\n    // The tests are written in this order to avoid overflow in unsigned\n    // integer arithmetic.\n\n    while (rest < dist\n            && delta - rest >= ten_k\n            && (rest + ten_k < dist || dist - rest > rest + ten_k - dist))\n    {\n        JSON_ASSERT(buf[len - 1] != '0');\n        buf[len - 1]--;\n        rest += ten_k;\n    }\n}\n\n/*!\nGenerates V = buffer * 10^decimal_exponent, such that M- <= V <= M+.\nM- and M+ must be normalized and share the same exponent -60 <= e <= -32.\n*/\ninline void grisu2_digit_gen(char* buffer, int& length, int& decimal_exponent,\n                             diyfp M_minus, diyfp w, diyfp M_plus)\n{\n    static_assert(kAlpha >= -60, \"internal error\");\n    static_assert(kGamma <= -32, \"internal error\");\n\n    // Generates the digits (and the exponent) of a decimal floating-point\n    // number V = buffer * 10^decimal_exponent in the range [M-, M+]. The diyfp's\n    // w, M- and M+ share the same exponent e, which satisfies alpha <= e <= gamma.\n    //\n    //               <--------------------------- delta ---->\n    //                                  <---- dist --------->\n    // --------------[------------------+-------------------]--------------\n    //               M-                 w                   M+\n    //\n    // Grisu2 generates the digits of M+ from left to right and stops as soon as\n    // V is in [M-,M+].\n\n    JSON_ASSERT(M_plus.e >= kAlpha);\n    JSON_ASSERT(M_plus.e <= kGamma);\n\n    std::uint64_t delta = diyfp::sub(M_plus, M_minus).f; // (significand of (M+ - M-), implicit exponent is e)\n    std::uint64_t dist  = diyfp::sub(M_plus, w      ).f; // (significand of (M+ - w ), implicit exponent is e)\n\n    // Split M+ = f * 2^e into two parts p1 and p2 (note: e < 0):\n    //\n    //      M+ = f * 2^e\n    //         = ((f div 2^-e) * 2^-e + (f mod 2^-e)) * 2^e\n    //         = ((p1        ) * 2^-e + (p2        )) * 2^e\n    //         = p1 + p2 * 2^e\n\n    const diyfp one(std::uint64_t{1} << -M_plus.e, M_plus.e);\n\n    auto p1 = static_cast<std::uint32_t>(M_plus.f >> -one.e); // p1 = f div 2^-e (Since -e >= 32, p1 fits into a 32-bit int.)\n    std::uint64_t p2 = M_plus.f & (one.f - 1);                    // p2 = f mod 2^-e\n\n    // 1)\n    //\n    // Generate the digits of the integral part p1 = d[n-1]...d[1]d[0]\n\n    JSON_ASSERT(p1 > 0);\n\n    std::uint32_t pow10{};\n    const int k = find_largest_pow10(p1, pow10);\n\n    //      10^(k-1) <= p1 < 10^k, pow10 = 10^(k-1)\n    //\n    //      p1 = (p1 div 10^(k-1)) * 10^(k-1) + (p1 mod 10^(k-1))\n    //         = (d[k-1]         ) * 10^(k-1) + (p1 mod 10^(k-1))\n    //\n    //      M+ = p1                                             + p2 * 2^e\n    //         = d[k-1] * 10^(k-1) + (p1 mod 10^(k-1))          + p2 * 2^e\n    //         = d[k-1] * 10^(k-1) + ((p1 mod 10^(k-1)) * 2^-e + p2) * 2^e\n    //         = d[k-1] * 10^(k-1) + (                         rest) * 2^e\n    //\n    // Now generate the digits d[n] of p1 from left to right (n = k-1,...,0)\n    //\n    //      p1 = d[k-1]...d[n] * 10^n + d[n-1]...d[0]\n    //\n    // but stop as soon as\n    //\n    //      rest * 2^e = (d[n-1]...d[0] * 2^-e + p2) * 2^e <= delta * 2^e\n\n    int n = k;\n    while (n > 0)\n    {\n        // Invariants:\n        //      M+ = buffer * 10^n + (p1 + p2 * 2^e)    (buffer = 0 for n = k)\n        //      pow10 = 10^(n-1) <= p1 < 10^n\n        //\n        const std::uint32_t d = p1 / pow10;  // d = p1 div 10^(n-1)\n        const std::uint32_t r = p1 % pow10;  // r = p1 mod 10^(n-1)\n        //\n        //      M+ = buffer * 10^n + (d * 10^(n-1) + r) + p2 * 2^e\n        //         = (buffer * 10 + d) * 10^(n-1) + (r + p2 * 2^e)\n        //\n        JSON_ASSERT(d <= 9);\n        buffer[length++] = static_cast<char>('0' + d); // buffer := buffer * 10 + d\n        //\n        //      M+ = buffer * 10^(n-1) + (r + p2 * 2^e)\n        //\n        p1 = r;\n        n--;\n        //\n        //      M+ = buffer * 10^n + (p1 + p2 * 2^e)\n        //      pow10 = 10^n\n        //\n\n        // Now check if enough digits have been generated.\n        // Compute\n        //\n        //      p1 + p2 * 2^e = (p1 * 2^-e + p2) * 2^e = rest * 2^e\n        //\n        // Note:\n        // Since rest and delta share the same exponent e, it suffices to\n        // compare the significands.\n        const std::uint64_t rest = (std::uint64_t{p1} << -one.e) + p2;\n        if (rest <= delta)\n        {\n            // V = buffer * 10^n, with M- <= V <= M+.\n\n            decimal_exponent += n;\n\n            // We may now just stop. But instead look if the buffer could be\n            // decremented to bring V closer to w.\n            //\n            // pow10 = 10^n is now 1 ulp in the decimal representation V.\n            // The rounding procedure works with diyfp's with an implicit\n            // exponent of e.\n            //\n            //      10^n = (10^n * 2^-e) * 2^e = ulp * 2^e\n            //\n            const std::uint64_t ten_n = std::uint64_t{pow10} << -one.e;\n            grisu2_round(buffer, length, dist, delta, rest, ten_n);\n\n            return;\n        }\n\n        pow10 /= 10;\n        //\n        //      pow10 = 10^(n-1) <= p1 < 10^n\n        // Invariants restored.\n    }\n\n    // 2)\n    //\n    // The digits of the integral part have been generated:\n    //\n    //      M+ = d[k-1]...d[1]d[0] + p2 * 2^e\n    //         = buffer            + p2 * 2^e\n    //\n    // Now generate the digits of the fractional part p2 * 2^e.\n    //\n    // Note:\n    // No decimal point is generated: the exponent is adjusted instead.\n    //\n    // p2 actually represents the fraction\n    //\n    //      p2 * 2^e\n    //          = p2 / 2^-e\n    //          = d[-1] / 10^1 + d[-2] / 10^2 + ...\n    //\n    // Now generate the digits d[-m] of p1 from left to right (m = 1,2,...)\n    //\n    //      p2 * 2^e = d[-1]d[-2]...d[-m] * 10^-m\n    //                      + 10^-m * (d[-m-1] / 10^1 + d[-m-2] / 10^2 + ...)\n    //\n    // using\n    //\n    //      10^m * p2 = ((10^m * p2) div 2^-e) * 2^-e + ((10^m * p2) mod 2^-e)\n    //                = (                   d) * 2^-e + (                   r)\n    //\n    // or\n    //      10^m * p2 * 2^e = d + r * 2^e\n    //\n    // i.e.\n    //\n    //      M+ = buffer + p2 * 2^e\n    //         = buffer + 10^-m * (d + r * 2^e)\n    //         = (buffer * 10^m + d) * 10^-m + 10^-m * r * 2^e\n    //\n    // and stop as soon as 10^-m * r * 2^e <= delta * 2^e\n\n    JSON_ASSERT(p2 > delta);\n\n    int m = 0;\n    for (;;)\n    {\n        // Invariant:\n        //      M+ = buffer * 10^-m + 10^-m * (d[-m-1] / 10 + d[-m-2] / 10^2 + ...) * 2^e\n        //         = buffer * 10^-m + 10^-m * (p2                                 ) * 2^e\n        //         = buffer * 10^-m + 10^-m * (1/10 * (10 * p2)                   ) * 2^e\n        //         = buffer * 10^-m + 10^-m * (1/10 * ((10*p2 div 2^-e) * 2^-e + (10*p2 mod 2^-e)) * 2^e\n        //\n        JSON_ASSERT(p2 <= (std::numeric_limits<std::uint64_t>::max)() / 10);\n        p2 *= 10;\n        const std::uint64_t d = p2 >> -one.e;     // d = (10 * p2) div 2^-e\n        const std::uint64_t r = p2 & (one.f - 1); // r = (10 * p2) mod 2^-e\n        //\n        //      M+ = buffer * 10^-m + 10^-m * (1/10 * (d * 2^-e + r) * 2^e\n        //         = buffer * 10^-m + 10^-m * (1/10 * (d + r * 2^e))\n        //         = (buffer * 10 + d) * 10^(-m-1) + 10^(-m-1) * r * 2^e\n        //\n        JSON_ASSERT(d <= 9);\n        buffer[length++] = static_cast<char>('0' + d); // buffer := buffer * 10 + d\n        //\n        //      M+ = buffer * 10^(-m-1) + 10^(-m-1) * r * 2^e\n        //\n        p2 = r;\n        m++;\n        //\n        //      M+ = buffer * 10^-m + 10^-m * p2 * 2^e\n        // Invariant restored.\n\n        // Check if enough digits have been generated.\n        //\n        //      10^-m * p2 * 2^e <= delta * 2^e\n        //              p2 * 2^e <= 10^m * delta * 2^e\n        //                    p2 <= 10^m * delta\n        delta *= 10;\n        dist  *= 10;\n        if (p2 <= delta)\n        {\n            break;\n        }\n    }\n\n    // V = buffer * 10^-m, with M- <= V <= M+.\n\n    decimal_exponent -= m;\n\n    // 1 ulp in the decimal representation is now 10^-m.\n    // Since delta and dist are now scaled by 10^m, we need to do the\n    // same with ulp in order to keep the units in sync.\n    //\n    //      10^m * 10^-m = 1 = 2^-e * 2^e = ten_m * 2^e\n    //\n    const std::uint64_t ten_m = one.f;\n    grisu2_round(buffer, length, dist, delta, p2, ten_m);\n\n    // By construction this algorithm generates the shortest possible decimal\n    // number (Loitsch, Theorem 6.2) which rounds back to w.\n    // For an input number of precision p, at least\n    //\n    //      N = 1 + ceil(p * log_10(2))\n    //\n    // decimal digits are sufficient to identify all binary floating-point\n    // numbers (Matula, \"In-and-Out conversions\").\n    // This implies that the algorithm does not produce more than N decimal\n    // digits.\n    //\n    //      N = 17 for p = 53 (IEEE double precision)\n    //      N = 9  for p = 24 (IEEE single precision)\n}\n\n/*!\nv = buf * 10^decimal_exponent\nlen is the length of the buffer (number of decimal digits)\nThe buffer must be large enough, i.e. >= max_digits10.\n*/\nJSON_HEDLEY_NON_NULL(1)\ninline void grisu2(char* buf, int& len, int& decimal_exponent,\n                   diyfp m_minus, diyfp v, diyfp m_plus)\n{\n    JSON_ASSERT(m_plus.e == m_minus.e);\n    JSON_ASSERT(m_plus.e == v.e);\n\n    //  --------(-----------------------+-----------------------)--------    (A)\n    //          m-                      v                       m+\n    //\n    //  --------------------(-----------+-----------------------)--------    (B)\n    //                      m-          v                       m+\n    //\n    // First scale v (and m- and m+) such that the exponent is in the range\n    // [alpha, gamma].\n\n    const cached_power cached = get_cached_power_for_binary_exponent(m_plus.e);\n\n    const diyfp c_minus_k(cached.f, cached.e); // = c ~= 10^-k\n\n    // The exponent of the products is = v.e + c_minus_k.e + q and is in the range [alpha,gamma]\n    const diyfp w       = diyfp::mul(v,       c_minus_k);\n    const diyfp w_minus = diyfp::mul(m_minus, c_minus_k);\n    const diyfp w_plus  = diyfp::mul(m_plus,  c_minus_k);\n\n    //  ----(---+---)---------------(---+---)---------------(---+---)----\n    //          w-                      w                       w+\n    //          = c*m-                  = c*v                   = c*m+\n    //\n    // diyfp::mul rounds its result and c_minus_k is approximated too. w, w- and\n    // w+ are now off by a small amount.\n    // In fact:\n    //\n    //      w - v * 10^k < 1 ulp\n    //\n    // To account for this inaccuracy, add resp. subtract 1 ulp.\n    //\n    //  --------+---[---------------(---+---)---------------]---+--------\n    //          w-  M-                  w                   M+  w+\n    //\n    // Now any number in [M-, M+] (bounds included) will round to w when input,\n    // regardless of how the input rounding algorithm breaks ties.\n    //\n    // And digit_gen generates the shortest possible such number in [M-, M+].\n    // Note that this does not mean that Grisu2 always generates the shortest\n    // possible number in the interval (m-, m+).\n    const diyfp M_minus(w_minus.f + 1, w_minus.e);\n    const diyfp M_plus (w_plus.f  - 1, w_plus.e );\n\n    decimal_exponent = -cached.k; // = -(-k) = k\n\n    grisu2_digit_gen(buf, len, decimal_exponent, M_minus, w, M_plus);\n}\n\n/*!\nv = buf * 10^decimal_exponent\nlen is the length of the buffer (number of decimal digits)\nThe buffer must be large enough, i.e. >= max_digits10.\n*/\ntemplate<typename FloatType>\nJSON_HEDLEY_NON_NULL(1)\nvoid grisu2(char* buf, int& len, int& decimal_exponent, FloatType value)\n{\n    static_assert(diyfp::kPrecision >= std::numeric_limits<FloatType>::digits + 3,\n                  \"internal error: not enough precision\");\n\n    JSON_ASSERT(std::isfinite(value));\n    JSON_ASSERT(value > 0);\n\n    // If the neighbors (and boundaries) of 'value' are always computed for double-precision\n    // numbers, all float's can be recovered using strtod (and strtof). However, the resulting\n    // decimal representations are not exactly \"short\".\n    //\n    // The documentation for 'std::to_chars' (https://en.cppreference.com/w/cpp/utility/to_chars)\n    // says \"value is converted to a string as if by std::sprintf in the default (\"C\") locale\"\n    // and since sprintf promotes floats to doubles, I think this is exactly what 'std::to_chars'\n    // does.\n    // On the other hand, the documentation for 'std::to_chars' requires that \"parsing the\n    // representation using the corresponding std::from_chars function recovers value exactly\". That\n    // indicates that single precision floating-point numbers should be recovered using\n    // 'std::strtof'.\n    //\n    // NB: If the neighbors are computed for single-precision numbers, there is a single float\n    //     (7.0385307e-26f) which can't be recovered using strtod. The resulting double precision\n    //     value is off by 1 ulp.\n#if 0\n    const boundaries w = compute_boundaries(static_cast<double>(value));\n#else\n    const boundaries w = compute_boundaries(value);\n#endif\n\n    grisu2(buf, len, decimal_exponent, w.minus, w.w, w.plus);\n}\n\n/*!\n@brief appends a decimal representation of e to buf\n@return a pointer to the element following the exponent.\n@pre -1000 < e < 1000\n*/\nJSON_HEDLEY_NON_NULL(1)\nJSON_HEDLEY_RETURNS_NON_NULL\ninline char* append_exponent(char* buf, int e)\n{\n    JSON_ASSERT(e > -1000);\n    JSON_ASSERT(e <  1000);\n\n    if (e < 0)\n    {\n        e = -e;\n        *buf++ = '-';\n    }\n    else\n    {\n        *buf++ = '+';\n    }\n\n    auto k = static_cast<std::uint32_t>(e);\n    if (k < 10)\n    {\n        // Always print at least two digits in the exponent.\n        // This is for compatibility with printf(\"%g\").\n        *buf++ = '0';\n        *buf++ = static_cast<char>('0' + k);\n    }\n    else if (k < 100)\n    {\n        *buf++ = static_cast<char>('0' + k / 10);\n        k %= 10;\n        *buf++ = static_cast<char>('0' + k);\n    }\n    else\n    {\n        *buf++ = static_cast<char>('0' + k / 100);\n        k %= 100;\n        *buf++ = static_cast<char>('0' + k / 10);\n        k %= 10;\n        *buf++ = static_cast<char>('0' + k);\n    }\n\n    return buf;\n}\n\n/*!\n@brief prettify v = buf * 10^decimal_exponent\n\nIf v is in the range [10^min_exp, 10^max_exp) it will be printed in fixed-point\nnotation. Otherwise it will be printed in exponential notation.\n\n@pre min_exp < 0\n@pre max_exp > 0\n*/\nJSON_HEDLEY_NON_NULL(1)\nJSON_HEDLEY_RETURNS_NON_NULL\ninline char* format_buffer(char* buf, int len, int decimal_exponent,\n                           int min_exp, int max_exp)\n{\n    JSON_ASSERT(min_exp < 0);\n    JSON_ASSERT(max_exp > 0);\n\n    const int k = len;\n    const int n = len + decimal_exponent;\n\n    // v = buf * 10^(n-k)\n    // k is the length of the buffer (number of decimal digits)\n    // n is the position of the decimal point relative to the start of the buffer.\n\n    if (k <= n && n <= max_exp)\n    {\n        // digits[000]\n        // len <= max_exp + 2\n\n        std::memset(buf + k, '0', static_cast<size_t>(n) - static_cast<size_t>(k));\n        // Make it look like a floating-point number (#362, #378)\n        buf[n + 0] = '.';\n        buf[n + 1] = '0';\n        return buf + (static_cast<size_t>(n) + 2);\n    }\n\n    if (0 < n && n <= max_exp)\n    {\n        // dig.its\n        // len <= max_digits10 + 1\n\n        JSON_ASSERT(k > n);\n\n        std::memmove(buf + (static_cast<size_t>(n) + 1), buf + n, static_cast<size_t>(k) - static_cast<size_t>(n));\n        buf[n] = '.';\n        return buf + (static_cast<size_t>(k) + 1U);\n    }\n\n    if (min_exp < n && n <= 0)\n    {\n        // 0.[000]digits\n        // len <= 2 + (-min_exp - 1) + max_digits10\n\n        std::memmove(buf + (2 + static_cast<size_t>(-n)), buf, static_cast<size_t>(k));\n        buf[0] = '0';\n        buf[1] = '.';\n        std::memset(buf + 2, '0', static_cast<size_t>(-n));\n        return buf + (2U + static_cast<size_t>(-n) + static_cast<size_t>(k));\n    }\n\n    if (k == 1)\n    {\n        // dE+123\n        // len <= 1 + 5\n\n        buf += 1;\n    }\n    else\n    {\n        // d.igitsE+123\n        // len <= max_digits10 + 1 + 5\n\n        std::memmove(buf + 2, buf + 1, static_cast<size_t>(k) - 1);\n        buf[1] = '.';\n        buf += 1 + static_cast<size_t>(k);\n    }\n\n    *buf++ = 'e';\n    return append_exponent(buf, n - 1);\n}\n\n}  // namespace dtoa_impl\n\n/*!\n@brief generates a decimal representation of the floating-point number value in [first, last).\n\nThe format of the resulting decimal representation is similar to printf's %g\nformat. Returns an iterator pointing past-the-end of the decimal representation.\n\n@note The input number must be finite, i.e. NaN's and Inf's are not supported.\n@note The buffer must be large enough.\n@note The result is NOT null-terminated.\n*/\ntemplate<typename FloatType>\nJSON_HEDLEY_NON_NULL(1, 2)\nJSON_HEDLEY_RETURNS_NON_NULL\nchar* to_chars(char* first, const char* last, FloatType value)\n{\n    static_cast<void>(last); // maybe unused - fix warning\n    JSON_ASSERT(std::isfinite(value));\n\n    // Use signbit(value) instead of (value < 0) since signbit works for -0.\n    if (std::signbit(value))\n    {\n        value = -value;\n        *first++ = '-';\n    }\n\n#ifdef __GNUC__\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wfloat-equal\"\n#endif\n    if (value == 0) // +-0\n    {\n        *first++ = '0';\n        // Make it look like a floating-point number (#362, #378)\n        *first++ = '.';\n        *first++ = '0';\n        return first;\n    }\n#ifdef __GNUC__\n#pragma GCC diagnostic pop\n#endif\n\n    JSON_ASSERT(last - first >= std::numeric_limits<FloatType>::max_digits10);\n\n    // Compute v = buffer * 10^decimal_exponent.\n    // The decimal digits are stored in the buffer, which needs to be interpreted\n    // as an unsigned decimal integer.\n    // len is the length of the buffer, i.e. the number of decimal digits.\n    int len = 0;\n    int decimal_exponent = 0;\n    dtoa_impl::grisu2(first, len, decimal_exponent, value);\n\n    JSON_ASSERT(len <= std::numeric_limits<FloatType>::max_digits10);\n\n    // Format the buffer like printf(\"%.*g\", prec, value)\n    constexpr int kMinExp = -4;\n    // Use digits10 here to increase compatibility with version 2.\n    constexpr int kMaxExp = std::numeric_limits<FloatType>::digits10;\n\n    JSON_ASSERT(last - first >= kMaxExp + 2);\n    JSON_ASSERT(last - first >= 2 + (-kMinExp - 1) + std::numeric_limits<FloatType>::max_digits10);\n    JSON_ASSERT(last - first >= std::numeric_limits<FloatType>::max_digits10 + 6);\n\n    return dtoa_impl::format_buffer(first, len, decimal_exponent, kMinExp, kMaxExp);\n}\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/exceptions.hpp>\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/meta/cpp_future.hpp>\n\n// #include <nlohmann/detail/output/binary_writer.hpp>\n\n// #include <nlohmann/detail/output/output_adapters.hpp>\n\n// #include <nlohmann/detail/string_concat.hpp>\n\n// #include <nlohmann/detail/value_t.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\nnamespace detail\n{\n\n///////////////////\n// serialization //\n///////////////////\n\n/// how to treat decoding errors\nenum class error_handler_t\n{\n    strict,  ///< throw a type_error exception in case of invalid UTF-8\n    replace, ///< replace invalid UTF-8 sequences with U+FFFD\n    ignore   ///< ignore invalid UTF-8 sequences\n};\n\ntemplate<typename BasicJsonType>\nclass serializer\n{\n    using string_t = typename BasicJsonType::string_t;\n    using number_float_t = typename BasicJsonType::number_float_t;\n    using number_integer_t = typename BasicJsonType::number_integer_t;\n    using number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n    using binary_char_t = typename BasicJsonType::binary_t::value_type;\n    static constexpr std::uint8_t UTF8_ACCEPT = 0;\n    static constexpr std::uint8_t UTF8_REJECT = 1;\n\n  public:\n    /*!\n    @param[in] s  output stream to serialize to\n    @param[in] ichar  indentation character to use\n    @param[in] error_handler_  how to react on decoding errors\n    */\n    serializer(output_adapter_t<char> s, const char ichar,\n               error_handler_t error_handler_ = error_handler_t::strict)\n        : o(std::move(s))\n        , loc(std::localeconv())\n        , thousands_sep(loc->thousands_sep == nullptr ? '\\0' : std::char_traits<char>::to_char_type(* (loc->thousands_sep)))\n        , decimal_point(loc->decimal_point == nullptr ? '\\0' : std::char_traits<char>::to_char_type(* (loc->decimal_point)))\n        , indent_char(ichar)\n        , indent_string(512, indent_char)\n        , error_handler(error_handler_)\n    {}\n\n    // delete because of pointer members\n    serializer(const serializer&) = delete;\n    serializer& operator=(const serializer&) = delete;\n    serializer(serializer&&) = delete;\n    serializer& operator=(serializer&&) = delete;\n    ~serializer() = default;\n\n    /*!\n    @brief internal implementation of the serialization function\n\n    This function is called by the public member function dump and organizes\n    the serialization internally. The indentation level is propagated as\n    additional parameter. In case of arrays and objects, the function is\n    called recursively.\n\n    - strings and object keys are escaped using `escape_string()`\n    - integer numbers are converted implicitly via `operator<<`\n    - floating-point numbers are converted to a string using `\"%g\"` format\n    - binary values are serialized as objects containing the subtype and the\n      byte array\n\n    @param[in] val               value to serialize\n    @param[in] pretty_print      whether the output shall be pretty-printed\n    @param[in] ensure_ascii If @a ensure_ascii is true, all non-ASCII characters\n    in the output are escaped with `\\uXXXX` sequences, and the result consists\n    of ASCII characters only.\n    @param[in] indent_step       the indent level\n    @param[in] current_indent    the current indent level (only used internally)\n    */\n    void dump(const BasicJsonType& val,\n              const bool pretty_print,\n              const bool ensure_ascii,\n              const unsigned int indent_step,\n              const unsigned int current_indent = 0)\n    {\n        switch (val.m_type)\n        {\n            case value_t::object:\n            {\n                if (val.m_value.object->empty())\n                {\n                    o->write_characters(\"{}\", 2);\n                    return;\n                }\n\n                if (pretty_print)\n                {\n                    o->write_characters(\"{\\n\", 2);\n\n                    // variable to hold indentation for recursive calls\n                    const auto new_indent = current_indent + indent_step;\n                    if (JSON_HEDLEY_UNLIKELY(indent_string.size() < new_indent))\n                    {\n                        indent_string.resize(indent_string.size() * 2, ' ');\n                    }\n\n                    // first n-1 elements\n                    auto i = val.m_value.object->cbegin();\n                    for (std::size_t cnt = 0; cnt < val.m_value.object->size() - 1; ++cnt, ++i)\n                    {\n                        o->write_characters(indent_string.c_str(), new_indent);\n                        o->write_character('\\\"');\n                        dump_escaped(i->first, ensure_ascii);\n                        o->write_characters(\"\\\": \", 3);\n                        dump(i->second, true, ensure_ascii, indent_step, new_indent);\n                        o->write_characters(\",\\n\", 2);\n                    }\n\n                    // last element\n                    JSON_ASSERT(i != val.m_value.object->cend());\n                    JSON_ASSERT(std::next(i) == val.m_value.object->cend());\n                    o->write_characters(indent_string.c_str(), new_indent);\n                    o->write_character('\\\"');\n                    dump_escaped(i->first, ensure_ascii);\n                    o->write_characters(\"\\\": \", 3);\n                    dump(i->second, true, ensure_ascii, indent_step, new_indent);\n\n                    o->write_character('\\n');\n                    o->write_characters(indent_string.c_str(), current_indent);\n                    o->write_character('}');\n                }\n                else\n                {\n                    o->write_character('{');\n\n                    // first n-1 elements\n                    auto i = val.m_value.object->cbegin();\n                    for (std::size_t cnt = 0; cnt < val.m_value.object->size() - 1; ++cnt, ++i)\n                    {\n                        o->write_character('\\\"');\n                        dump_escaped(i->first, ensure_ascii);\n                        o->write_characters(\"\\\":\", 2);\n                        dump(i->second, false, ensure_ascii, indent_step, current_indent);\n                        o->write_character(',');\n                    }\n\n                    // last element\n                    JSON_ASSERT(i != val.m_value.object->cend());\n                    JSON_ASSERT(std::next(i) == val.m_value.object->cend());\n                    o->write_character('\\\"');\n                    dump_escaped(i->first, ensure_ascii);\n                    o->write_characters(\"\\\":\", 2);\n                    dump(i->second, false, ensure_ascii, indent_step, current_indent);\n\n                    o->write_character('}');\n                }\n\n                return;\n            }\n\n            case value_t::array:\n            {\n                if (val.m_value.array->empty())\n                {\n                    o->write_characters(\"[]\", 2);\n                    return;\n                }\n\n                if (pretty_print)\n                {\n                    o->write_characters(\"[\\n\", 2);\n\n                    // variable to hold indentation for recursive calls\n                    const auto new_indent = current_indent + indent_step;\n                    if (JSON_HEDLEY_UNLIKELY(indent_string.size() < new_indent))\n                    {\n                        indent_string.resize(indent_string.size() * 2, ' ');\n                    }\n\n                    // first n-1 elements\n                    for (auto i = val.m_value.array->cbegin();\n                            i != val.m_value.array->cend() - 1; ++i)\n                    {\n                        o->write_characters(indent_string.c_str(), new_indent);\n                        dump(*i, true, ensure_ascii, indent_step, new_indent);\n                        o->write_characters(\",\\n\", 2);\n                    }\n\n                    // last element\n                    JSON_ASSERT(!val.m_value.array->empty());\n                    o->write_characters(indent_string.c_str(), new_indent);\n                    dump(val.m_value.array->back(), true, ensure_ascii, indent_step, new_indent);\n\n                    o->write_character('\\n');\n                    o->write_characters(indent_string.c_str(), current_indent);\n                    o->write_character(']');\n                }\n                else\n                {\n                    o->write_character('[');\n\n                    // first n-1 elements\n                    for (auto i = val.m_value.array->cbegin();\n                            i != val.m_value.array->cend() - 1; ++i)\n                    {\n                        dump(*i, false, ensure_ascii, indent_step, current_indent);\n                        o->write_character(',');\n                    }\n\n                    // last element\n                    JSON_ASSERT(!val.m_value.array->empty());\n                    dump(val.m_value.array->back(), false, ensure_ascii, indent_step, current_indent);\n\n                    o->write_character(']');\n                }\n\n                return;\n            }\n\n            case value_t::string:\n            {\n                o->write_character('\\\"');\n                dump_escaped(*val.m_value.string, ensure_ascii);\n                o->write_character('\\\"');\n                return;\n            }\n\n            case value_t::binary:\n            {\n                if (pretty_print)\n                {\n                    o->write_characters(\"{\\n\", 2);\n\n                    // variable to hold indentation for recursive calls\n                    const auto new_indent = current_indent + indent_step;\n                    if (JSON_HEDLEY_UNLIKELY(indent_string.size() < new_indent))\n                    {\n                        indent_string.resize(indent_string.size() * 2, ' ');\n                    }\n\n                    o->write_characters(indent_string.c_str(), new_indent);\n\n                    o->write_characters(\"\\\"bytes\\\": [\", 10);\n\n                    if (!val.m_value.binary->empty())\n                    {\n                        for (auto i = val.m_value.binary->cbegin();\n                                i != val.m_value.binary->cend() - 1; ++i)\n                        {\n                            dump_integer(*i);\n                            o->write_characters(\", \", 2);\n                        }\n                        dump_integer(val.m_value.binary->back());\n                    }\n\n                    o->write_characters(\"],\\n\", 3);\n                    o->write_characters(indent_string.c_str(), new_indent);\n\n                    o->write_characters(\"\\\"subtype\\\": \", 11);\n                    if (val.m_value.binary->has_subtype())\n                    {\n                        dump_integer(val.m_value.binary->subtype());\n                    }\n                    else\n                    {\n                        o->write_characters(\"null\", 4);\n                    }\n                    o->write_character('\\n');\n                    o->write_characters(indent_string.c_str(), current_indent);\n                    o->write_character('}');\n                }\n                else\n                {\n                    o->write_characters(\"{\\\"bytes\\\":[\", 10);\n\n                    if (!val.m_value.binary->empty())\n                    {\n                        for (auto i = val.m_value.binary->cbegin();\n                                i != val.m_value.binary->cend() - 1; ++i)\n                        {\n                            dump_integer(*i);\n                            o->write_character(',');\n                        }\n                        dump_integer(val.m_value.binary->back());\n                    }\n\n                    o->write_characters(\"],\\\"subtype\\\":\", 12);\n                    if (val.m_value.binary->has_subtype())\n                    {\n                        dump_integer(val.m_value.binary->subtype());\n                        o->write_character('}');\n                    }\n                    else\n                    {\n                        o->write_characters(\"null}\", 5);\n                    }\n                }\n                return;\n            }\n\n            case value_t::boolean:\n            {\n                if (val.m_value.boolean)\n                {\n                    o->write_characters(\"true\", 4);\n                }\n                else\n                {\n                    o->write_characters(\"false\", 5);\n                }\n                return;\n            }\n\n            case value_t::number_integer:\n            {\n                dump_integer(val.m_value.number_integer);\n                return;\n            }\n\n            case value_t::number_unsigned:\n            {\n                dump_integer(val.m_value.number_unsigned);\n                return;\n            }\n\n            case value_t::number_float:\n            {\n                dump_float(val.m_value.number_float);\n                return;\n            }\n\n            case value_t::discarded:\n            {\n                o->write_characters(\"<discarded>\", 11);\n                return;\n            }\n\n            case value_t::null:\n            {\n                o->write_characters(\"null\", 4);\n                return;\n            }\n\n            default:            // LCOV_EXCL_LINE\n                JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE\n        }\n    }\n\n  JSON_PRIVATE_UNLESS_TESTED:\n    /*!\n    @brief dump escaped string\n\n    Escape a string by replacing certain special characters by a sequence of an\n    escape character (backslash) and another character and other control\n    characters by a sequence of \"\\u\" followed by a four-digit hex\n    representation. The escaped string is written to output stream @a o.\n\n    @param[in] s  the string to escape\n    @param[in] ensure_ascii  whether to escape non-ASCII characters with\n                             \\uXXXX sequences\n\n    @complexity Linear in the length of string @a s.\n    */\n    void dump_escaped(const string_t& s, const bool ensure_ascii)\n    {\n        std::uint32_t codepoint{};\n        std::uint8_t state = UTF8_ACCEPT;\n        std::size_t bytes = 0;  // number of bytes written to string_buffer\n\n        // number of bytes written at the point of the last valid byte\n        std::size_t bytes_after_last_accept = 0;\n        std::size_t undumped_chars = 0;\n\n        for (std::size_t i = 0; i < s.size(); ++i)\n        {\n            const auto byte = static_cast<std::uint8_t>(s[i]);\n\n            switch (decode(state, codepoint, byte))\n            {\n                case UTF8_ACCEPT:  // decode found a new code point\n                {\n                    switch (codepoint)\n                    {\n                        case 0x08: // backspace\n                        {\n                            string_buffer[bytes++] = '\\\\';\n                            string_buffer[bytes++] = 'b';\n                            break;\n                        }\n\n                        case 0x09: // horizontal tab\n                        {\n                            string_buffer[bytes++] = '\\\\';\n                            string_buffer[bytes++] = 't';\n                            break;\n                        }\n\n                        case 0x0A: // newline\n                        {\n                            string_buffer[bytes++] = '\\\\';\n                            string_buffer[bytes++] = 'n';\n                            break;\n                        }\n\n                        case 0x0C: // formfeed\n                        {\n                            string_buffer[bytes++] = '\\\\';\n                            string_buffer[bytes++] = 'f';\n                            break;\n                        }\n\n                        case 0x0D: // carriage return\n                        {\n                            string_buffer[bytes++] = '\\\\';\n                            string_buffer[bytes++] = 'r';\n                            break;\n                        }\n\n                        case 0x22: // quotation mark\n                        {\n                            string_buffer[bytes++] = '\\\\';\n                            string_buffer[bytes++] = '\\\"';\n                            break;\n                        }\n\n                        case 0x5C: // reverse solidus\n                        {\n                            string_buffer[bytes++] = '\\\\';\n                            string_buffer[bytes++] = '\\\\';\n                            break;\n                        }\n\n                        default:\n                        {\n                            // escape control characters (0x00..0x1F) or, if\n                            // ensure_ascii parameter is used, non-ASCII characters\n                            if ((codepoint <= 0x1F) || (ensure_ascii && (codepoint >= 0x7F)))\n                            {\n                                if (codepoint <= 0xFFFF)\n                                {\n                                    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-vararg,hicpp-vararg)\n                                    static_cast<void>((std::snprintf)(string_buffer.data() + bytes, 7, \"\\\\u%04x\",\n                                                                      static_cast<std::uint16_t>(codepoint)));\n                                    bytes += 6;\n                                }\n                                else\n                                {\n                                    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-vararg,hicpp-vararg)\n                                    static_cast<void>((std::snprintf)(string_buffer.data() + bytes, 13, \"\\\\u%04x\\\\u%04x\",\n                                                                      static_cast<std::uint16_t>(0xD7C0u + (codepoint >> 10u)),\n                                                                      static_cast<std::uint16_t>(0xDC00u + (codepoint & 0x3FFu))));\n                                    bytes += 12;\n                                }\n                            }\n                            else\n                            {\n                                // copy byte to buffer (all previous bytes\n                                // been copied have in default case above)\n                                string_buffer[bytes++] = s[i];\n                            }\n                            break;\n                        }\n                    }\n\n                    // write buffer and reset index; there must be 13 bytes\n                    // left, as this is the maximal number of bytes to be\n                    // written (\"\\uxxxx\\uxxxx\\0\") for one code point\n                    if (string_buffer.size() - bytes < 13)\n                    {\n                        o->write_characters(string_buffer.data(), bytes);\n                        bytes = 0;\n                    }\n\n                    // remember the byte position of this accept\n                    bytes_after_last_accept = bytes;\n                    undumped_chars = 0;\n                    break;\n                }\n\n                case UTF8_REJECT:  // decode found invalid UTF-8 byte\n                {\n                    switch (error_handler)\n                    {\n                        case error_handler_t::strict:\n                        {\n                            JSON_THROW(type_error::create(316, concat(\"invalid UTF-8 byte at index \", std::to_string(i), \": 0x\", hex_bytes(byte | 0)), nullptr));\n                        }\n\n                        case error_handler_t::ignore:\n                        case error_handler_t::replace:\n                        {\n                            // in case we saw this character the first time, we\n                            // would like to read it again, because the byte\n                            // may be OK for itself, but just not OK for the\n                            // previous sequence\n                            if (undumped_chars > 0)\n                            {\n                                --i;\n                            }\n\n                            // reset length buffer to the last accepted index;\n                            // thus removing/ignoring the invalid characters\n                            bytes = bytes_after_last_accept;\n\n                            if (error_handler == error_handler_t::replace)\n                            {\n                                // add a replacement character\n                                if (ensure_ascii)\n                                {\n                                    string_buffer[bytes++] = '\\\\';\n                                    string_buffer[bytes++] = 'u';\n                                    string_buffer[bytes++] = 'f';\n                                    string_buffer[bytes++] = 'f';\n                                    string_buffer[bytes++] = 'f';\n                                    string_buffer[bytes++] = 'd';\n                                }\n                                else\n                                {\n                                    string_buffer[bytes++] = detail::binary_writer<BasicJsonType, char>::to_char_type('\\xEF');\n                                    string_buffer[bytes++] = detail::binary_writer<BasicJsonType, char>::to_char_type('\\xBF');\n                                    string_buffer[bytes++] = detail::binary_writer<BasicJsonType, char>::to_char_type('\\xBD');\n                                }\n\n                                // write buffer and reset index; there must be 13 bytes\n                                // left, as this is the maximal number of bytes to be\n                                // written (\"\\uxxxx\\uxxxx\\0\") for one code point\n                                if (string_buffer.size() - bytes < 13)\n                                {\n                                    o->write_characters(string_buffer.data(), bytes);\n                                    bytes = 0;\n                                }\n\n                                bytes_after_last_accept = bytes;\n                            }\n\n                            undumped_chars = 0;\n\n                            // continue processing the string\n                            state = UTF8_ACCEPT;\n                            break;\n                        }\n\n                        default:            // LCOV_EXCL_LINE\n                            JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE\n                    }\n                    break;\n                }\n\n                default:  // decode found yet incomplete multi-byte code point\n                {\n                    if (!ensure_ascii)\n                    {\n                        // code point will not be escaped - copy byte to buffer\n                        string_buffer[bytes++] = s[i];\n                    }\n                    ++undumped_chars;\n                    break;\n                }\n            }\n        }\n\n        // we finished processing the string\n        if (JSON_HEDLEY_LIKELY(state == UTF8_ACCEPT))\n        {\n            // write buffer\n            if (bytes > 0)\n            {\n                o->write_characters(string_buffer.data(), bytes);\n            }\n        }\n        else\n        {\n            // we finish reading, but do not accept: string was incomplete\n            switch (error_handler)\n            {\n                case error_handler_t::strict:\n                {\n                    JSON_THROW(type_error::create(316, concat(\"incomplete UTF-8 string; last byte: 0x\", hex_bytes(static_cast<std::uint8_t>(s.back() | 0))), nullptr));\n                }\n\n                case error_handler_t::ignore:\n                {\n                    // write all accepted bytes\n                    o->write_characters(string_buffer.data(), bytes_after_last_accept);\n                    break;\n                }\n\n                case error_handler_t::replace:\n                {\n                    // write all accepted bytes\n                    o->write_characters(string_buffer.data(), bytes_after_last_accept);\n                    // add a replacement character\n                    if (ensure_ascii)\n                    {\n                        o->write_characters(\"\\\\ufffd\", 6);\n                    }\n                    else\n                    {\n                        o->write_characters(\"\\xEF\\xBF\\xBD\", 3);\n                    }\n                    break;\n                }\n\n                default:            // LCOV_EXCL_LINE\n                    JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE\n            }\n        }\n    }\n\n  private:\n    /*!\n    @brief count digits\n\n    Count the number of decimal (base 10) digits for an input unsigned integer.\n\n    @param[in] x  unsigned integer number to count its digits\n    @return    number of decimal digits\n    */\n    inline unsigned int count_digits(number_unsigned_t x) noexcept\n    {\n        unsigned int n_digits = 1;\n        for (;;)\n        {\n            if (x < 10)\n            {\n                return n_digits;\n            }\n            if (x < 100)\n            {\n                return n_digits + 1;\n            }\n            if (x < 1000)\n            {\n                return n_digits + 2;\n            }\n            if (x < 10000)\n            {\n                return n_digits + 3;\n            }\n            x = x / 10000u;\n            n_digits += 4;\n        }\n    }\n\n    /*!\n     * @brief convert a byte to a uppercase hex representation\n     * @param[in] byte byte to represent\n     * @return representation (\"00\"..\"FF\")\n     */\n    static std::string hex_bytes(std::uint8_t byte)\n    {\n        std::string result = \"FF\";\n        constexpr const char* nibble_to_hex = \"0123456789ABCDEF\";\n        result[0] = nibble_to_hex[byte / 16];\n        result[1] = nibble_to_hex[byte % 16];\n        return result;\n    }\n\n    // templates to avoid warnings about useless casts\n    template <typename NumberType, enable_if_t<std::is_signed<NumberType>::value, int> = 0>\n    bool is_negative_number(NumberType x)\n    {\n        return x < 0;\n    }\n\n    template < typename NumberType, enable_if_t <std::is_unsigned<NumberType>::value, int > = 0 >\n    bool is_negative_number(NumberType /*unused*/)\n    {\n        return false;\n    }\n\n    /*!\n    @brief dump an integer\n\n    Dump a given integer to output stream @a o. Works internally with\n    @a number_buffer.\n\n    @param[in] x  integer number (signed or unsigned) to dump\n    @tparam NumberType either @a number_integer_t or @a number_unsigned_t\n    */\n    template < typename NumberType, detail::enable_if_t <\n                   std::is_integral<NumberType>::value ||\n                   std::is_same<NumberType, number_unsigned_t>::value ||\n                   std::is_same<NumberType, number_integer_t>::value ||\n                   std::is_same<NumberType, binary_char_t>::value,\n                   int > = 0 >\n    void dump_integer(NumberType x)\n    {\n        static constexpr std::array<std::array<char, 2>, 100> digits_to_99\n        {\n            {\n                {{'0', '0'}}, {{'0', '1'}}, {{'0', '2'}}, {{'0', '3'}}, {{'0', '4'}}, {{'0', '5'}}, {{'0', '6'}}, {{'0', '7'}}, {{'0', '8'}}, {{'0', '9'}},\n                {{'1', '0'}}, {{'1', '1'}}, {{'1', '2'}}, {{'1', '3'}}, {{'1', '4'}}, {{'1', '5'}}, {{'1', '6'}}, {{'1', '7'}}, {{'1', '8'}}, {{'1', '9'}},\n                {{'2', '0'}}, {{'2', '1'}}, {{'2', '2'}}, {{'2', '3'}}, {{'2', '4'}}, {{'2', '5'}}, {{'2', '6'}}, {{'2', '7'}}, {{'2', '8'}}, {{'2', '9'}},\n                {{'3', '0'}}, {{'3', '1'}}, {{'3', '2'}}, {{'3', '3'}}, {{'3', '4'}}, {{'3', '5'}}, {{'3', '6'}}, {{'3', '7'}}, {{'3', '8'}}, {{'3', '9'}},\n                {{'4', '0'}}, {{'4', '1'}}, {{'4', '2'}}, {{'4', '3'}}, {{'4', '4'}}, {{'4', '5'}}, {{'4', '6'}}, {{'4', '7'}}, {{'4', '8'}}, {{'4', '9'}},\n                {{'5', '0'}}, {{'5', '1'}}, {{'5', '2'}}, {{'5', '3'}}, {{'5', '4'}}, {{'5', '5'}}, {{'5', '6'}}, {{'5', '7'}}, {{'5', '8'}}, {{'5', '9'}},\n                {{'6', '0'}}, {{'6', '1'}}, {{'6', '2'}}, {{'6', '3'}}, {{'6', '4'}}, {{'6', '5'}}, {{'6', '6'}}, {{'6', '7'}}, {{'6', '8'}}, {{'6', '9'}},\n                {{'7', '0'}}, {{'7', '1'}}, {{'7', '2'}}, {{'7', '3'}}, {{'7', '4'}}, {{'7', '5'}}, {{'7', '6'}}, {{'7', '7'}}, {{'7', '8'}}, {{'7', '9'}},\n                {{'8', '0'}}, {{'8', '1'}}, {{'8', '2'}}, {{'8', '3'}}, {{'8', '4'}}, {{'8', '5'}}, {{'8', '6'}}, {{'8', '7'}}, {{'8', '8'}}, {{'8', '9'}},\n                {{'9', '0'}}, {{'9', '1'}}, {{'9', '2'}}, {{'9', '3'}}, {{'9', '4'}}, {{'9', '5'}}, {{'9', '6'}}, {{'9', '7'}}, {{'9', '8'}}, {{'9', '9'}},\n            }\n        };\n\n        // special case for \"0\"\n        if (x == 0)\n        {\n            o->write_character('0');\n            return;\n        }\n\n        // use a pointer to fill the buffer\n        auto buffer_ptr = number_buffer.begin(); // NOLINT(llvm-qualified-auto,readability-qualified-auto,cppcoreguidelines-pro-type-vararg,hicpp-vararg)\n\n        number_unsigned_t abs_value;\n\n        unsigned int n_chars{};\n\n        if (is_negative_number(x))\n        {\n            *buffer_ptr = '-';\n            abs_value = remove_sign(static_cast<number_integer_t>(x));\n\n            // account one more byte for the minus sign\n            n_chars = 1 + count_digits(abs_value);\n        }\n        else\n        {\n            abs_value = static_cast<number_unsigned_t>(x);\n            n_chars = count_digits(abs_value);\n        }\n\n        // spare 1 byte for '\\0'\n        JSON_ASSERT(n_chars < number_buffer.size() - 1);\n\n        // jump to the end to generate the string from backward,\n        // so we later avoid reversing the result\n        buffer_ptr += n_chars;\n\n        // Fast int2ascii implementation inspired by \"Fastware\" talk by Andrei Alexandrescu\n        // See: https://www.youtube.com/watch?v=o4-CwDo2zpg\n        while (abs_value >= 100)\n        {\n            const auto digits_index = static_cast<unsigned>((abs_value % 100));\n            abs_value /= 100;\n            *(--buffer_ptr) = digits_to_99[digits_index][1];\n            *(--buffer_ptr) = digits_to_99[digits_index][0];\n        }\n\n        if (abs_value >= 10)\n        {\n            const auto digits_index = static_cast<unsigned>(abs_value);\n            *(--buffer_ptr) = digits_to_99[digits_index][1];\n            *(--buffer_ptr) = digits_to_99[digits_index][0];\n        }\n        else\n        {\n            *(--buffer_ptr) = static_cast<char>('0' + abs_value);\n        }\n\n        o->write_characters(number_buffer.data(), n_chars);\n    }\n\n    /*!\n    @brief dump a floating-point number\n\n    Dump a given floating-point number to output stream @a o. Works internally\n    with @a number_buffer.\n\n    @param[in] x  floating-point number to dump\n    */\n    void dump_float(number_float_t x)\n    {\n        // NaN / inf\n        if (!std::isfinite(x))\n        {\n            o->write_characters(\"null\", 4);\n            return;\n        }\n\n        // If number_float_t is an IEEE-754 single or double precision number,\n        // use the Grisu2 algorithm to produce short numbers which are\n        // guaranteed to round-trip, using strtof and strtod, resp.\n        //\n        // NB: The test below works if <long double> == <double>.\n        static constexpr bool is_ieee_single_or_double\n            = (std::numeric_limits<number_float_t>::is_iec559 && std::numeric_limits<number_float_t>::digits == 24 && std::numeric_limits<number_float_t>::max_exponent == 128) ||\n              (std::numeric_limits<number_float_t>::is_iec559 && std::numeric_limits<number_float_t>::digits == 53 && std::numeric_limits<number_float_t>::max_exponent == 1024);\n\n        dump_float(x, std::integral_constant<bool, is_ieee_single_or_double>());\n    }\n\n    void dump_float(number_float_t x, std::true_type /*is_ieee_single_or_double*/)\n    {\n        auto* begin = number_buffer.data();\n        auto* end = ::nlohmann::detail::to_chars(begin, begin + number_buffer.size(), x);\n\n        o->write_characters(begin, static_cast<size_t>(end - begin));\n    }\n\n    void dump_float(number_float_t x, std::false_type /*is_ieee_single_or_double*/)\n    {\n        // get number of digits for a float -> text -> float round-trip\n        static constexpr auto d = std::numeric_limits<number_float_t>::max_digits10;\n\n        // the actual conversion\n        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-vararg,hicpp-vararg)\n        std::ptrdiff_t len = (std::snprintf)(number_buffer.data(), number_buffer.size(), \"%.*g\", d, x);\n\n        // negative value indicates an error\n        JSON_ASSERT(len > 0);\n        // check if buffer was large enough\n        JSON_ASSERT(static_cast<std::size_t>(len) < number_buffer.size());\n\n        // erase thousands separator\n        if (thousands_sep != '\\0')\n        {\n            // NOLINTNEXTLINE(readability-qualified-auto,llvm-qualified-auto): std::remove returns an iterator, see https://github.com/nlohmann/json/issues/3081\n            const auto end = std::remove(number_buffer.begin(), number_buffer.begin() + len, thousands_sep);\n            std::fill(end, number_buffer.end(), '\\0');\n            JSON_ASSERT((end - number_buffer.begin()) <= len);\n            len = (end - number_buffer.begin());\n        }\n\n        // convert decimal point to '.'\n        if (decimal_point != '\\0' && decimal_point != '.')\n        {\n            // NOLINTNEXTLINE(readability-qualified-auto,llvm-qualified-auto): std::find returns an iterator, see https://github.com/nlohmann/json/issues/3081\n            const auto dec_pos = std::find(number_buffer.begin(), number_buffer.end(), decimal_point);\n            if (dec_pos != number_buffer.end())\n            {\n                *dec_pos = '.';\n            }\n        }\n\n        o->write_characters(number_buffer.data(), static_cast<std::size_t>(len));\n\n        // determine if we need to append \".0\"\n        const bool value_is_int_like =\n            std::none_of(number_buffer.begin(), number_buffer.begin() + len + 1,\n                         [](char c)\n        {\n            return c == '.' || c == 'e';\n        });\n\n        if (value_is_int_like)\n        {\n            o->write_characters(\".0\", 2);\n        }\n    }\n\n    /*!\n    @brief check whether a string is UTF-8 encoded\n\n    The function checks each byte of a string whether it is UTF-8 encoded. The\n    result of the check is stored in the @a state parameter. The function must\n    be called initially with state 0 (accept). State 1 means the string must\n    be rejected, because the current byte is not allowed. If the string is\n    completely processed, but the state is non-zero, the string ended\n    prematurely; that is, the last byte indicated more bytes should have\n    followed.\n\n    @param[in,out] state  the state of the decoding\n    @param[in,out] codep  codepoint (valid only if resulting state is UTF8_ACCEPT)\n    @param[in] byte       next byte to decode\n    @return               new state\n\n    @note The function has been edited: a std::array is used.\n\n    @copyright Copyright (c) 2008-2009 Bjoern Hoehrmann <bjoern@hoehrmann.de>\n    @sa http://bjoern.hoehrmann.de/utf-8/decoder/dfa/\n    */\n    static std::uint8_t decode(std::uint8_t& state, std::uint32_t& codep, const std::uint8_t byte) noexcept\n    {\n        static const std::array<std::uint8_t, 400> utf8d =\n        {\n            {\n                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 00..1F\n                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 20..3F\n                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 40..5F\n                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 60..7F\n                1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, // 80..9F\n                7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, // A0..BF\n                8, 8, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // C0..DF\n                0xA, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x4, 0x3, 0x3, // E0..EF\n                0xB, 0x6, 0x6, 0x6, 0x5, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, // F0..FF\n                0x0, 0x1, 0x2, 0x3, 0x5, 0x8, 0x7, 0x1, 0x1, 0x1, 0x4, 0x6, 0x1, 0x1, 0x1, 0x1, // s0..s0\n                1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, // s1..s2\n                1, 2, 1, 1, 1, 1, 1, 2, 1, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 1, 1, 1, 1, 1, 1, 1, 1, // s3..s4\n                1, 2, 1, 1, 1, 1, 1, 1, 1, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3, 1, 3, 1, 1, 1, 1, 1, 1, // s5..s6\n                1, 3, 1, 1, 1, 1, 1, 3, 1, 3, 1, 1, 1, 1, 1, 1, 1, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // s7..s8\n            }\n        };\n\n        JSON_ASSERT(byte < utf8d.size());\n        const std::uint8_t type = utf8d[byte];\n\n        codep = (state != UTF8_ACCEPT)\n                ? (byte & 0x3fu) | (codep << 6u)\n                : (0xFFu >> type) & (byte);\n\n        std::size_t index = 256u + static_cast<size_t>(state) * 16u + static_cast<size_t>(type);\n        JSON_ASSERT(index < 400);\n        state = utf8d[index];\n        return state;\n    }\n\n    /*\n     * Overload to make the compiler happy while it is instantiating\n     * dump_integer for number_unsigned_t.\n     * Must never be called.\n     */\n    number_unsigned_t remove_sign(number_unsigned_t x)\n    {\n        JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE\n        return x; // LCOV_EXCL_LINE\n    }\n\n    /*\n     * Helper function for dump_integer\n     *\n     * This function takes a negative signed integer and returns its absolute\n     * value as unsigned integer. The plus/minus shuffling is necessary as we can\n     * not directly remove the sign of an arbitrary signed integer as the\n     * absolute values of INT_MIN and INT_MAX are usually not the same. See\n     * #1708 for details.\n     */\n    inline number_unsigned_t remove_sign(number_integer_t x) noexcept\n    {\n        JSON_ASSERT(x < 0 && x < (std::numeric_limits<number_integer_t>::max)()); // NOLINT(misc-redundant-expression)\n        return static_cast<number_unsigned_t>(-(x + 1)) + 1;\n    }\n\n  private:\n    /// the output of the serializer\n    output_adapter_t<char> o = nullptr;\n\n    /// a (hopefully) large enough character buffer\n    std::array<char, 64> number_buffer{{}};\n\n    /// the locale\n    const std::lconv* loc = nullptr;\n    /// the locale's thousand separator character\n    const char thousands_sep = '\\0';\n    /// the locale's decimal point character\n    const char decimal_point = '\\0';\n\n    /// string buffer\n    std::array<char, 512> string_buffer{{}};\n\n    /// the indentation character\n    const char indent_char;\n    /// the indentation string\n    string_t indent_string;\n\n    /// error_handler how to react on decoding errors\n    const error_handler_t error_handler;\n};\n\n}  // namespace detail\nNLOHMANN_JSON_NAMESPACE_END\n\n// #include <nlohmann/detail/value_t.hpp>\n\n// #include <nlohmann/json_fwd.hpp>\n\n// #include <nlohmann/ordered_map.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#include <functional> // equal_to, less\n#include <initializer_list> // initializer_list\n#include <iterator> // input_iterator_tag, iterator_traits\n#include <memory> // allocator\n#include <stdexcept> // for out_of_range\n#include <type_traits> // enable_if, is_convertible\n#include <utility> // pair\n#include <vector> // vector\n\n// #include <nlohmann/detail/macro_scope.hpp>\n\n// #include <nlohmann/detail/meta/type_traits.hpp>\n\n\nNLOHMANN_JSON_NAMESPACE_BEGIN\n\n/// ordered_map: a minimal map-like container that preserves insertion order\n/// for use within nlohmann::basic_json<ordered_map>\ntemplate <class Key, class T, class IgnoredLess = std::less<Key>,\n          class Allocator = std::allocator<std::pair<const Key, T>>>\n                  struct ordered_map : std::vector<std::pair<const Key, T>, Allocator>\n{\n    using key_type = Key;\n    using mapped_type = T;\n    using Container = std::vector<std::pair<const Key, T>, Allocator>;\n    using iterator = typename Container::iterator;\n    using const_iterator = typename Container::const_iterator;\n    using size_type = typename Container::size_type;\n    using value_type = typename Container::value_type;\n#ifdef JSON_HAS_CPP_14\n    using key_compare = std::equal_to<>;\n#else\n    using key_compare = std::equal_to<Key>;\n#endif\n\n    // Explicit constructors instead of `using Container::Container`\n    // otherwise older compilers choke on it (GCC <= 5.5, xcode <= 9.4)\n    ordered_map() noexcept(noexcept(Container())) : Container{} {}\n    explicit ordered_map(const Allocator& alloc) noexcept(noexcept(Container(alloc))) : Container{alloc} {}\n    template <class It>\n    ordered_map(It first, It last, const Allocator& alloc = Allocator())\n        : Container{first, last, alloc} {}\n    ordered_map(std::initializer_list<value_type> init, const Allocator& alloc = Allocator() )\n        : Container{init, alloc} {}\n\n    std::pair<iterator, bool> emplace(const key_type& key, T&& t)\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                return {it, false};\n            }\n        }\n        Container::emplace_back(key, std::forward<T>(t));\n        return {std::prev(this->end()), true};\n    }\n\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_key_type<key_compare, key_type, KeyType>::value, int> = 0>\n    std::pair<iterator, bool> emplace(KeyType && key, T && t)\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                return {it, false};\n            }\n        }\n        Container::emplace_back(std::forward<KeyType>(key), std::forward<T>(t));\n        return {std::prev(this->end()), true};\n    }\n\n    T& operator[](const key_type& key)\n    {\n        return emplace(key, T{}).first->second;\n    }\n\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_key_type<key_compare, key_type, KeyType>::value, int> = 0>\n    T & operator[](KeyType && key)\n    {\n        return emplace(std::forward<KeyType>(key), T{}).first->second;\n    }\n\n    const T& operator[](const key_type& key) const\n    {\n        return at(key);\n    }\n\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_key_type<key_compare, key_type, KeyType>::value, int> = 0>\n    const T & operator[](KeyType && key) const\n    {\n        return at(std::forward<KeyType>(key));\n    }\n\n    T& at(const key_type& key)\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                return it->second;\n            }\n        }\n\n        JSON_THROW(std::out_of_range(\"key not found\"));\n    }\n\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_key_type<key_compare, key_type, KeyType>::value, int> = 0>\n    T & at(KeyType && key)\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                return it->second;\n            }\n        }\n\n        JSON_THROW(std::out_of_range(\"key not found\"));\n    }\n\n    const T& at(const key_type& key) const\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                return it->second;\n            }\n        }\n\n        JSON_THROW(std::out_of_range(\"key not found\"));\n    }\n\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_key_type<key_compare, key_type, KeyType>::value, int> = 0>\n    const T & at(KeyType && key) const\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                return it->second;\n            }\n        }\n\n        JSON_THROW(std::out_of_range(\"key not found\"));\n    }\n\n    size_type erase(const key_type& key)\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                // Since we cannot move const Keys, re-construct them in place\n                for (auto next = it; ++next != this->end(); ++it)\n                {\n                    it->~value_type(); // Destroy but keep allocation\n                    new (&*it) value_type{std::move(*next)};\n                }\n                Container::pop_back();\n                return 1;\n            }\n        }\n        return 0;\n    }\n\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_key_type<key_compare, key_type, KeyType>::value, int> = 0>\n    size_type erase(KeyType && key)\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                // Since we cannot move const Keys, re-construct them in place\n                for (auto next = it; ++next != this->end(); ++it)\n                {\n                    it->~value_type(); // Destroy but keep allocation\n                    new (&*it) value_type{std::move(*next)};\n                }\n                Container::pop_back();\n                return 1;\n            }\n        }\n        return 0;\n    }\n\n    iterator erase(iterator pos)\n    {\n        return erase(pos, std::next(pos));\n    }\n\n    iterator erase(iterator first, iterator last)\n    {\n        if (first == last)\n        {\n            return first;\n        }\n\n        const auto elements_affected = std::distance(first, last);\n        const auto offset = std::distance(Container::begin(), first);\n\n        // This is the start situation. We need to delete elements_affected\n        // elements (3 in this example: e, f, g), and need to return an\n        // iterator past the last deleted element (h in this example).\n        // Note that offset is the distance from the start of the vector\n        // to first. We will need this later.\n\n        // [ a, b, c, d, e, f, g, h, i, j ]\n        //               ^        ^\n        //             first    last\n\n        // Since we cannot move const Keys, we re-construct them in place.\n        // We start at first and re-construct (viz. copy) the elements from\n        // the back of the vector. Example for first iteration:\n\n        //               ,--------.\n        //               v        |   destroy e and re-construct with h\n        // [ a, b, c, d, e, f, g, h, i, j ]\n        //               ^        ^\n        //               it       it + elements_affected\n\n        for (auto it = first; std::next(it, elements_affected) != Container::end(); ++it)\n        {\n            it->~value_type(); // destroy but keep allocation\n            new (&*it) value_type{std::move(*std::next(it, elements_affected))}; // \"move\" next element to it\n        }\n\n        // [ a, b, c, d, h, i, j, h, i, j ]\n        //               ^        ^\n        //             first    last\n\n        // remove the unneeded elements at the end of the vector\n        Container::resize(this->size() - static_cast<size_type>(elements_affected));\n\n        // [ a, b, c, d, h, i, j ]\n        //               ^        ^\n        //             first    last\n\n        // first is now pointing past the last deleted element, but we cannot\n        // use this iterator, because it may have been invalidated by the\n        // resize call. Instead, we can return begin() + offset.\n        return Container::begin() + offset;\n    }\n\n    size_type count(const key_type& key) const\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                return 1;\n            }\n        }\n        return 0;\n    }\n\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_key_type<key_compare, key_type, KeyType>::value, int> = 0>\n    size_type count(KeyType && key) const\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                return 1;\n            }\n        }\n        return 0;\n    }\n\n    iterator find(const key_type& key)\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                return it;\n            }\n        }\n        return Container::end();\n    }\n\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_key_type<key_compare, key_type, KeyType>::value, int> = 0>\n    iterator find(KeyType && key)\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                return it;\n            }\n        }\n        return Container::end();\n    }\n\n    const_iterator find(const key_type& key) const\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, key))\n            {\n                return it;\n            }\n        }\n        return Container::end();\n    }\n\n    std::pair<iterator, bool> insert( value_type&& value )\n    {\n        return emplace(value.first, std::move(value.second));\n    }\n\n    std::pair<iterator, bool> insert( const value_type& value )\n    {\n        for (auto it = this->begin(); it != this->end(); ++it)\n        {\n            if (m_compare(it->first, value.first))\n            {\n                return {it, false};\n            }\n        }\n        Container::push_back(value);\n        return {--this->end(), true};\n    }\n\n    template<typename InputIt>\n    using require_input_iter = typename std::enable_if<std::is_convertible<typename std::iterator_traits<InputIt>::iterator_category,\n            std::input_iterator_tag>::value>::type;\n\n    template<typename InputIt, typename = require_input_iter<InputIt>>\n    void insert(InputIt first, InputIt last)\n    {\n        for (auto it = first; it != last; ++it)\n        {\n            insert(*it);\n        }\n    }\n\nprivate:\n    JSON_NO_UNIQUE_ADDRESS key_compare m_compare = key_compare();\n};\n\nNLOHMANN_JSON_NAMESPACE_END\n\n\n#if defined(JSON_HAS_CPP_17)\n    #include <any>\n    #include <string_view>\n#endif\n\n/*!\n@brief namespace for Niels Lohmann\n@see https://github.com/nlohmann\n@since version 1.0.0\n*/\nNLOHMANN_JSON_NAMESPACE_BEGIN\n\n/*!\n@brief a class to store JSON values\n\n@internal\n@invariant The member variables @a m_value and @a m_type have the following\nrelationship:\n- If `m_type == value_t::object`, then `m_value.object != nullptr`.\n- If `m_type == value_t::array`, then `m_value.array != nullptr`.\n- If `m_type == value_t::string`, then `m_value.string != nullptr`.\nThe invariants are checked by member function assert_invariant().\n\n@note ObjectType trick from https://stackoverflow.com/a/9860911\n@endinternal\n\n@since version 1.0.0\n\n@nosubgrouping\n*/\nNLOHMANN_BASIC_JSON_TPL_DECLARATION\nclass basic_json // NOLINT(cppcoreguidelines-special-member-functions,hicpp-special-member-functions)\n{\n  private:\n    template<detail::value_t> friend struct detail::external_constructor;\n\n    template<typename>\n    friend class ::nlohmann::json_pointer;\n    // can be restored when json_pointer backwards compatibility is removed\n    // friend ::nlohmann::json_pointer<StringType>;\n\n    template<typename BasicJsonType, typename InputType>\n    friend class ::nlohmann::detail::parser;\n    friend ::nlohmann::detail::serializer<basic_json>;\n    template<typename BasicJsonType>\n    friend class ::nlohmann::detail::iter_impl;\n    template<typename BasicJsonType, typename CharType>\n    friend class ::nlohmann::detail::binary_writer;\n    template<typename BasicJsonType, typename InputType, typename SAX>\n    friend class ::nlohmann::detail::binary_reader;\n    template<typename BasicJsonType>\n    friend class ::nlohmann::detail::json_sax_dom_parser;\n    template<typename BasicJsonType>\n    friend class ::nlohmann::detail::json_sax_dom_callback_parser;\n    friend class ::nlohmann::detail::exception;\n\n    /// workaround type for MSVC\n    using basic_json_t = NLOHMANN_BASIC_JSON_TPL;\n\n  JSON_PRIVATE_UNLESS_TESTED:\n    // convenience aliases for types residing in namespace detail;\n    using lexer = ::nlohmann::detail::lexer_base<basic_json>;\n\n    template<typename InputAdapterType>\n    static ::nlohmann::detail::parser<basic_json, InputAdapterType> parser(\n        InputAdapterType adapter,\n        detail::parser_callback_t<basic_json>cb = nullptr,\n        const bool allow_exceptions = true,\n        const bool ignore_comments = false\n                                 )\n    {\n        return ::nlohmann::detail::parser<basic_json, InputAdapterType>(std::move(adapter),\n                std::move(cb), allow_exceptions, ignore_comments);\n    }\n\n  private:\n    using primitive_iterator_t = ::nlohmann::detail::primitive_iterator_t;\n    template<typename BasicJsonType>\n    using internal_iterator = ::nlohmann::detail::internal_iterator<BasicJsonType>;\n    template<typename BasicJsonType>\n    using iter_impl = ::nlohmann::detail::iter_impl<BasicJsonType>;\n    template<typename Iterator>\n    using iteration_proxy = ::nlohmann::detail::iteration_proxy<Iterator>;\n    template<typename Base> using json_reverse_iterator = ::nlohmann::detail::json_reverse_iterator<Base>;\n\n    template<typename CharType>\n    using output_adapter_t = ::nlohmann::detail::output_adapter_t<CharType>;\n\n    template<typename InputType>\n    using binary_reader = ::nlohmann::detail::binary_reader<basic_json, InputType>;\n    template<typename CharType> using binary_writer = ::nlohmann::detail::binary_writer<basic_json, CharType>;\n\n  JSON_PRIVATE_UNLESS_TESTED:\n    using serializer = ::nlohmann::detail::serializer<basic_json>;\n\n  public:\n    using value_t = detail::value_t;\n    /// JSON Pointer, see @ref nlohmann::json_pointer\n    using json_pointer = ::nlohmann::json_pointer<StringType>;\n    template<typename T, typename SFINAE>\n    using json_serializer = JSONSerializer<T, SFINAE>;\n    /// how to treat decoding errors\n    using error_handler_t = detail::error_handler_t;\n    /// how to treat CBOR tags\n    using cbor_tag_handler_t = detail::cbor_tag_handler_t;\n    /// helper type for initializer lists of basic_json values\n    using initializer_list_t = std::initializer_list<detail::json_ref<basic_json>>;\n\n    using input_format_t = detail::input_format_t;\n    /// SAX interface type, see @ref nlohmann::json_sax\n    using json_sax_t = json_sax<basic_json>;\n\n    ////////////////\n    // exceptions //\n    ////////////////\n\n    /// @name exceptions\n    /// Classes to implement user-defined exceptions.\n    /// @{\n\n    using exception = detail::exception;\n    using parse_error = detail::parse_error;\n    using invalid_iterator = detail::invalid_iterator;\n    using type_error = detail::type_error;\n    using out_of_range = detail::out_of_range;\n    using other_error = detail::other_error;\n\n    /// @}\n\n\n    /////////////////////\n    // container types //\n    /////////////////////\n\n    /// @name container types\n    /// The canonic container types to use @ref basic_json like any other STL\n    /// container.\n    /// @{\n\n    /// the type of elements in a basic_json container\n    using value_type = basic_json;\n\n    /// the type of an element reference\n    using reference = value_type&;\n    /// the type of an element const reference\n    using const_reference = const value_type&;\n\n    /// a type to represent differences between iterators\n    using difference_type = std::ptrdiff_t;\n    /// a type to represent container sizes\n    using size_type = std::size_t;\n\n    /// the allocator type\n    using allocator_type = AllocatorType<basic_json>;\n\n    /// the type of an element pointer\n    using pointer = typename std::allocator_traits<allocator_type>::pointer;\n    /// the type of an element const pointer\n    using const_pointer = typename std::allocator_traits<allocator_type>::const_pointer;\n\n    /// an iterator for a basic_json container\n    using iterator = iter_impl<basic_json>;\n    /// a const iterator for a basic_json container\n    using const_iterator = iter_impl<const basic_json>;\n    /// a reverse iterator for a basic_json container\n    using reverse_iterator = json_reverse_iterator<typename basic_json::iterator>;\n    /// a const reverse iterator for a basic_json container\n    using const_reverse_iterator = json_reverse_iterator<typename basic_json::const_iterator>;\n\n    /// @}\n\n\n    /// @brief returns the allocator associated with the container\n    /// @sa https://json.nlohmann.me/api/basic_json/get_allocator/\n    static allocator_type get_allocator()\n    {\n        return allocator_type();\n    }\n\n    /// @brief returns version information on the library\n    /// @sa https://json.nlohmann.me/api/basic_json/meta/\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json meta()\n    {\n        basic_json result;\n\n        result[\"copyright\"] = \"(C) 2013-2022 Niels Lohmann\";\n        result[\"name\"] = \"JSON for Modern C++\";\n        result[\"url\"] = \"https://github.com/nlohmann/json\";\n        result[\"version\"][\"string\"] =\n            detail::concat(std::to_string(NLOHMANN_JSON_VERSION_MAJOR), '.',\n                           std::to_string(NLOHMANN_JSON_VERSION_MINOR), '.',\n                           std::to_string(NLOHMANN_JSON_VERSION_PATCH));\n        result[\"version\"][\"major\"] = NLOHMANN_JSON_VERSION_MAJOR;\n        result[\"version\"][\"minor\"] = NLOHMANN_JSON_VERSION_MINOR;\n        result[\"version\"][\"patch\"] = NLOHMANN_JSON_VERSION_PATCH;\n\n#ifdef _WIN32\n        result[\"platform\"] = \"win32\";\n#elif defined __linux__\n        result[\"platform\"] = \"linux\";\n#elif defined __APPLE__\n        result[\"platform\"] = \"apple\";\n#elif defined __unix__\n        result[\"platform\"] = \"unix\";\n#else\n        result[\"platform\"] = \"unknown\";\n#endif\n\n#if defined(__ICC) || defined(__INTEL_COMPILER)\n        result[\"compiler\"] = {{\"family\", \"icc\"}, {\"version\", __INTEL_COMPILER}};\n#elif defined(__clang__)\n        result[\"compiler\"] = {{\"family\", \"clang\"}, {\"version\", __clang_version__}};\n#elif defined(__GNUC__) || defined(__GNUG__)\n        result[\"compiler\"] = {{\"family\", \"gcc\"}, {\"version\", detail::concat(\n                    std::to_string(__GNUC__), '.',\n                    std::to_string(__GNUC_MINOR__), '.',\n                    std::to_string(__GNUC_PATCHLEVEL__))\n            }\n        };\n#elif defined(__HP_cc) || defined(__HP_aCC)\n        result[\"compiler\"] = \"hp\"\n#elif defined(__IBMCPP__)\n        result[\"compiler\"] = {{\"family\", \"ilecpp\"}, {\"version\", __IBMCPP__}};\n#elif defined(_MSC_VER)\n        result[\"compiler\"] = {{\"family\", \"msvc\"}, {\"version\", _MSC_VER}};\n#elif defined(__PGI)\n        result[\"compiler\"] = {{\"family\", \"pgcpp\"}, {\"version\", __PGI}};\n#elif defined(__SUNPRO_CC)\n        result[\"compiler\"] = {{\"family\", \"sunpro\"}, {\"version\", __SUNPRO_CC}};\n#else\n        result[\"compiler\"] = {{\"family\", \"unknown\"}, {\"version\", \"unknown\"}};\n#endif\n\n\n#if defined(_MSVC_LANG)\n        result[\"compiler\"][\"c++\"] = std::to_string(_MSVC_LANG);\n#elif defined(__cplusplus)\n        result[\"compiler\"][\"c++\"] = std::to_string(__cplusplus);\n#else\n        result[\"compiler\"][\"c++\"] = \"unknown\";\n#endif\n        return result;\n    }\n\n\n    ///////////////////////////\n    // JSON value data types //\n    ///////////////////////////\n\n    /// @name JSON value data types\n    /// The data types to store a JSON value. These types are derived from\n    /// the template arguments passed to class @ref basic_json.\n    /// @{\n\n    /// @brief default object key comparator type\n    /// The actual object key comparator type (@ref object_comparator_t) may be\n    /// different.\n    /// @sa https://json.nlohmann.me/api/basic_json/default_object_comparator_t/\n#if defined(JSON_HAS_CPP_14)\n    // use of transparent comparator avoids unnecessary repeated construction of temporaries\n    // in functions involving lookup by key with types other than object_t::key_type (aka. StringType)\n    using default_object_comparator_t = std::less<>;\n#else\n    using default_object_comparator_t = std::less<StringType>;\n#endif\n\n    /// @brief a type for an object\n    /// @sa https://json.nlohmann.me/api/basic_json/object_t/\n    using object_t = ObjectType<StringType,\n          basic_json,\n          default_object_comparator_t,\n          AllocatorType<std::pair<const StringType,\n          basic_json>>>;\n\n    /// @brief a type for an array\n    /// @sa https://json.nlohmann.me/api/basic_json/array_t/\n    using array_t = ArrayType<basic_json, AllocatorType<basic_json>>;\n\n    /// @brief a type for a string\n    /// @sa https://json.nlohmann.me/api/basic_json/string_t/\n    using string_t = StringType;\n\n    /// @brief a type for a boolean\n    /// @sa https://json.nlohmann.me/api/basic_json/boolean_t/\n    using boolean_t = BooleanType;\n\n    /// @brief a type for a number (integer)\n    /// @sa https://json.nlohmann.me/api/basic_json/number_integer_t/\n    using number_integer_t = NumberIntegerType;\n\n    /// @brief a type for a number (unsigned)\n    /// @sa https://json.nlohmann.me/api/basic_json/number_unsigned_t/\n    using number_unsigned_t = NumberUnsignedType;\n\n    /// @brief a type for a number (floating-point)\n    /// @sa https://json.nlohmann.me/api/basic_json/number_float_t/\n    using number_float_t = NumberFloatType;\n\n    /// @brief a type for a packed binary type\n    /// @sa https://json.nlohmann.me/api/basic_json/binary_t/\n    using binary_t = nlohmann::byte_container_with_subtype<BinaryType>;\n\n    /// @brief object key comparator type\n    /// @sa https://json.nlohmann.me/api/basic_json/object_comparator_t/\n    using object_comparator_t = detail::actual_object_comparator_t<basic_json>;\n\n    /// @}\n\n  private:\n\n    /// helper for exception-safe object creation\n    template<typename T, typename... Args>\n    JSON_HEDLEY_RETURNS_NON_NULL\n    static T* create(Args&& ... args)\n    {\n        AllocatorType<T> alloc;\n        using AllocatorTraits = std::allocator_traits<AllocatorType<T>>;\n\n        auto deleter = [&](T * obj)\n        {\n            AllocatorTraits::deallocate(alloc, obj, 1);\n        };\n        std::unique_ptr<T, decltype(deleter)> obj(AllocatorTraits::allocate(alloc, 1), deleter);\n        AllocatorTraits::construct(alloc, obj.get(), std::forward<Args>(args)...);\n        JSON_ASSERT(obj != nullptr);\n        return obj.release();\n    }\n\n    ////////////////////////\n    // JSON value storage //\n    ////////////////////////\n\n  JSON_PRIVATE_UNLESS_TESTED:\n    /*!\n    @brief a JSON value\n\n    The actual storage for a JSON value of the @ref basic_json class. This\n    union combines the different storage types for the JSON value types\n    defined in @ref value_t.\n\n    JSON type | value_t type    | used type\n    --------- | --------------- | ------------------------\n    object    | object          | pointer to @ref object_t\n    array     | array           | pointer to @ref array_t\n    string    | string          | pointer to @ref string_t\n    boolean   | boolean         | @ref boolean_t\n    number    | number_integer  | @ref number_integer_t\n    number    | number_unsigned | @ref number_unsigned_t\n    number    | number_float    | @ref number_float_t\n    binary    | binary          | pointer to @ref binary_t\n    null      | null            | *no value is stored*\n\n    @note Variable-length types (objects, arrays, and strings) are stored as\n    pointers. The size of the union should not exceed 64 bits if the default\n    value types are used.\n\n    @since version 1.0.0\n    */\n    union json_value\n    {\n        /// object (stored with pointer to save storage)\n        object_t* object;\n        /// array (stored with pointer to save storage)\n        array_t* array;\n        /// string (stored with pointer to save storage)\n        string_t* string;\n        /// binary (stored with pointer to save storage)\n        binary_t* binary;\n        /// boolean\n        boolean_t boolean;\n        /// number (integer)\n        number_integer_t number_integer;\n        /// number (unsigned integer)\n        number_unsigned_t number_unsigned;\n        /// number (floating-point)\n        number_float_t number_float;\n\n        /// default constructor (for null values)\n        json_value() = default;\n        /// constructor for booleans\n        json_value(boolean_t v) noexcept : boolean(v) {}\n        /// constructor for numbers (integer)\n        json_value(number_integer_t v) noexcept : number_integer(v) {}\n        /// constructor for numbers (unsigned)\n        json_value(number_unsigned_t v) noexcept : number_unsigned(v) {}\n        /// constructor for numbers (floating-point)\n        json_value(number_float_t v) noexcept : number_float(v) {}\n        /// constructor for empty values of a given type\n        json_value(value_t t)\n        {\n            switch (t)\n            {\n                case value_t::object:\n                {\n                    object = create<object_t>();\n                    break;\n                }\n\n                case value_t::array:\n                {\n                    array = create<array_t>();\n                    break;\n                }\n\n                case value_t::string:\n                {\n                    string = create<string_t>(\"\");\n                    break;\n                }\n\n                case value_t::binary:\n                {\n                    binary = create<binary_t>();\n                    break;\n                }\n\n                case value_t::boolean:\n                {\n                    boolean = static_cast<boolean_t>(false);\n                    break;\n                }\n\n                case value_t::number_integer:\n                {\n                    number_integer = static_cast<number_integer_t>(0);\n                    break;\n                }\n\n                case value_t::number_unsigned:\n                {\n                    number_unsigned = static_cast<number_unsigned_t>(0);\n                    break;\n                }\n\n                case value_t::number_float:\n                {\n                    number_float = static_cast<number_float_t>(0.0);\n                    break;\n                }\n\n                case value_t::null:\n                {\n                    object = nullptr;  // silence warning, see #821\n                    break;\n                }\n\n                case value_t::discarded:\n                default:\n                {\n                    object = nullptr;  // silence warning, see #821\n                    if (JSON_HEDLEY_UNLIKELY(t == value_t::null))\n                    {\n                        JSON_THROW(other_error::create(500, \"961c151d2e87f2686a955a9be24d316f1362bf21 3.11.2\", nullptr)); // LCOV_EXCL_LINE\n                    }\n                    break;\n                }\n            }\n        }\n\n        /// constructor for strings\n        json_value(const string_t& value) : string(create<string_t>(value)) {}\n\n        /// constructor for rvalue strings\n        json_value(string_t&& value) : string(create<string_t>(std::move(value))) {}\n\n        /// constructor for objects\n        json_value(const object_t& value) : object(create<object_t>(value)) {}\n\n        /// constructor for rvalue objects\n        json_value(object_t&& value) : object(create<object_t>(std::move(value))) {}\n\n        /// constructor for arrays\n        json_value(const array_t& value) : array(create<array_t>(value)) {}\n\n        /// constructor for rvalue arrays\n        json_value(array_t&& value) : array(create<array_t>(std::move(value))) {}\n\n        /// constructor for binary arrays\n        json_value(const typename binary_t::container_type& value) : binary(create<binary_t>(value)) {}\n\n        /// constructor for rvalue binary arrays\n        json_value(typename binary_t::container_type&& value) : binary(create<binary_t>(std::move(value))) {}\n\n        /// constructor for binary arrays (internal type)\n        json_value(const binary_t& value) : binary(create<binary_t>(value)) {}\n\n        /// constructor for rvalue binary arrays (internal type)\n        json_value(binary_t&& value) : binary(create<binary_t>(std::move(value))) {}\n\n        void destroy(value_t t)\n        {\n            if (t == value_t::array || t == value_t::object)\n            {\n                // flatten the current json_value to a heap-allocated stack\n                std::vector<basic_json> stack;\n\n                // move the top-level items to stack\n                if (t == value_t::array)\n                {\n                    stack.reserve(array->size());\n                    std::move(array->begin(), array->end(), std::back_inserter(stack));\n                }\n                else\n                {\n                    stack.reserve(object->size());\n                    for (auto&& it : *object)\n                    {\n                        stack.push_back(std::move(it.second));\n                    }\n                }\n\n                while (!stack.empty())\n                {\n                    // move the last item to local variable to be processed\n                    basic_json current_item(std::move(stack.back()));\n                    stack.pop_back();\n\n                    // if current_item is array/object, move\n                    // its children to the stack to be processed later\n                    if (current_item.is_array())\n                    {\n                        std::move(current_item.m_value.array->begin(), current_item.m_value.array->end(), std::back_inserter(stack));\n\n                        current_item.m_value.array->clear();\n                    }\n                    else if (current_item.is_object())\n                    {\n                        for (auto&& it : *current_item.m_value.object)\n                        {\n                            stack.push_back(std::move(it.second));\n                        }\n\n                        current_item.m_value.object->clear();\n                    }\n\n                    // it's now safe that current_item get destructed\n                    // since it doesn't have any children\n                }\n            }\n\n            switch (t)\n            {\n                case value_t::object:\n                {\n                    AllocatorType<object_t> alloc;\n                    std::allocator_traits<decltype(alloc)>::destroy(alloc, object);\n                    std::allocator_traits<decltype(alloc)>::deallocate(alloc, object, 1);\n                    break;\n                }\n\n                case value_t::array:\n                {\n                    AllocatorType<array_t> alloc;\n                    std::allocator_traits<decltype(alloc)>::destroy(alloc, array);\n                    std::allocator_traits<decltype(alloc)>::deallocate(alloc, array, 1);\n                    break;\n                }\n\n                case value_t::string:\n                {\n                    AllocatorType<string_t> alloc;\n                    std::allocator_traits<decltype(alloc)>::destroy(alloc, string);\n                    std::allocator_traits<decltype(alloc)>::deallocate(alloc, string, 1);\n                    break;\n                }\n\n                case value_t::binary:\n                {\n                    AllocatorType<binary_t> alloc;\n                    std::allocator_traits<decltype(alloc)>::destroy(alloc, binary);\n                    std::allocator_traits<decltype(alloc)>::deallocate(alloc, binary, 1);\n                    break;\n                }\n\n                case value_t::null:\n                case value_t::boolean:\n                case value_t::number_integer:\n                case value_t::number_unsigned:\n                case value_t::number_float:\n                case value_t::discarded:\n                default:\n                {\n                    break;\n                }\n            }\n        }\n    };\n\n  private:\n    /*!\n    @brief checks the class invariants\n\n    This function asserts the class invariants. It needs to be called at the\n    end of every constructor to make sure that created objects respect the\n    invariant. Furthermore, it has to be called each time the type of a JSON\n    value is changed, because the invariant expresses a relationship between\n    @a m_type and @a m_value.\n\n    Furthermore, the parent relation is checked for arrays and objects: If\n    @a check_parents true and the value is an array or object, then the\n    container's elements must have the current value as parent.\n\n    @param[in] check_parents  whether the parent relation should be checked.\n               The value is true by default and should only be set to false\n               during destruction of objects when the invariant does not\n               need to hold.\n    */\n    void assert_invariant(bool check_parents = true) const noexcept\n    {\n        JSON_ASSERT(m_type != value_t::object || m_value.object != nullptr);\n        JSON_ASSERT(m_type != value_t::array || m_value.array != nullptr);\n        JSON_ASSERT(m_type != value_t::string || m_value.string != nullptr);\n        JSON_ASSERT(m_type != value_t::binary || m_value.binary != nullptr);\n\n#if JSON_DIAGNOSTICS\n        JSON_TRY\n        {\n            // cppcheck-suppress assertWithSideEffect\n            JSON_ASSERT(!check_parents || !is_structured() || std::all_of(begin(), end(), [this](const basic_json & j)\n            {\n                return j.m_parent == this;\n            }));\n        }\n        JSON_CATCH(...) {} // LCOV_EXCL_LINE\n#endif\n        static_cast<void>(check_parents);\n    }\n\n    void set_parents()\n    {\n#if JSON_DIAGNOSTICS\n        switch (m_type)\n        {\n            case value_t::array:\n            {\n                for (auto& element : *m_value.array)\n                {\n                    element.m_parent = this;\n                }\n                break;\n            }\n\n            case value_t::object:\n            {\n                for (auto& element : *m_value.object)\n                {\n                    element.second.m_parent = this;\n                }\n                break;\n            }\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n                break;\n        }\n#endif\n    }\n\n    iterator set_parents(iterator it, typename iterator::difference_type count_set_parents)\n    {\n#if JSON_DIAGNOSTICS\n        for (typename iterator::difference_type i = 0; i < count_set_parents; ++i)\n        {\n            (it + i)->m_parent = this;\n        }\n#else\n        static_cast<void>(count_set_parents);\n#endif\n        return it;\n    }\n\n    reference set_parent(reference j, std::size_t old_capacity = static_cast<std::size_t>(-1))\n    {\n#if JSON_DIAGNOSTICS\n        if (old_capacity != static_cast<std::size_t>(-1))\n        {\n            // see https://github.com/nlohmann/json/issues/2838\n            JSON_ASSERT(type() == value_t::array);\n            if (JSON_HEDLEY_UNLIKELY(m_value.array->capacity() != old_capacity))\n            {\n                // capacity has changed: update all parents\n                set_parents();\n                return j;\n            }\n        }\n\n        // ordered_json uses a vector internally, so pointers could have\n        // been invalidated; see https://github.com/nlohmann/json/issues/2962\n#ifdef JSON_HEDLEY_MSVC_VERSION\n#pragma warning(push )\n#pragma warning(disable : 4127) // ignore warning to replace if with if constexpr\n#endif\n        if (detail::is_ordered_map<object_t>::value)\n        {\n            set_parents();\n            return j;\n        }\n#ifdef JSON_HEDLEY_MSVC_VERSION\n#pragma warning( pop )\n#endif\n\n        j.m_parent = this;\n#else\n        static_cast<void>(j);\n        static_cast<void>(old_capacity);\n#endif\n        return j;\n    }\n\n  public:\n    //////////////////////////\n    // JSON parser callback //\n    //////////////////////////\n\n    /// @brief parser event types\n    /// @sa https://json.nlohmann.me/api/basic_json/parse_event_t/\n    using parse_event_t = detail::parse_event_t;\n\n    /// @brief per-element parser callback type\n    /// @sa https://json.nlohmann.me/api/basic_json/parser_callback_t/\n    using parser_callback_t = detail::parser_callback_t<basic_json>;\n\n    //////////////////\n    // constructors //\n    //////////////////\n\n    /// @name constructors and destructors\n    /// Constructors of class @ref basic_json, copy/move constructor, copy\n    /// assignment, static functions creating objects, and the destructor.\n    /// @{\n\n    /// @brief create an empty value with a given type\n    /// @sa https://json.nlohmann.me/api/basic_json/basic_json/\n    basic_json(const value_t v)\n        : m_type(v), m_value(v)\n    {\n        assert_invariant();\n    }\n\n    /// @brief create a null object\n    /// @sa https://json.nlohmann.me/api/basic_json/basic_json/\n    basic_json(std::nullptr_t = nullptr) noexcept // NOLINT(bugprone-exception-escape)\n        : basic_json(value_t::null)\n    {\n        assert_invariant();\n    }\n\n    /// @brief create a JSON value from compatible types\n    /// @sa https://json.nlohmann.me/api/basic_json/basic_json/\n    template < typename CompatibleType,\n               typename U = detail::uncvref_t<CompatibleType>,\n               detail::enable_if_t <\n                   !detail::is_basic_json<U>::value && detail::is_compatible_type<basic_json_t, U>::value, int > = 0 >\n    basic_json(CompatibleType && val) noexcept(noexcept( // NOLINT(bugprone-forwarding-reference-overload,bugprone-exception-escape)\n                JSONSerializer<U>::to_json(std::declval<basic_json_t&>(),\n                                           std::forward<CompatibleType>(val))))\n    {\n        JSONSerializer<U>::to_json(*this, std::forward<CompatibleType>(val));\n        set_parents();\n        assert_invariant();\n    }\n\n    /// @brief create a JSON value from an existing one\n    /// @sa https://json.nlohmann.me/api/basic_json/basic_json/\n    template < typename BasicJsonType,\n               detail::enable_if_t <\n                   detail::is_basic_json<BasicJsonType>::value&& !std::is_same<basic_json, BasicJsonType>::value, int > = 0 >\n    basic_json(const BasicJsonType& val)\n    {\n        using other_boolean_t = typename BasicJsonType::boolean_t;\n        using other_number_float_t = typename BasicJsonType::number_float_t;\n        using other_number_integer_t = typename BasicJsonType::number_integer_t;\n        using other_number_unsigned_t = typename BasicJsonType::number_unsigned_t;\n        using other_string_t = typename BasicJsonType::string_t;\n        using other_object_t = typename BasicJsonType::object_t;\n        using other_array_t = typename BasicJsonType::array_t;\n        using other_binary_t = typename BasicJsonType::binary_t;\n\n        switch (val.type())\n        {\n            case value_t::boolean:\n                JSONSerializer<other_boolean_t>::to_json(*this, val.template get<other_boolean_t>());\n                break;\n            case value_t::number_float:\n                JSONSerializer<other_number_float_t>::to_json(*this, val.template get<other_number_float_t>());\n                break;\n            case value_t::number_integer:\n                JSONSerializer<other_number_integer_t>::to_json(*this, val.template get<other_number_integer_t>());\n                break;\n            case value_t::number_unsigned:\n                JSONSerializer<other_number_unsigned_t>::to_json(*this, val.template get<other_number_unsigned_t>());\n                break;\n            case value_t::string:\n                JSONSerializer<other_string_t>::to_json(*this, val.template get_ref<const other_string_t&>());\n                break;\n            case value_t::object:\n                JSONSerializer<other_object_t>::to_json(*this, val.template get_ref<const other_object_t&>());\n                break;\n            case value_t::array:\n                JSONSerializer<other_array_t>::to_json(*this, val.template get_ref<const other_array_t&>());\n                break;\n            case value_t::binary:\n                JSONSerializer<other_binary_t>::to_json(*this, val.template get_ref<const other_binary_t&>());\n                break;\n            case value_t::null:\n                *this = nullptr;\n                break;\n            case value_t::discarded:\n                m_type = value_t::discarded;\n                break;\n            default:            // LCOV_EXCL_LINE\n                JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE\n        }\n        JSON_ASSERT(m_type == val.type());\n        set_parents();\n        assert_invariant();\n    }\n\n    /// @brief create a container (array or object) from an initializer list\n    /// @sa https://json.nlohmann.me/api/basic_json/basic_json/\n    basic_json(initializer_list_t init,\n               bool type_deduction = true,\n               value_t manual_type = value_t::array)\n    {\n        // check if each element is an array with two elements whose first\n        // element is a string\n        bool is_an_object = std::all_of(init.begin(), init.end(),\n                                        [](const detail::json_ref<basic_json>& element_ref)\n        {\n            return element_ref->is_array() && element_ref->size() == 2 && (*element_ref)[0].is_string();\n        });\n\n        // adjust type if type deduction is not wanted\n        if (!type_deduction)\n        {\n            // if array is wanted, do not create an object though possible\n            if (manual_type == value_t::array)\n            {\n                is_an_object = false;\n            }\n\n            // if object is wanted but impossible, throw an exception\n            if (JSON_HEDLEY_UNLIKELY(manual_type == value_t::object && !is_an_object))\n            {\n                JSON_THROW(type_error::create(301, \"cannot create object from initializer list\", nullptr));\n            }\n        }\n\n        if (is_an_object)\n        {\n            // the initializer list is a list of pairs -> create object\n            m_type = value_t::object;\n            m_value = value_t::object;\n\n            for (auto& element_ref : init)\n            {\n                auto element = element_ref.moved_or_copied();\n                m_value.object->emplace(\n                    std::move(*((*element.m_value.array)[0].m_value.string)),\n                    std::move((*element.m_value.array)[1]));\n            }\n        }\n        else\n        {\n            // the initializer list describes an array -> create array\n            m_type = value_t::array;\n            m_value.array = create<array_t>(init.begin(), init.end());\n        }\n\n        set_parents();\n        assert_invariant();\n    }\n\n    /// @brief explicitly create a binary array (without subtype)\n    /// @sa https://json.nlohmann.me/api/basic_json/binary/\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json binary(const typename binary_t::container_type& init)\n    {\n        auto res = basic_json();\n        res.m_type = value_t::binary;\n        res.m_value = init;\n        return res;\n    }\n\n    /// @brief explicitly create a binary array (with subtype)\n    /// @sa https://json.nlohmann.me/api/basic_json/binary/\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json binary(const typename binary_t::container_type& init, typename binary_t::subtype_type subtype)\n    {\n        auto res = basic_json();\n        res.m_type = value_t::binary;\n        res.m_value = binary_t(init, subtype);\n        return res;\n    }\n\n    /// @brief explicitly create a binary array\n    /// @sa https://json.nlohmann.me/api/basic_json/binary/\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json binary(typename binary_t::container_type&& init)\n    {\n        auto res = basic_json();\n        res.m_type = value_t::binary;\n        res.m_value = std::move(init);\n        return res;\n    }\n\n    /// @brief explicitly create a binary array (with subtype)\n    /// @sa https://json.nlohmann.me/api/basic_json/binary/\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json binary(typename binary_t::container_type&& init, typename binary_t::subtype_type subtype)\n    {\n        auto res = basic_json();\n        res.m_type = value_t::binary;\n        res.m_value = binary_t(std::move(init), subtype);\n        return res;\n    }\n\n    /// @brief explicitly create an array from an initializer list\n    /// @sa https://json.nlohmann.me/api/basic_json/array/\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json array(initializer_list_t init = {})\n    {\n        return basic_json(init, false, value_t::array);\n    }\n\n    /// @brief explicitly create an object from an initializer list\n    /// @sa https://json.nlohmann.me/api/basic_json/object/\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json object(initializer_list_t init = {})\n    {\n        return basic_json(init, false, value_t::object);\n    }\n\n    /// @brief construct an array with count copies of given value\n    /// @sa https://json.nlohmann.me/api/basic_json/basic_json/\n    basic_json(size_type cnt, const basic_json& val)\n        : m_type(value_t::array)\n    {\n        m_value.array = create<array_t>(cnt, val);\n        set_parents();\n        assert_invariant();\n    }\n\n    /// @brief construct a JSON container given an iterator range\n    /// @sa https://json.nlohmann.me/api/basic_json/basic_json/\n    template < class InputIT, typename std::enable_if <\n                   std::is_same<InputIT, typename basic_json_t::iterator>::value ||\n                   std::is_same<InputIT, typename basic_json_t::const_iterator>::value, int >::type = 0 >\n    basic_json(InputIT first, InputIT last)\n    {\n        JSON_ASSERT(first.m_object != nullptr);\n        JSON_ASSERT(last.m_object != nullptr);\n\n        // make sure iterator fits the current value\n        if (JSON_HEDLEY_UNLIKELY(first.m_object != last.m_object))\n        {\n            JSON_THROW(invalid_iterator::create(201, \"iterators are not compatible\", nullptr));\n        }\n\n        // copy type from first iterator\n        m_type = first.m_object->m_type;\n\n        // check if iterator range is complete for primitive values\n        switch (m_type)\n        {\n            case value_t::boolean:\n            case value_t::number_float:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::string:\n            {\n                if (JSON_HEDLEY_UNLIKELY(!first.m_it.primitive_iterator.is_begin()\n                                         || !last.m_it.primitive_iterator.is_end()))\n                {\n                    JSON_THROW(invalid_iterator::create(204, \"iterators out of range\", first.m_object));\n                }\n                break;\n            }\n\n            case value_t::null:\n            case value_t::object:\n            case value_t::array:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n                break;\n        }\n\n        switch (m_type)\n        {\n            case value_t::number_integer:\n            {\n                m_value.number_integer = first.m_object->m_value.number_integer;\n                break;\n            }\n\n            case value_t::number_unsigned:\n            {\n                m_value.number_unsigned = first.m_object->m_value.number_unsigned;\n                break;\n            }\n\n            case value_t::number_float:\n            {\n                m_value.number_float = first.m_object->m_value.number_float;\n                break;\n            }\n\n            case value_t::boolean:\n            {\n                m_value.boolean = first.m_object->m_value.boolean;\n                break;\n            }\n\n            case value_t::string:\n            {\n                m_value = *first.m_object->m_value.string;\n                break;\n            }\n\n            case value_t::object:\n            {\n                m_value.object = create<object_t>(first.m_it.object_iterator,\n                                                  last.m_it.object_iterator);\n                break;\n            }\n\n            case value_t::array:\n            {\n                m_value.array = create<array_t>(first.m_it.array_iterator,\n                                                last.m_it.array_iterator);\n                break;\n            }\n\n            case value_t::binary:\n            {\n                m_value = *first.m_object->m_value.binary;\n                break;\n            }\n\n            case value_t::null:\n            case value_t::discarded:\n            default:\n                JSON_THROW(invalid_iterator::create(206, detail::concat(\"cannot construct with iterators from \", first.m_object->type_name()), first.m_object));\n        }\n\n        set_parents();\n        assert_invariant();\n    }\n\n\n    ///////////////////////////////////////\n    // other constructors and destructor //\n    ///////////////////////////////////////\n\n    template<typename JsonRef,\n             detail::enable_if_t<detail::conjunction<detail::is_json_ref<JsonRef>,\n                                 std::is_same<typename JsonRef::value_type, basic_json>>::value, int> = 0 >\n    basic_json(const JsonRef& ref) : basic_json(ref.moved_or_copied()) {}\n\n    /// @brief copy constructor\n    /// @sa https://json.nlohmann.me/api/basic_json/basic_json/\n    basic_json(const basic_json& other)\n        : m_type(other.m_type)\n    {\n        // check of passed value is valid\n        other.assert_invariant();\n\n        switch (m_type)\n        {\n            case value_t::object:\n            {\n                m_value = *other.m_value.object;\n                break;\n            }\n\n            case value_t::array:\n            {\n                m_value = *other.m_value.array;\n                break;\n            }\n\n            case value_t::string:\n            {\n                m_value = *other.m_value.string;\n                break;\n            }\n\n            case value_t::boolean:\n            {\n                m_value = other.m_value.boolean;\n                break;\n            }\n\n            case value_t::number_integer:\n            {\n                m_value = other.m_value.number_integer;\n                break;\n            }\n\n            case value_t::number_unsigned:\n            {\n                m_value = other.m_value.number_unsigned;\n                break;\n            }\n\n            case value_t::number_float:\n            {\n                m_value = other.m_value.number_float;\n                break;\n            }\n\n            case value_t::binary:\n            {\n                m_value = *other.m_value.binary;\n                break;\n            }\n\n            case value_t::null:\n            case value_t::discarded:\n            default:\n                break;\n        }\n\n        set_parents();\n        assert_invariant();\n    }\n\n    /// @brief move constructor\n    /// @sa https://json.nlohmann.me/api/basic_json/basic_json/\n    basic_json(basic_json&& other) noexcept\n        : m_type(std::move(other.m_type)),\n          m_value(std::move(other.m_value))\n    {\n        // check that passed value is valid\n        other.assert_invariant(false);\n\n        // invalidate payload\n        other.m_type = value_t::null;\n        other.m_value = {};\n\n        set_parents();\n        assert_invariant();\n    }\n\n    /// @brief copy assignment\n    /// @sa https://json.nlohmann.me/api/basic_json/operator=/\n    basic_json& operator=(basic_json other) noexcept (\n        std::is_nothrow_move_constructible<value_t>::value&&\n        std::is_nothrow_move_assignable<value_t>::value&&\n        std::is_nothrow_move_constructible<json_value>::value&&\n        std::is_nothrow_move_assignable<json_value>::value\n    )\n    {\n        // check that passed value is valid\n        other.assert_invariant();\n\n        using std::swap;\n        swap(m_type, other.m_type);\n        swap(m_value, other.m_value);\n\n        set_parents();\n        assert_invariant();\n        return *this;\n    }\n\n    /// @brief destructor\n    /// @sa https://json.nlohmann.me/api/basic_json/~basic_json/\n    ~basic_json() noexcept\n    {\n        assert_invariant(false);\n        m_value.destroy(m_type);\n    }\n\n    /// @}\n\n  public:\n    ///////////////////////\n    // object inspection //\n    ///////////////////////\n\n    /// @name object inspection\n    /// Functions to inspect the type of a JSON value.\n    /// @{\n\n    /// @brief serialization\n    /// @sa https://json.nlohmann.me/api/basic_json/dump/\n    string_t dump(const int indent = -1,\n                  const char indent_char = ' ',\n                  const bool ensure_ascii = false,\n                  const error_handler_t error_handler = error_handler_t::strict) const\n    {\n        string_t result;\n        serializer s(detail::output_adapter<char, string_t>(result), indent_char, error_handler);\n\n        if (indent >= 0)\n        {\n            s.dump(*this, true, ensure_ascii, static_cast<unsigned int>(indent));\n        }\n        else\n        {\n            s.dump(*this, false, ensure_ascii, 0);\n        }\n\n        return result;\n    }\n\n    /// @brief return the type of the JSON value (explicit)\n    /// @sa https://json.nlohmann.me/api/basic_json/type/\n    constexpr value_t type() const noexcept\n    {\n        return m_type;\n    }\n\n    /// @brief return whether type is primitive\n    /// @sa https://json.nlohmann.me/api/basic_json/is_primitive/\n    constexpr bool is_primitive() const noexcept\n    {\n        return is_null() || is_string() || is_boolean() || is_number() || is_binary();\n    }\n\n    /// @brief return whether type is structured\n    /// @sa https://json.nlohmann.me/api/basic_json/is_structured/\n    constexpr bool is_structured() const noexcept\n    {\n        return is_array() || is_object();\n    }\n\n    /// @brief return whether value is null\n    /// @sa https://json.nlohmann.me/api/basic_json/is_null/\n    constexpr bool is_null() const noexcept\n    {\n        return m_type == value_t::null;\n    }\n\n    /// @brief return whether value is a boolean\n    /// @sa https://json.nlohmann.me/api/basic_json/is_boolean/\n    constexpr bool is_boolean() const noexcept\n    {\n        return m_type == value_t::boolean;\n    }\n\n    /// @brief return whether value is a number\n    /// @sa https://json.nlohmann.me/api/basic_json/is_number/\n    constexpr bool is_number() const noexcept\n    {\n        return is_number_integer() || is_number_float();\n    }\n\n    /// @brief return whether value is an integer number\n    /// @sa https://json.nlohmann.me/api/basic_json/is_number_integer/\n    constexpr bool is_number_integer() const noexcept\n    {\n        return m_type == value_t::number_integer || m_type == value_t::number_unsigned;\n    }\n\n    /// @brief return whether value is an unsigned integer number\n    /// @sa https://json.nlohmann.me/api/basic_json/is_number_unsigned/\n    constexpr bool is_number_unsigned() const noexcept\n    {\n        return m_type == value_t::number_unsigned;\n    }\n\n    /// @brief return whether value is a floating-point number\n    /// @sa https://json.nlohmann.me/api/basic_json/is_number_float/\n    constexpr bool is_number_float() const noexcept\n    {\n        return m_type == value_t::number_float;\n    }\n\n    /// @brief return whether value is an object\n    /// @sa https://json.nlohmann.me/api/basic_json/is_object/\n    constexpr bool is_object() const noexcept\n    {\n        return m_type == value_t::object;\n    }\n\n    /// @brief return whether value is an array\n    /// @sa https://json.nlohmann.me/api/basic_json/is_array/\n    constexpr bool is_array() const noexcept\n    {\n        return m_type == value_t::array;\n    }\n\n    /// @brief return whether value is a string\n    /// @sa https://json.nlohmann.me/api/basic_json/is_string/\n    constexpr bool is_string() const noexcept\n    {\n        return m_type == value_t::string;\n    }\n\n    /// @brief return whether value is a binary array\n    /// @sa https://json.nlohmann.me/api/basic_json/is_binary/\n    constexpr bool is_binary() const noexcept\n    {\n        return m_type == value_t::binary;\n    }\n\n    /// @brief return whether value is discarded\n    /// @sa https://json.nlohmann.me/api/basic_json/is_discarded/\n    constexpr bool is_discarded() const noexcept\n    {\n        return m_type == value_t::discarded;\n    }\n\n    /// @brief return the type of the JSON value (implicit)\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_value_t/\n    constexpr operator value_t() const noexcept\n    {\n        return m_type;\n    }\n\n    /// @}\n\n  private:\n    //////////////////\n    // value access //\n    //////////////////\n\n    /// get a boolean (explicit)\n    boolean_t get_impl(boolean_t* /*unused*/) const\n    {\n        if (JSON_HEDLEY_LIKELY(is_boolean()))\n        {\n            return m_value.boolean;\n        }\n\n        JSON_THROW(type_error::create(302, detail::concat(\"type must be boolean, but is \", type_name()), this));\n    }\n\n    /// get a pointer to the value (object)\n    object_t* get_impl_ptr(object_t* /*unused*/) noexcept\n    {\n        return is_object() ? m_value.object : nullptr;\n    }\n\n    /// get a pointer to the value (object)\n    constexpr const object_t* get_impl_ptr(const object_t* /*unused*/) const noexcept\n    {\n        return is_object() ? m_value.object : nullptr;\n    }\n\n    /// get a pointer to the value (array)\n    array_t* get_impl_ptr(array_t* /*unused*/) noexcept\n    {\n        return is_array() ? m_value.array : nullptr;\n    }\n\n    /// get a pointer to the value (array)\n    constexpr const array_t* get_impl_ptr(const array_t* /*unused*/) const noexcept\n    {\n        return is_array() ? m_value.array : nullptr;\n    }\n\n    /// get a pointer to the value (string)\n    string_t* get_impl_ptr(string_t* /*unused*/) noexcept\n    {\n        return is_string() ? m_value.string : nullptr;\n    }\n\n    /// get a pointer to the value (string)\n    constexpr const string_t* get_impl_ptr(const string_t* /*unused*/) const noexcept\n    {\n        return is_string() ? m_value.string : nullptr;\n    }\n\n    /// get a pointer to the value (boolean)\n    boolean_t* get_impl_ptr(boolean_t* /*unused*/) noexcept\n    {\n        return is_boolean() ? &m_value.boolean : nullptr;\n    }\n\n    /// get a pointer to the value (boolean)\n    constexpr const boolean_t* get_impl_ptr(const boolean_t* /*unused*/) const noexcept\n    {\n        return is_boolean() ? &m_value.boolean : nullptr;\n    }\n\n    /// get a pointer to the value (integer number)\n    number_integer_t* get_impl_ptr(number_integer_t* /*unused*/) noexcept\n    {\n        return is_number_integer() ? &m_value.number_integer : nullptr;\n    }\n\n    /// get a pointer to the value (integer number)\n    constexpr const number_integer_t* get_impl_ptr(const number_integer_t* /*unused*/) const noexcept\n    {\n        return is_number_integer() ? &m_value.number_integer : nullptr;\n    }\n\n    /// get a pointer to the value (unsigned number)\n    number_unsigned_t* get_impl_ptr(number_unsigned_t* /*unused*/) noexcept\n    {\n        return is_number_unsigned() ? &m_value.number_unsigned : nullptr;\n    }\n\n    /// get a pointer to the value (unsigned number)\n    constexpr const number_unsigned_t* get_impl_ptr(const number_unsigned_t* /*unused*/) const noexcept\n    {\n        return is_number_unsigned() ? &m_value.number_unsigned : nullptr;\n    }\n\n    /// get a pointer to the value (floating-point number)\n    number_float_t* get_impl_ptr(number_float_t* /*unused*/) noexcept\n    {\n        return is_number_float() ? &m_value.number_float : nullptr;\n    }\n\n    /// get a pointer to the value (floating-point number)\n    constexpr const number_float_t* get_impl_ptr(const number_float_t* /*unused*/) const noexcept\n    {\n        return is_number_float() ? &m_value.number_float : nullptr;\n    }\n\n    /// get a pointer to the value (binary)\n    binary_t* get_impl_ptr(binary_t* /*unused*/) noexcept\n    {\n        return is_binary() ? m_value.binary : nullptr;\n    }\n\n    /// get a pointer to the value (binary)\n    constexpr const binary_t* get_impl_ptr(const binary_t* /*unused*/) const noexcept\n    {\n        return is_binary() ? m_value.binary : nullptr;\n    }\n\n    /*!\n    @brief helper function to implement get_ref()\n\n    This function helps to implement get_ref() without code duplication for\n    const and non-const overloads\n\n    @tparam ThisType will be deduced as `basic_json` or `const basic_json`\n\n    @throw type_error.303 if ReferenceType does not match underlying value\n    type of the current JSON\n    */\n    template<typename ReferenceType, typename ThisType>\n    static ReferenceType get_ref_impl(ThisType& obj)\n    {\n        // delegate the call to get_ptr<>()\n        auto* ptr = obj.template get_ptr<typename std::add_pointer<ReferenceType>::type>();\n\n        if (JSON_HEDLEY_LIKELY(ptr != nullptr))\n        {\n            return *ptr;\n        }\n\n        JSON_THROW(type_error::create(303, detail::concat(\"incompatible ReferenceType for get_ref, actual type is \", obj.type_name()), &obj));\n    }\n\n  public:\n    /// @name value access\n    /// Direct access to the stored value of a JSON value.\n    /// @{\n\n    /// @brief get a pointer value (implicit)\n    /// @sa https://json.nlohmann.me/api/basic_json/get_ptr/\n    template<typename PointerType, typename std::enable_if<\n                 std::is_pointer<PointerType>::value, int>::type = 0>\n    auto get_ptr() noexcept -> decltype(std::declval<basic_json_t&>().get_impl_ptr(std::declval<PointerType>()))\n    {\n        // delegate the call to get_impl_ptr<>()\n        return get_impl_ptr(static_cast<PointerType>(nullptr));\n    }\n\n    /// @brief get a pointer value (implicit)\n    /// @sa https://json.nlohmann.me/api/basic_json/get_ptr/\n    template < typename PointerType, typename std::enable_if <\n                   std::is_pointer<PointerType>::value&&\n                   std::is_const<typename std::remove_pointer<PointerType>::type>::value, int >::type = 0 >\n    constexpr auto get_ptr() const noexcept -> decltype(std::declval<const basic_json_t&>().get_impl_ptr(std::declval<PointerType>()))\n    {\n        // delegate the call to get_impl_ptr<>() const\n        return get_impl_ptr(static_cast<PointerType>(nullptr));\n    }\n\n  private:\n    /*!\n    @brief get a value (explicit)\n\n    Explicit type conversion between the JSON value and a compatible value\n    which is [CopyConstructible](https://en.cppreference.com/w/cpp/named_req/CopyConstructible)\n    and [DefaultConstructible](https://en.cppreference.com/w/cpp/named_req/DefaultConstructible).\n    The value is converted by calling the @ref json_serializer<ValueType>\n    `from_json()` method.\n\n    The function is equivalent to executing\n    @code {.cpp}\n    ValueType ret;\n    JSONSerializer<ValueType>::from_json(*this, ret);\n    return ret;\n    @endcode\n\n    This overloads is chosen if:\n    - @a ValueType is not @ref basic_json,\n    - @ref json_serializer<ValueType> has a `from_json()` method of the form\n      `void from_json(const basic_json&, ValueType&)`, and\n    - @ref json_serializer<ValueType> does not have a `from_json()` method of\n      the form `ValueType from_json(const basic_json&)`\n\n    @tparam ValueType the returned value type\n\n    @return copy of the JSON value, converted to @a ValueType\n\n    @throw what @ref json_serializer<ValueType> `from_json()` method throws\n\n    @liveexample{The example below shows several conversions from JSON values\n    to other types. There a few things to note: (1) Floating-point numbers can\n    be converted to integers\\, (2) A JSON array can be converted to a standard\n    `std::vector<short>`\\, (3) A JSON object can be converted to C++\n    associative containers such as `std::unordered_map<std::string\\,\n    json>`.,get__ValueType_const}\n\n    @since version 2.1.0\n    */\n    template < typename ValueType,\n               detail::enable_if_t <\n                   detail::is_default_constructible<ValueType>::value&&\n                   detail::has_from_json<basic_json_t, ValueType>::value,\n                   int > = 0 >\n    ValueType get_impl(detail::priority_tag<0> /*unused*/) const noexcept(noexcept(\n                JSONSerializer<ValueType>::from_json(std::declval<const basic_json_t&>(), std::declval<ValueType&>())))\n    {\n        auto ret = ValueType();\n        JSONSerializer<ValueType>::from_json(*this, ret);\n        return ret;\n    }\n\n    /*!\n    @brief get a value (explicit); special case\n\n    Explicit type conversion between the JSON value and a compatible value\n    which is **not** [CopyConstructible](https://en.cppreference.com/w/cpp/named_req/CopyConstructible)\n    and **not** [DefaultConstructible](https://en.cppreference.com/w/cpp/named_req/DefaultConstructible).\n    The value is converted by calling the @ref json_serializer<ValueType>\n    `from_json()` method.\n\n    The function is equivalent to executing\n    @code {.cpp}\n    return JSONSerializer<ValueType>::from_json(*this);\n    @endcode\n\n    This overloads is chosen if:\n    - @a ValueType is not @ref basic_json and\n    - @ref json_serializer<ValueType> has a `from_json()` method of the form\n      `ValueType from_json(const basic_json&)`\n\n    @note If @ref json_serializer<ValueType> has both overloads of\n    `from_json()`, this one is chosen.\n\n    @tparam ValueType the returned value type\n\n    @return copy of the JSON value, converted to @a ValueType\n\n    @throw what @ref json_serializer<ValueType> `from_json()` method throws\n\n    @since version 2.1.0\n    */\n    template < typename ValueType,\n               detail::enable_if_t <\n                   detail::has_non_default_from_json<basic_json_t, ValueType>::value,\n                   int > = 0 >\n    ValueType get_impl(detail::priority_tag<1> /*unused*/) const noexcept(noexcept(\n                JSONSerializer<ValueType>::from_json(std::declval<const basic_json_t&>())))\n    {\n        return JSONSerializer<ValueType>::from_json(*this);\n    }\n\n    /*!\n    @brief get special-case overload\n\n    This overloads converts the current @ref basic_json in a different\n    @ref basic_json type\n\n    @tparam BasicJsonType == @ref basic_json\n\n    @return a copy of *this, converted into @a BasicJsonType\n\n    @complexity Depending on the implementation of the called `from_json()`\n                method.\n\n    @since version 3.2.0\n    */\n    template < typename BasicJsonType,\n               detail::enable_if_t <\n                   detail::is_basic_json<BasicJsonType>::value,\n                   int > = 0 >\n    BasicJsonType get_impl(detail::priority_tag<2> /*unused*/) const\n    {\n        return *this;\n    }\n\n    /*!\n    @brief get special-case overload\n\n    This overloads avoids a lot of template boilerplate, it can be seen as the\n    identity method\n\n    @tparam BasicJsonType == @ref basic_json\n\n    @return a copy of *this\n\n    @complexity Constant.\n\n    @since version 2.1.0\n    */\n    template<typename BasicJsonType,\n             detail::enable_if_t<\n                 std::is_same<BasicJsonType, basic_json_t>::value,\n                 int> = 0>\n    basic_json get_impl(detail::priority_tag<3> /*unused*/) const\n    {\n        return *this;\n    }\n\n    /*!\n    @brief get a pointer value (explicit)\n    @copydoc get()\n    */\n    template<typename PointerType,\n             detail::enable_if_t<\n                 std::is_pointer<PointerType>::value,\n                 int> = 0>\n    constexpr auto get_impl(detail::priority_tag<4> /*unused*/) const noexcept\n    -> decltype(std::declval<const basic_json_t&>().template get_ptr<PointerType>())\n    {\n        // delegate the call to get_ptr\n        return get_ptr<PointerType>();\n    }\n\n  public:\n    /*!\n    @brief get a (pointer) value (explicit)\n\n    Performs explicit type conversion between the JSON value and a compatible value if required.\n\n    - If the requested type is a pointer to the internally stored JSON value that pointer is returned.\n    No copies are made.\n\n    - If the requested type is the current @ref basic_json, or a different @ref basic_json convertible\n    from the current @ref basic_json.\n\n    - Otherwise the value is converted by calling the @ref json_serializer<ValueType> `from_json()`\n    method.\n\n    @tparam ValueTypeCV the provided value type\n    @tparam ValueType the returned value type\n\n    @return copy of the JSON value, converted to @tparam ValueType if necessary\n\n    @throw what @ref json_serializer<ValueType> `from_json()` method throws if conversion is required\n\n    @since version 2.1.0\n    */\n    template < typename ValueTypeCV, typename ValueType = detail::uncvref_t<ValueTypeCV>>\n#if defined(JSON_HAS_CPP_14)\n    constexpr\n#endif\n    auto get() const noexcept(\n    noexcept(std::declval<const basic_json_t&>().template get_impl<ValueType>(detail::priority_tag<4> {})))\n    -> decltype(std::declval<const basic_json_t&>().template get_impl<ValueType>(detail::priority_tag<4> {}))\n    {\n        // we cannot static_assert on ValueTypeCV being non-const, because\n        // there is support for get<const basic_json_t>(), which is why we\n        // still need the uncvref\n        static_assert(!std::is_reference<ValueTypeCV>::value,\n                      \"get() cannot be used with reference types, you might want to use get_ref()\");\n        return get_impl<ValueType>(detail::priority_tag<4> {});\n    }\n\n    /*!\n    @brief get a pointer value (explicit)\n\n    Explicit pointer access to the internally stored JSON value. No copies are\n    made.\n\n    @warning The pointer becomes invalid if the underlying JSON object\n    changes.\n\n    @tparam PointerType pointer type; must be a pointer to @ref array_t, @ref\n    object_t, @ref string_t, @ref boolean_t, @ref number_integer_t,\n    @ref number_unsigned_t, or @ref number_float_t.\n\n    @return pointer to the internally stored JSON value if the requested\n    pointer type @a PointerType fits to the JSON value; `nullptr` otherwise\n\n    @complexity Constant.\n\n    @liveexample{The example below shows how pointers to internal values of a\n    JSON value can be requested. Note that no type conversions are made and a\n    `nullptr` is returned if the value and the requested pointer type does not\n    match.,get__PointerType}\n\n    @sa see @ref get_ptr() for explicit pointer-member access\n\n    @since version 1.0.0\n    */\n    template<typename PointerType, typename std::enable_if<\n                 std::is_pointer<PointerType>::value, int>::type = 0>\n    auto get() noexcept -> decltype(std::declval<basic_json_t&>().template get_ptr<PointerType>())\n    {\n        // delegate the call to get_ptr\n        return get_ptr<PointerType>();\n    }\n\n    /// @brief get a value (explicit)\n    /// @sa https://json.nlohmann.me/api/basic_json/get_to/\n    template < typename ValueType,\n               detail::enable_if_t <\n                   !detail::is_basic_json<ValueType>::value&&\n                   detail::has_from_json<basic_json_t, ValueType>::value,\n                   int > = 0 >\n    ValueType & get_to(ValueType& v) const noexcept(noexcept(\n                JSONSerializer<ValueType>::from_json(std::declval<const basic_json_t&>(), v)))\n    {\n        JSONSerializer<ValueType>::from_json(*this, v);\n        return v;\n    }\n\n    // specialization to allow calling get_to with a basic_json value\n    // see https://github.com/nlohmann/json/issues/2175\n    template<typename ValueType,\n             detail::enable_if_t <\n                 detail::is_basic_json<ValueType>::value,\n                 int> = 0>\n    ValueType & get_to(ValueType& v) const\n    {\n        v = *this;\n        return v;\n    }\n\n    template <\n        typename T, std::size_t N,\n        typename Array = T (&)[N], // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays)\n        detail::enable_if_t <\n            detail::has_from_json<basic_json_t, Array>::value, int > = 0 >\n    Array get_to(T (&v)[N]) const // NOLINT(cppcoreguidelines-avoid-c-arrays,hicpp-avoid-c-arrays,modernize-avoid-c-arrays)\n    noexcept(noexcept(JSONSerializer<Array>::from_json(\n                          std::declval<const basic_json_t&>(), v)))\n    {\n        JSONSerializer<Array>::from_json(*this, v);\n        return v;\n    }\n\n    /// @brief get a reference value (implicit)\n    /// @sa https://json.nlohmann.me/api/basic_json/get_ref/\n    template<typename ReferenceType, typename std::enable_if<\n                 std::is_reference<ReferenceType>::value, int>::type = 0>\n    ReferenceType get_ref()\n    {\n        // delegate call to get_ref_impl\n        return get_ref_impl<ReferenceType>(*this);\n    }\n\n    /// @brief get a reference value (implicit)\n    /// @sa https://json.nlohmann.me/api/basic_json/get_ref/\n    template < typename ReferenceType, typename std::enable_if <\n                   std::is_reference<ReferenceType>::value&&\n                   std::is_const<typename std::remove_reference<ReferenceType>::type>::value, int >::type = 0 >\n    ReferenceType get_ref() const\n    {\n        // delegate call to get_ref_impl\n        return get_ref_impl<ReferenceType>(*this);\n    }\n\n    /*!\n    @brief get a value (implicit)\n\n    Implicit type conversion between the JSON value and a compatible value.\n    The call is realized by calling @ref get() const.\n\n    @tparam ValueType non-pointer type compatible to the JSON value, for\n    instance `int` for JSON integer numbers, `bool` for JSON booleans, or\n    `std::vector` types for JSON arrays. The character type of @ref string_t\n    as well as an initializer list of this type is excluded to avoid\n    ambiguities as these types implicitly convert to `std::string`.\n\n    @return copy of the JSON value, converted to type @a ValueType\n\n    @throw type_error.302 in case passed type @a ValueType is incompatible\n    to the JSON value type (e.g., the JSON value is of type boolean, but a\n    string is requested); see example below\n\n    @complexity Linear in the size of the JSON value.\n\n    @liveexample{The example below shows several conversions from JSON values\n    to other types. There a few things to note: (1) Floating-point numbers can\n    be converted to integers\\, (2) A JSON array can be converted to a standard\n    `std::vector<short>`\\, (3) A JSON object can be converted to C++\n    associative containers such as `std::unordered_map<std::string\\,\n    json>`.,operator__ValueType}\n\n    @since version 1.0.0\n    */\n    template < typename ValueType, typename std::enable_if <\n                   detail::conjunction <\n                       detail::negation<std::is_pointer<ValueType>>,\n                       detail::negation<std::is_same<ValueType, std::nullptr_t>>,\n                       detail::negation<std::is_same<ValueType, detail::json_ref<basic_json>>>,\n                                        detail::negation<std::is_same<ValueType, typename string_t::value_type>>,\n                                        detail::negation<detail::is_basic_json<ValueType>>,\n                                        detail::negation<std::is_same<ValueType, std::initializer_list<typename string_t::value_type>>>,\n#if defined(JSON_HAS_CPP_17) && (defined(__GNUC__) || (defined(_MSC_VER) && _MSC_VER >= 1910 && _MSC_VER <= 1914))\n                                                detail::negation<std::is_same<ValueType, std::string_view>>,\n#endif\n#if defined(JSON_HAS_CPP_17)\n                                                detail::negation<std::is_same<ValueType, std::any>>,\n#endif\n                                                detail::is_detected_lazy<detail::get_template_function, const basic_json_t&, ValueType>\n                                                >::value, int >::type = 0 >\n                                        JSON_EXPLICIT operator ValueType() const\n    {\n        // delegate the call to get<>() const\n        return get<ValueType>();\n    }\n\n    /// @brief get a binary value\n    /// @sa https://json.nlohmann.me/api/basic_json/get_binary/\n    binary_t& get_binary()\n    {\n        if (!is_binary())\n        {\n            JSON_THROW(type_error::create(302, detail::concat(\"type must be binary, but is \", type_name()), this));\n        }\n\n        return *get_ptr<binary_t*>();\n    }\n\n    /// @brief get a binary value\n    /// @sa https://json.nlohmann.me/api/basic_json/get_binary/\n    const binary_t& get_binary() const\n    {\n        if (!is_binary())\n        {\n            JSON_THROW(type_error::create(302, detail::concat(\"type must be binary, but is \", type_name()), this));\n        }\n\n        return *get_ptr<const binary_t*>();\n    }\n\n    /// @}\n\n\n    ////////////////////\n    // element access //\n    ////////////////////\n\n    /// @name element access\n    /// Access to the JSON value.\n    /// @{\n\n    /// @brief access specified array element with bounds checking\n    /// @sa https://json.nlohmann.me/api/basic_json/at/\n    reference at(size_type idx)\n    {\n        // at only works for arrays\n        if (JSON_HEDLEY_LIKELY(is_array()))\n        {\n            JSON_TRY\n            {\n                return set_parent(m_value.array->at(idx));\n            }\n            JSON_CATCH (std::out_of_range&)\n            {\n                // create better exception explanation\n                JSON_THROW(out_of_range::create(401, detail::concat(\"array index \", std::to_string(idx), \" is out of range\"), this));\n            }\n        }\n        else\n        {\n            JSON_THROW(type_error::create(304, detail::concat(\"cannot use at() with \", type_name()), this));\n        }\n    }\n\n    /// @brief access specified array element with bounds checking\n    /// @sa https://json.nlohmann.me/api/basic_json/at/\n    const_reference at(size_type idx) const\n    {\n        // at only works for arrays\n        if (JSON_HEDLEY_LIKELY(is_array()))\n        {\n            JSON_TRY\n            {\n                return m_value.array->at(idx);\n            }\n            JSON_CATCH (std::out_of_range&)\n            {\n                // create better exception explanation\n                JSON_THROW(out_of_range::create(401, detail::concat(\"array index \", std::to_string(idx), \" is out of range\"), this));\n            }\n        }\n        else\n        {\n            JSON_THROW(type_error::create(304, detail::concat(\"cannot use at() with \", type_name()), this));\n        }\n    }\n\n    /// @brief access specified object element with bounds checking\n    /// @sa https://json.nlohmann.me/api/basic_json/at/\n    reference at(const typename object_t::key_type& key)\n    {\n        // at only works for objects\n        if (JSON_HEDLEY_UNLIKELY(!is_object()))\n        {\n            JSON_THROW(type_error::create(304, detail::concat(\"cannot use at() with \", type_name()), this));\n        }\n\n        auto it = m_value.object->find(key);\n        if (it == m_value.object->end())\n        {\n            JSON_THROW(out_of_range::create(403, detail::concat(\"key '\", key, \"' not found\"), this));\n        }\n        return set_parent(it->second);\n    }\n\n    /// @brief access specified object element with bounds checking\n    /// @sa https://json.nlohmann.me/api/basic_json/at/\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_basic_json_key_type<basic_json_t, KeyType>::value, int> = 0>\n    reference at(KeyType && key)\n    {\n        // at only works for objects\n        if (JSON_HEDLEY_UNLIKELY(!is_object()))\n        {\n            JSON_THROW(type_error::create(304, detail::concat(\"cannot use at() with \", type_name()), this));\n        }\n\n        auto it = m_value.object->find(std::forward<KeyType>(key));\n        if (it == m_value.object->end())\n        {\n            JSON_THROW(out_of_range::create(403, detail::concat(\"key '\", string_t(std::forward<KeyType>(key)), \"' not found\"), this));\n        }\n        return set_parent(it->second);\n    }\n\n    /// @brief access specified object element with bounds checking\n    /// @sa https://json.nlohmann.me/api/basic_json/at/\n    const_reference at(const typename object_t::key_type& key) const\n    {\n        // at only works for objects\n        if (JSON_HEDLEY_UNLIKELY(!is_object()))\n        {\n            JSON_THROW(type_error::create(304, detail::concat(\"cannot use at() with \", type_name()), this));\n        }\n\n        auto it = m_value.object->find(key);\n        if (it == m_value.object->end())\n        {\n            JSON_THROW(out_of_range::create(403, detail::concat(\"key '\", key, \"' not found\"), this));\n        }\n        return it->second;\n    }\n\n    /// @brief access specified object element with bounds checking\n    /// @sa https://json.nlohmann.me/api/basic_json/at/\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_basic_json_key_type<basic_json_t, KeyType>::value, int> = 0>\n    const_reference at(KeyType && key) const\n    {\n        // at only works for objects\n        if (JSON_HEDLEY_UNLIKELY(!is_object()))\n        {\n            JSON_THROW(type_error::create(304, detail::concat(\"cannot use at() with \", type_name()), this));\n        }\n\n        auto it = m_value.object->find(std::forward<KeyType>(key));\n        if (it == m_value.object->end())\n        {\n            JSON_THROW(out_of_range::create(403, detail::concat(\"key '\", string_t(std::forward<KeyType>(key)), \"' not found\"), this));\n        }\n        return it->second;\n    }\n\n    /// @brief access specified array element\n    /// @sa https://json.nlohmann.me/api/basic_json/operator%5B%5D/\n    reference operator[](size_type idx)\n    {\n        // implicitly convert null value to an empty array\n        if (is_null())\n        {\n            m_type = value_t::array;\n            m_value.array = create<array_t>();\n            assert_invariant();\n        }\n\n        // operator[] only works for arrays\n        if (JSON_HEDLEY_LIKELY(is_array()))\n        {\n            // fill up array with null values if given idx is outside range\n            if (idx >= m_value.array->size())\n            {\n#if JSON_DIAGNOSTICS\n                // remember array size & capacity before resizing\n                const auto old_size = m_value.array->size();\n                const auto old_capacity = m_value.array->capacity();\n#endif\n                m_value.array->resize(idx + 1);\n\n#if JSON_DIAGNOSTICS\n                if (JSON_HEDLEY_UNLIKELY(m_value.array->capacity() != old_capacity))\n                {\n                    // capacity has changed: update all parents\n                    set_parents();\n                }\n                else\n                {\n                    // set parent for values added above\n                    set_parents(begin() + static_cast<typename iterator::difference_type>(old_size), static_cast<typename iterator::difference_type>(idx + 1 - old_size));\n                }\n#endif\n                assert_invariant();\n            }\n\n            return m_value.array->operator[](idx);\n        }\n\n        JSON_THROW(type_error::create(305, detail::concat(\"cannot use operator[] with a numeric argument with \", type_name()), this));\n    }\n\n    /// @brief access specified array element\n    /// @sa https://json.nlohmann.me/api/basic_json/operator%5B%5D/\n    const_reference operator[](size_type idx) const\n    {\n        // const operator[] only works for arrays\n        if (JSON_HEDLEY_LIKELY(is_array()))\n        {\n            return m_value.array->operator[](idx);\n        }\n\n        JSON_THROW(type_error::create(305, detail::concat(\"cannot use operator[] with a numeric argument with \", type_name()), this));\n    }\n\n    /// @brief access specified object element\n    /// @sa https://json.nlohmann.me/api/basic_json/operator%5B%5D/\n    reference operator[](typename object_t::key_type key)\n    {\n        // implicitly convert null value to an empty object\n        if (is_null())\n        {\n            m_type = value_t::object;\n            m_value.object = create<object_t>();\n            assert_invariant();\n        }\n\n        // operator[] only works for objects\n        if (JSON_HEDLEY_LIKELY(is_object()))\n        {\n            auto result = m_value.object->emplace(std::move(key), nullptr);\n            return set_parent(result.first->second);\n        }\n\n        JSON_THROW(type_error::create(305, detail::concat(\"cannot use operator[] with a string argument with \", type_name()), this));\n    }\n\n    /// @brief access specified object element\n    /// @sa https://json.nlohmann.me/api/basic_json/operator%5B%5D/\n    const_reference operator[](const typename object_t::key_type& key) const\n    {\n        // const operator[] only works for objects\n        if (JSON_HEDLEY_LIKELY(is_object()))\n        {\n            auto it = m_value.object->find(key);\n            JSON_ASSERT(it != m_value.object->end());\n            return it->second;\n        }\n\n        JSON_THROW(type_error::create(305, detail::concat(\"cannot use operator[] with a string argument with \", type_name()), this));\n    }\n\n    // these two functions resolve a (const) char * ambiguity affecting Clang and MSVC\n    // (they seemingly cannot be constrained to resolve the ambiguity)\n    template<typename T>\n    reference operator[](T* key)\n    {\n        return operator[](typename object_t::key_type(key));\n    }\n\n    template<typename T>\n    const_reference operator[](T* key) const\n    {\n        return operator[](typename object_t::key_type(key));\n    }\n\n    /// @brief access specified object element\n    /// @sa https://json.nlohmann.me/api/basic_json/operator%5B%5D/\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_basic_json_key_type<basic_json_t, KeyType>::value, int > = 0 >\n    reference operator[](KeyType && key)\n    {\n        // implicitly convert null value to an empty object\n        if (is_null())\n        {\n            m_type = value_t::object;\n            m_value.object = create<object_t>();\n            assert_invariant();\n        }\n\n        // operator[] only works for objects\n        if (JSON_HEDLEY_LIKELY(is_object()))\n        {\n            auto result = m_value.object->emplace(std::forward<KeyType>(key), nullptr);\n            return set_parent(result.first->second);\n        }\n\n        JSON_THROW(type_error::create(305, detail::concat(\"cannot use operator[] with a string argument with \", type_name()), this));\n    }\n\n    /// @brief access specified object element\n    /// @sa https://json.nlohmann.me/api/basic_json/operator%5B%5D/\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_basic_json_key_type<basic_json_t, KeyType>::value, int > = 0 >\n    const_reference operator[](KeyType && key) const\n    {\n        // const operator[] only works for objects\n        if (JSON_HEDLEY_LIKELY(is_object()))\n        {\n            auto it = m_value.object->find(std::forward<KeyType>(key));\n            JSON_ASSERT(it != m_value.object->end());\n            return it->second;\n        }\n\n        JSON_THROW(type_error::create(305, detail::concat(\"cannot use operator[] with a string argument with \", type_name()), this));\n    }\n\n  private:\n    template<typename KeyType>\n    using is_comparable_with_object_key = detail::is_comparable <\n        object_comparator_t, const typename object_t::key_type&, KeyType >;\n\n    template<typename ValueType>\n    using value_return_type = std::conditional <\n        detail::is_c_string_uncvref<ValueType>::value,\n        string_t, typename std::decay<ValueType>::type >;\n\n  public:\n    /// @brief access specified object element with default value\n    /// @sa https://json.nlohmann.me/api/basic_json/value/\n    template < class ValueType, detail::enable_if_t <\n                   !detail::is_transparent<object_comparator_t>::value\n                   && detail::is_getable<basic_json_t, ValueType>::value\n                   && !std::is_same<value_t, detail::uncvref_t<ValueType>>::value, int > = 0 >\n    ValueType value(const typename object_t::key_type& key, const ValueType& default_value) const\n    {\n        // value only works for objects\n        if (JSON_HEDLEY_LIKELY(is_object()))\n        {\n            // if key is found, return value and given default value otherwise\n            const auto it = find(key);\n            if (it != end())\n            {\n                return it->template get<ValueType>();\n            }\n\n            return default_value;\n        }\n\n        JSON_THROW(type_error::create(306, detail::concat(\"cannot use value() with \", type_name()), this));\n    }\n\n    /// @brief access specified object element with default value\n    /// @sa https://json.nlohmann.me/api/basic_json/value/\n    template < class ValueType, class ReturnType = typename value_return_type<ValueType>::type,\n               detail::enable_if_t <\n                   !detail::is_transparent<object_comparator_t>::value\n                   && detail::is_getable<basic_json_t, ReturnType>::value\n                   && !std::is_same<value_t, detail::uncvref_t<ValueType>>::value, int > = 0 >\n    ReturnType value(const typename object_t::key_type& key, ValueType && default_value) const\n    {\n        // value only works for objects\n        if (JSON_HEDLEY_LIKELY(is_object()))\n        {\n            // if key is found, return value and given default value otherwise\n            const auto it = find(key);\n            if (it != end())\n            {\n                return it->template get<ReturnType>();\n            }\n\n            return std::forward<ValueType>(default_value);\n        }\n\n        JSON_THROW(type_error::create(306, detail::concat(\"cannot use value() with \", type_name()), this));\n    }\n\n    /// @brief access specified object element with default value\n    /// @sa https://json.nlohmann.me/api/basic_json/value/\n    template < class ValueType, class KeyType, detail::enable_if_t <\n                   detail::is_transparent<object_comparator_t>::value\n                   && !detail::is_json_pointer<KeyType>::value\n                   && is_comparable_with_object_key<KeyType>::value\n                   && detail::is_getable<basic_json_t, ValueType>::value\n                   && !std::is_same<value_t, detail::uncvref_t<ValueType>>::value, int > = 0 >\n    ValueType value(KeyType && key, const ValueType& default_value) const\n    {\n        // value only works for objects\n        if (JSON_HEDLEY_LIKELY(is_object()))\n        {\n            // if key is found, return value and given default value otherwise\n            const auto it = find(std::forward<KeyType>(key));\n            if (it != end())\n            {\n                return it->template get<ValueType>();\n            }\n\n            return default_value;\n        }\n\n        JSON_THROW(type_error::create(306, detail::concat(\"cannot use value() with \", type_name()), this));\n    }\n\n    /// @brief access specified object element via JSON Pointer with default value\n    /// @sa https://json.nlohmann.me/api/basic_json/value/\n    template < class ValueType, class KeyType, class ReturnType = typename value_return_type<ValueType>::type,\n               detail::enable_if_t <\n                   detail::is_transparent<object_comparator_t>::value\n                   && !detail::is_json_pointer<KeyType>::value\n                   && is_comparable_with_object_key<KeyType>::value\n                   && detail::is_getable<basic_json_t, ReturnType>::value\n                   && !std::is_same<value_t, detail::uncvref_t<ValueType>>::value, int > = 0 >\n    ReturnType value(KeyType && key, ValueType && default_value) const\n    {\n        // value only works for objects\n        if (JSON_HEDLEY_LIKELY(is_object()))\n        {\n            // if key is found, return value and given default value otherwise\n            const auto it = find(std::forward<KeyType>(key));\n            if (it != end())\n            {\n                return it->template get<ReturnType>();\n            }\n\n            return std::forward<ValueType>(default_value);\n        }\n\n        JSON_THROW(type_error::create(306, detail::concat(\"cannot use value() with \", type_name()), this));\n    }\n\n    /// @brief access specified object element via JSON Pointer with default value\n    /// @sa https://json.nlohmann.me/api/basic_json/value/\n    template < class ValueType, detail::enable_if_t <\n                   detail::is_getable<basic_json_t, ValueType>::value\n                   && !std::is_same<value_t, detail::uncvref_t<ValueType>>::value, int > = 0 >\n    ValueType value(const json_pointer& ptr, const ValueType& default_value) const\n    {\n        // value only works for objects\n        if (JSON_HEDLEY_LIKELY(is_object()))\n        {\n            // if pointer resolves a value, return it or use default value\n            JSON_TRY\n            {\n                return ptr.get_checked(this).template get<ValueType>();\n            }\n            JSON_INTERNAL_CATCH (out_of_range&)\n            {\n                return default_value;\n            }\n        }\n\n        JSON_THROW(type_error::create(306, detail::concat(\"cannot use value() with \", type_name()), this));\n    }\n\n    /// @brief access specified object element via JSON Pointer with default value\n    /// @sa https://json.nlohmann.me/api/basic_json/value/\n    template < class ValueType, class ReturnType = typename value_return_type<ValueType>::type,\n               detail::enable_if_t <\n                   detail::is_getable<basic_json_t, ReturnType>::value\n                   && !std::is_same<value_t, detail::uncvref_t<ValueType>>::value, int > = 0 >\n    ReturnType value(const json_pointer& ptr, ValueType && default_value) const\n    {\n        // value only works for objects\n        if (JSON_HEDLEY_LIKELY(is_object()))\n        {\n            // if pointer resolves a value, return it or use default value\n            JSON_TRY\n            {\n                return ptr.get_checked(this).template get<ReturnType>();\n            }\n            JSON_INTERNAL_CATCH (out_of_range&)\n            {\n                return std::forward<ValueType>(default_value);\n            }\n        }\n\n        JSON_THROW(type_error::create(306, detail::concat(\"cannot use value() with \", type_name()), this));\n    }\n\n    template < class ValueType, class BasicJsonType, detail::enable_if_t <\n                   detail::is_basic_json<BasicJsonType>::value\n                   && detail::is_getable<basic_json_t, ValueType>::value\n                   && !std::is_same<value_t, detail::uncvref_t<ValueType>>::value, int > = 0 >\n    JSON_HEDLEY_DEPRECATED_FOR(3.11.0, basic_json::json_pointer or nlohmann::json_pointer<basic_json::string_t>) // NOLINT(readability/alt_tokens)\n    ValueType value(const ::nlohmann::json_pointer<BasicJsonType>& ptr, const ValueType& default_value) const\n    {\n        return value(ptr.convert(), default_value);\n    }\n\n    template < class ValueType, class BasicJsonType, class ReturnType = typename value_return_type<ValueType>::type,\n               detail::enable_if_t <\n                   detail::is_basic_json<BasicJsonType>::value\n                   && detail::is_getable<basic_json_t, ReturnType>::value\n                   && !std::is_same<value_t, detail::uncvref_t<ValueType>>::value, int > = 0 >\n    JSON_HEDLEY_DEPRECATED_FOR(3.11.0, basic_json::json_pointer or nlohmann::json_pointer<basic_json::string_t>) // NOLINT(readability/alt_tokens)\n    ReturnType value(const ::nlohmann::json_pointer<BasicJsonType>& ptr, ValueType && default_value) const\n    {\n        return value(ptr.convert(), std::forward<ValueType>(default_value));\n    }\n\n    /// @brief access the first element\n    /// @sa https://json.nlohmann.me/api/basic_json/front/\n    reference front()\n    {\n        return *begin();\n    }\n\n    /// @brief access the first element\n    /// @sa https://json.nlohmann.me/api/basic_json/front/\n    const_reference front() const\n    {\n        return *cbegin();\n    }\n\n    /// @brief access the last element\n    /// @sa https://json.nlohmann.me/api/basic_json/back/\n    reference back()\n    {\n        auto tmp = end();\n        --tmp;\n        return *tmp;\n    }\n\n    /// @brief access the last element\n    /// @sa https://json.nlohmann.me/api/basic_json/back/\n    const_reference back() const\n    {\n        auto tmp = cend();\n        --tmp;\n        return *tmp;\n    }\n\n    /// @brief remove element given an iterator\n    /// @sa https://json.nlohmann.me/api/basic_json/erase/\n    template < class IteratorType, detail::enable_if_t <\n                   std::is_same<IteratorType, typename basic_json_t::iterator>::value ||\n                   std::is_same<IteratorType, typename basic_json_t::const_iterator>::value, int > = 0 >\n    IteratorType erase(IteratorType pos)\n    {\n        // make sure iterator fits the current value\n        if (JSON_HEDLEY_UNLIKELY(this != pos.m_object))\n        {\n            JSON_THROW(invalid_iterator::create(202, \"iterator does not fit current value\", this));\n        }\n\n        IteratorType result = end();\n\n        switch (m_type)\n        {\n            case value_t::boolean:\n            case value_t::number_float:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::string:\n            case value_t::binary:\n            {\n                if (JSON_HEDLEY_UNLIKELY(!pos.m_it.primitive_iterator.is_begin()))\n                {\n                    JSON_THROW(invalid_iterator::create(205, \"iterator out of range\", this));\n                }\n\n                if (is_string())\n                {\n                    AllocatorType<string_t> alloc;\n                    std::allocator_traits<decltype(alloc)>::destroy(alloc, m_value.string);\n                    std::allocator_traits<decltype(alloc)>::deallocate(alloc, m_value.string, 1);\n                    m_value.string = nullptr;\n                }\n                else if (is_binary())\n                {\n                    AllocatorType<binary_t> alloc;\n                    std::allocator_traits<decltype(alloc)>::destroy(alloc, m_value.binary);\n                    std::allocator_traits<decltype(alloc)>::deallocate(alloc, m_value.binary, 1);\n                    m_value.binary = nullptr;\n                }\n\n                m_type = value_t::null;\n                assert_invariant();\n                break;\n            }\n\n            case value_t::object:\n            {\n                result.m_it.object_iterator = m_value.object->erase(pos.m_it.object_iterator);\n                break;\n            }\n\n            case value_t::array:\n            {\n                result.m_it.array_iterator = m_value.array->erase(pos.m_it.array_iterator);\n                break;\n            }\n\n            case value_t::null:\n            case value_t::discarded:\n            default:\n                JSON_THROW(type_error::create(307, detail::concat(\"cannot use erase() with \", type_name()), this));\n        }\n\n        return result;\n    }\n\n    /// @brief remove elements given an iterator range\n    /// @sa https://json.nlohmann.me/api/basic_json/erase/\n    template < class IteratorType, detail::enable_if_t <\n                   std::is_same<IteratorType, typename basic_json_t::iterator>::value ||\n                   std::is_same<IteratorType, typename basic_json_t::const_iterator>::value, int > = 0 >\n    IteratorType erase(IteratorType first, IteratorType last)\n    {\n        // make sure iterator fits the current value\n        if (JSON_HEDLEY_UNLIKELY(this != first.m_object || this != last.m_object))\n        {\n            JSON_THROW(invalid_iterator::create(203, \"iterators do not fit current value\", this));\n        }\n\n        IteratorType result = end();\n\n        switch (m_type)\n        {\n            case value_t::boolean:\n            case value_t::number_float:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::string:\n            case value_t::binary:\n            {\n                if (JSON_HEDLEY_LIKELY(!first.m_it.primitive_iterator.is_begin()\n                                       || !last.m_it.primitive_iterator.is_end()))\n                {\n                    JSON_THROW(invalid_iterator::create(204, \"iterators out of range\", this));\n                }\n\n                if (is_string())\n                {\n                    AllocatorType<string_t> alloc;\n                    std::allocator_traits<decltype(alloc)>::destroy(alloc, m_value.string);\n                    std::allocator_traits<decltype(alloc)>::deallocate(alloc, m_value.string, 1);\n                    m_value.string = nullptr;\n                }\n                else if (is_binary())\n                {\n                    AllocatorType<binary_t> alloc;\n                    std::allocator_traits<decltype(alloc)>::destroy(alloc, m_value.binary);\n                    std::allocator_traits<decltype(alloc)>::deallocate(alloc, m_value.binary, 1);\n                    m_value.binary = nullptr;\n                }\n\n                m_type = value_t::null;\n                assert_invariant();\n                break;\n            }\n\n            case value_t::object:\n            {\n                result.m_it.object_iterator = m_value.object->erase(first.m_it.object_iterator,\n                                              last.m_it.object_iterator);\n                break;\n            }\n\n            case value_t::array:\n            {\n                result.m_it.array_iterator = m_value.array->erase(first.m_it.array_iterator,\n                                             last.m_it.array_iterator);\n                break;\n            }\n\n            case value_t::null:\n            case value_t::discarded:\n            default:\n                JSON_THROW(type_error::create(307, detail::concat(\"cannot use erase() with \", type_name()), this));\n        }\n\n        return result;\n    }\n\n  private:\n    template < typename KeyType, detail::enable_if_t <\n                   detail::has_erase_with_key_type<basic_json_t, KeyType>::value, int > = 0 >\n    size_type erase_internal(KeyType && key)\n    {\n        // this erase only works for objects\n        if (JSON_HEDLEY_UNLIKELY(!is_object()))\n        {\n            JSON_THROW(type_error::create(307, detail::concat(\"cannot use erase() with \", type_name()), this));\n        }\n\n        return m_value.object->erase(std::forward<KeyType>(key));\n    }\n\n    template < typename KeyType, detail::enable_if_t <\n                   !detail::has_erase_with_key_type<basic_json_t, KeyType>::value, int > = 0 >\n    size_type erase_internal(KeyType && key)\n    {\n        // this erase only works for objects\n        if (JSON_HEDLEY_UNLIKELY(!is_object()))\n        {\n            JSON_THROW(type_error::create(307, detail::concat(\"cannot use erase() with \", type_name()), this));\n        }\n\n        const auto it = m_value.object->find(std::forward<KeyType>(key));\n        if (it != m_value.object->end())\n        {\n            m_value.object->erase(it);\n            return 1;\n        }\n        return 0;\n    }\n\n  public:\n\n    /// @brief remove element from a JSON object given a key\n    /// @sa https://json.nlohmann.me/api/basic_json/erase/\n    size_type erase(const typename object_t::key_type& key)\n    {\n        // the indirection via erase_internal() is added to avoid making this\n        // function a template and thus de-rank it during overload resolution\n        return erase_internal(key);\n    }\n\n    /// @brief remove element from a JSON object given a key\n    /// @sa https://json.nlohmann.me/api/basic_json/erase/\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_basic_json_key_type<basic_json_t, KeyType>::value, int> = 0>\n    size_type erase(KeyType && key)\n    {\n        return erase_internal(std::forward<KeyType>(key));\n    }\n\n    /// @brief remove element from a JSON array given an index\n    /// @sa https://json.nlohmann.me/api/basic_json/erase/\n    void erase(const size_type idx)\n    {\n        // this erase only works for arrays\n        if (JSON_HEDLEY_LIKELY(is_array()))\n        {\n            if (JSON_HEDLEY_UNLIKELY(idx >= size()))\n            {\n                JSON_THROW(out_of_range::create(401, detail::concat(\"array index \", std::to_string(idx), \" is out of range\"), this));\n            }\n\n            m_value.array->erase(m_value.array->begin() + static_cast<difference_type>(idx));\n        }\n        else\n        {\n            JSON_THROW(type_error::create(307, detail::concat(\"cannot use erase() with \", type_name()), this));\n        }\n    }\n\n    /// @}\n\n\n    ////////////\n    // lookup //\n    ////////////\n\n    /// @name lookup\n    /// @{\n\n    /// @brief find an element in a JSON object\n    /// @sa https://json.nlohmann.me/api/basic_json/find/\n    iterator find(const typename object_t::key_type& key)\n    {\n        auto result = end();\n\n        if (is_object())\n        {\n            result.m_it.object_iterator = m_value.object->find(key);\n        }\n\n        return result;\n    }\n\n    /// @brief find an element in a JSON object\n    /// @sa https://json.nlohmann.me/api/basic_json/find/\n    const_iterator find(const typename object_t::key_type& key) const\n    {\n        auto result = cend();\n\n        if (is_object())\n        {\n            result.m_it.object_iterator = m_value.object->find(key);\n        }\n\n        return result;\n    }\n\n    /// @brief find an element in a JSON object\n    /// @sa https://json.nlohmann.me/api/basic_json/find/\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_basic_json_key_type<basic_json_t, KeyType>::value, int> = 0>\n    iterator find(KeyType && key)\n    {\n        auto result = end();\n\n        if (is_object())\n        {\n            result.m_it.object_iterator = m_value.object->find(std::forward<KeyType>(key));\n        }\n\n        return result;\n    }\n\n    /// @brief find an element in a JSON object\n    /// @sa https://json.nlohmann.me/api/basic_json/find/\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_basic_json_key_type<basic_json_t, KeyType>::value, int> = 0>\n    const_iterator find(KeyType && key) const\n    {\n        auto result = cend();\n\n        if (is_object())\n        {\n            result.m_it.object_iterator = m_value.object->find(std::forward<KeyType>(key));\n        }\n\n        return result;\n    }\n\n    /// @brief returns the number of occurrences of a key in a JSON object\n    /// @sa https://json.nlohmann.me/api/basic_json/count/\n    size_type count(const typename object_t::key_type& key) const\n    {\n        // return 0 for all nonobject types\n        return is_object() ? m_value.object->count(key) : 0;\n    }\n\n    /// @brief returns the number of occurrences of a key in a JSON object\n    /// @sa https://json.nlohmann.me/api/basic_json/count/\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_basic_json_key_type<basic_json_t, KeyType>::value, int> = 0>\n    size_type count(KeyType && key) const\n    {\n        // return 0 for all nonobject types\n        return is_object() ? m_value.object->count(std::forward<KeyType>(key)) : 0;\n    }\n\n    /// @brief check the existence of an element in a JSON object\n    /// @sa https://json.nlohmann.me/api/basic_json/contains/\n    bool contains(const typename object_t::key_type& key) const\n    {\n        return is_object() && m_value.object->find(key) != m_value.object->end();\n    }\n\n    /// @brief check the existence of an element in a JSON object\n    /// @sa https://json.nlohmann.me/api/basic_json/contains/\n    template<class KeyType, detail::enable_if_t<\n                 detail::is_usable_as_basic_json_key_type<basic_json_t, KeyType>::value, int> = 0>\n    bool contains(KeyType && key) const\n    {\n        return is_object() && m_value.object->find(std::forward<KeyType>(key)) != m_value.object->end();\n    }\n\n    /// @brief check the existence of an element in a JSON object given a JSON pointer\n    /// @sa https://json.nlohmann.me/api/basic_json/contains/\n    bool contains(const json_pointer& ptr) const\n    {\n        return ptr.contains(this);\n    }\n\n    template<typename BasicJsonType, detail::enable_if_t<detail::is_basic_json<BasicJsonType>::value, int> = 0>\n    JSON_HEDLEY_DEPRECATED_FOR(3.11.0, basic_json::json_pointer or nlohmann::json_pointer<basic_json::string_t>) // NOLINT(readability/alt_tokens)\n    bool contains(const typename ::nlohmann::json_pointer<BasicJsonType>& ptr) const\n    {\n        return ptr.contains(this);\n    }\n\n    /// @}\n\n\n    ///////////////\n    // iterators //\n    ///////////////\n\n    /// @name iterators\n    /// @{\n\n    /// @brief returns an iterator to the first element\n    /// @sa https://json.nlohmann.me/api/basic_json/begin/\n    iterator begin() noexcept\n    {\n        iterator result(this);\n        result.set_begin();\n        return result;\n    }\n\n    /// @brief returns an iterator to the first element\n    /// @sa https://json.nlohmann.me/api/basic_json/begin/\n    const_iterator begin() const noexcept\n    {\n        return cbegin();\n    }\n\n    /// @brief returns a const iterator to the first element\n    /// @sa https://json.nlohmann.me/api/basic_json/cbegin/\n    const_iterator cbegin() const noexcept\n    {\n        const_iterator result(this);\n        result.set_begin();\n        return result;\n    }\n\n    /// @brief returns an iterator to one past the last element\n    /// @sa https://json.nlohmann.me/api/basic_json/end/\n    iterator end() noexcept\n    {\n        iterator result(this);\n        result.set_end();\n        return result;\n    }\n\n    /// @brief returns an iterator to one past the last element\n    /// @sa https://json.nlohmann.me/api/basic_json/end/\n    const_iterator end() const noexcept\n    {\n        return cend();\n    }\n\n    /// @brief returns an iterator to one past the last element\n    /// @sa https://json.nlohmann.me/api/basic_json/cend/\n    const_iterator cend() const noexcept\n    {\n        const_iterator result(this);\n        result.set_end();\n        return result;\n    }\n\n    /// @brief returns an iterator to the reverse-beginning\n    /// @sa https://json.nlohmann.me/api/basic_json/rbegin/\n    reverse_iterator rbegin() noexcept\n    {\n        return reverse_iterator(end());\n    }\n\n    /// @brief returns an iterator to the reverse-beginning\n    /// @sa https://json.nlohmann.me/api/basic_json/rbegin/\n    const_reverse_iterator rbegin() const noexcept\n    {\n        return crbegin();\n    }\n\n    /// @brief returns an iterator to the reverse-end\n    /// @sa https://json.nlohmann.me/api/basic_json/rend/\n    reverse_iterator rend() noexcept\n    {\n        return reverse_iterator(begin());\n    }\n\n    /// @brief returns an iterator to the reverse-end\n    /// @sa https://json.nlohmann.me/api/basic_json/rend/\n    const_reverse_iterator rend() const noexcept\n    {\n        return crend();\n    }\n\n    /// @brief returns a const reverse iterator to the last element\n    /// @sa https://json.nlohmann.me/api/basic_json/crbegin/\n    const_reverse_iterator crbegin() const noexcept\n    {\n        return const_reverse_iterator(cend());\n    }\n\n    /// @brief returns a const reverse iterator to one before the first\n    /// @sa https://json.nlohmann.me/api/basic_json/crend/\n    const_reverse_iterator crend() const noexcept\n    {\n        return const_reverse_iterator(cbegin());\n    }\n\n  public:\n    /// @brief wrapper to access iterator member functions in range-based for\n    /// @sa https://json.nlohmann.me/api/basic_json/items/\n    /// @deprecated This function is deprecated since 3.1.0 and will be removed in\n    ///             version 4.0.0 of the library. Please use @ref items() instead;\n    ///             that is, replace `json::iterator_wrapper(j)` with `j.items()`.\n    JSON_HEDLEY_DEPRECATED_FOR(3.1.0, items())\n    static iteration_proxy<iterator> iterator_wrapper(reference ref) noexcept\n    {\n        return ref.items();\n    }\n\n    /// @brief wrapper to access iterator member functions in range-based for\n    /// @sa https://json.nlohmann.me/api/basic_json/items/\n    /// @deprecated This function is deprecated since 3.1.0 and will be removed in\n    ///         version 4.0.0 of the library. Please use @ref items() instead;\n    ///         that is, replace `json::iterator_wrapper(j)` with `j.items()`.\n    JSON_HEDLEY_DEPRECATED_FOR(3.1.0, items())\n    static iteration_proxy<const_iterator> iterator_wrapper(const_reference ref) noexcept\n    {\n        return ref.items();\n    }\n\n    /// @brief helper to access iterator member functions in range-based for\n    /// @sa https://json.nlohmann.me/api/basic_json/items/\n    iteration_proxy<iterator> items() noexcept\n    {\n        return iteration_proxy<iterator>(*this);\n    }\n\n    /// @brief helper to access iterator member functions in range-based for\n    /// @sa https://json.nlohmann.me/api/basic_json/items/\n    iteration_proxy<const_iterator> items() const noexcept\n    {\n        return iteration_proxy<const_iterator>(*this);\n    }\n\n    /// @}\n\n\n    //////////////\n    // capacity //\n    //////////////\n\n    /// @name capacity\n    /// @{\n\n    /// @brief checks whether the container is empty.\n    /// @sa https://json.nlohmann.me/api/basic_json/empty/\n    bool empty() const noexcept\n    {\n        switch (m_type)\n        {\n            case value_t::null:\n            {\n                // null values are empty\n                return true;\n            }\n\n            case value_t::array:\n            {\n                // delegate call to array_t::empty()\n                return m_value.array->empty();\n            }\n\n            case value_t::object:\n            {\n                // delegate call to object_t::empty()\n                return m_value.object->empty();\n            }\n\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                // all other types are nonempty\n                return false;\n            }\n        }\n    }\n\n    /// @brief returns the number of elements\n    /// @sa https://json.nlohmann.me/api/basic_json/size/\n    size_type size() const noexcept\n    {\n        switch (m_type)\n        {\n            case value_t::null:\n            {\n                // null values are empty\n                return 0;\n            }\n\n            case value_t::array:\n            {\n                // delegate call to array_t::size()\n                return m_value.array->size();\n            }\n\n            case value_t::object:\n            {\n                // delegate call to object_t::size()\n                return m_value.object->size();\n            }\n\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                // all other types have size 1\n                return 1;\n            }\n        }\n    }\n\n    /// @brief returns the maximum possible number of elements\n    /// @sa https://json.nlohmann.me/api/basic_json/max_size/\n    size_type max_size() const noexcept\n    {\n        switch (m_type)\n        {\n            case value_t::array:\n            {\n                // delegate call to array_t::max_size()\n                return m_value.array->max_size();\n            }\n\n            case value_t::object:\n            {\n                // delegate call to object_t::max_size()\n                return m_value.object->max_size();\n            }\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                // all other types have max_size() == size()\n                return size();\n            }\n        }\n    }\n\n    /// @}\n\n\n    ///////////////\n    // modifiers //\n    ///////////////\n\n    /// @name modifiers\n    /// @{\n\n    /// @brief clears the contents\n    /// @sa https://json.nlohmann.me/api/basic_json/clear/\n    void clear() noexcept\n    {\n        switch (m_type)\n        {\n            case value_t::number_integer:\n            {\n                m_value.number_integer = 0;\n                break;\n            }\n\n            case value_t::number_unsigned:\n            {\n                m_value.number_unsigned = 0;\n                break;\n            }\n\n            case value_t::number_float:\n            {\n                m_value.number_float = 0.0;\n                break;\n            }\n\n            case value_t::boolean:\n            {\n                m_value.boolean = false;\n                break;\n            }\n\n            case value_t::string:\n            {\n                m_value.string->clear();\n                break;\n            }\n\n            case value_t::binary:\n            {\n                m_value.binary->clear();\n                break;\n            }\n\n            case value_t::array:\n            {\n                m_value.array->clear();\n                break;\n            }\n\n            case value_t::object:\n            {\n                m_value.object->clear();\n                break;\n            }\n\n            case value_t::null:\n            case value_t::discarded:\n            default:\n                break;\n        }\n    }\n\n    /// @brief add an object to an array\n    /// @sa https://json.nlohmann.me/api/basic_json/push_back/\n    void push_back(basic_json&& val)\n    {\n        // push_back only works for null objects or arrays\n        if (JSON_HEDLEY_UNLIKELY(!(is_null() || is_array())))\n        {\n            JSON_THROW(type_error::create(308, detail::concat(\"cannot use push_back() with \", type_name()), this));\n        }\n\n        // transform null object into an array\n        if (is_null())\n        {\n            m_type = value_t::array;\n            m_value = value_t::array;\n            assert_invariant();\n        }\n\n        // add element to array (move semantics)\n        const auto old_capacity = m_value.array->capacity();\n        m_value.array->push_back(std::move(val));\n        set_parent(m_value.array->back(), old_capacity);\n        // if val is moved from, basic_json move constructor marks it null, so we do not call the destructor\n    }\n\n    /// @brief add an object to an array\n    /// @sa https://json.nlohmann.me/api/basic_json/operator+=/\n    reference operator+=(basic_json&& val)\n    {\n        push_back(std::move(val));\n        return *this;\n    }\n\n    /// @brief add an object to an array\n    /// @sa https://json.nlohmann.me/api/basic_json/push_back/\n    void push_back(const basic_json& val)\n    {\n        // push_back only works for null objects or arrays\n        if (JSON_HEDLEY_UNLIKELY(!(is_null() || is_array())))\n        {\n            JSON_THROW(type_error::create(308, detail::concat(\"cannot use push_back() with \", type_name()), this));\n        }\n\n        // transform null object into an array\n        if (is_null())\n        {\n            m_type = value_t::array;\n            m_value = value_t::array;\n            assert_invariant();\n        }\n\n        // add element to array\n        const auto old_capacity = m_value.array->capacity();\n        m_value.array->push_back(val);\n        set_parent(m_value.array->back(), old_capacity);\n    }\n\n    /// @brief add an object to an array\n    /// @sa https://json.nlohmann.me/api/basic_json/operator+=/\n    reference operator+=(const basic_json& val)\n    {\n        push_back(val);\n        return *this;\n    }\n\n    /// @brief add an object to an object\n    /// @sa https://json.nlohmann.me/api/basic_json/push_back/\n    void push_back(const typename object_t::value_type& val)\n    {\n        // push_back only works for null objects or objects\n        if (JSON_HEDLEY_UNLIKELY(!(is_null() || is_object())))\n        {\n            JSON_THROW(type_error::create(308, detail::concat(\"cannot use push_back() with \", type_name()), this));\n        }\n\n        // transform null object into an object\n        if (is_null())\n        {\n            m_type = value_t::object;\n            m_value = value_t::object;\n            assert_invariant();\n        }\n\n        // add element to object\n        auto res = m_value.object->insert(val);\n        set_parent(res.first->second);\n    }\n\n    /// @brief add an object to an object\n    /// @sa https://json.nlohmann.me/api/basic_json/operator+=/\n    reference operator+=(const typename object_t::value_type& val)\n    {\n        push_back(val);\n        return *this;\n    }\n\n    /// @brief add an object to an object\n    /// @sa https://json.nlohmann.me/api/basic_json/push_back/\n    void push_back(initializer_list_t init)\n    {\n        if (is_object() && init.size() == 2 && (*init.begin())->is_string())\n        {\n            basic_json&& key = init.begin()->moved_or_copied();\n            push_back(typename object_t::value_type(\n                          std::move(key.get_ref<string_t&>()), (init.begin() + 1)->moved_or_copied()));\n        }\n        else\n        {\n            push_back(basic_json(init));\n        }\n    }\n\n    /// @brief add an object to an object\n    /// @sa https://json.nlohmann.me/api/basic_json/operator+=/\n    reference operator+=(initializer_list_t init)\n    {\n        push_back(init);\n        return *this;\n    }\n\n    /// @brief add an object to an array\n    /// @sa https://json.nlohmann.me/api/basic_json/emplace_back/\n    template<class... Args>\n    reference emplace_back(Args&& ... args)\n    {\n        // emplace_back only works for null objects or arrays\n        if (JSON_HEDLEY_UNLIKELY(!(is_null() || is_array())))\n        {\n            JSON_THROW(type_error::create(311, detail::concat(\"cannot use emplace_back() with \", type_name()), this));\n        }\n\n        // transform null object into an array\n        if (is_null())\n        {\n            m_type = value_t::array;\n            m_value = value_t::array;\n            assert_invariant();\n        }\n\n        // add element to array (perfect forwarding)\n        const auto old_capacity = m_value.array->capacity();\n        m_value.array->emplace_back(std::forward<Args>(args)...);\n        return set_parent(m_value.array->back(), old_capacity);\n    }\n\n    /// @brief add an object to an object if key does not exist\n    /// @sa https://json.nlohmann.me/api/basic_json/emplace/\n    template<class... Args>\n    std::pair<iterator, bool> emplace(Args&& ... args)\n    {\n        // emplace only works for null objects or arrays\n        if (JSON_HEDLEY_UNLIKELY(!(is_null() || is_object())))\n        {\n            JSON_THROW(type_error::create(311, detail::concat(\"cannot use emplace() with \", type_name()), this));\n        }\n\n        // transform null object into an object\n        if (is_null())\n        {\n            m_type = value_t::object;\n            m_value = value_t::object;\n            assert_invariant();\n        }\n\n        // add element to array (perfect forwarding)\n        auto res = m_value.object->emplace(std::forward<Args>(args)...);\n        set_parent(res.first->second);\n\n        // create result iterator and set iterator to the result of emplace\n        auto it = begin();\n        it.m_it.object_iterator = res.first;\n\n        // return pair of iterator and boolean\n        return {it, res.second};\n    }\n\n    /// Helper for insertion of an iterator\n    /// @note: This uses std::distance to support GCC 4.8,\n    ///        see https://github.com/nlohmann/json/pull/1257\n    template<typename... Args>\n    iterator insert_iterator(const_iterator pos, Args&& ... args)\n    {\n        iterator result(this);\n        JSON_ASSERT(m_value.array != nullptr);\n\n        auto insert_pos = std::distance(m_value.array->begin(), pos.m_it.array_iterator);\n        m_value.array->insert(pos.m_it.array_iterator, std::forward<Args>(args)...);\n        result.m_it.array_iterator = m_value.array->begin() + insert_pos;\n\n        // This could have been written as:\n        // result.m_it.array_iterator = m_value.array->insert(pos.m_it.array_iterator, cnt, val);\n        // but the return value of insert is missing in GCC 4.8, so it is written this way instead.\n\n        set_parents();\n        return result;\n    }\n\n    /// @brief inserts element into array\n    /// @sa https://json.nlohmann.me/api/basic_json/insert/\n    iterator insert(const_iterator pos, const basic_json& val)\n    {\n        // insert only works for arrays\n        if (JSON_HEDLEY_LIKELY(is_array()))\n        {\n            // check if iterator pos fits to this JSON value\n            if (JSON_HEDLEY_UNLIKELY(pos.m_object != this))\n            {\n                JSON_THROW(invalid_iterator::create(202, \"iterator does not fit current value\", this));\n            }\n\n            // insert to array and return iterator\n            return insert_iterator(pos, val);\n        }\n\n        JSON_THROW(type_error::create(309, detail::concat(\"cannot use insert() with \", type_name()), this));\n    }\n\n    /// @brief inserts element into array\n    /// @sa https://json.nlohmann.me/api/basic_json/insert/\n    iterator insert(const_iterator pos, basic_json&& val)\n    {\n        return insert(pos, val);\n    }\n\n    /// @brief inserts copies of element into array\n    /// @sa https://json.nlohmann.me/api/basic_json/insert/\n    iterator insert(const_iterator pos, size_type cnt, const basic_json& val)\n    {\n        // insert only works for arrays\n        if (JSON_HEDLEY_LIKELY(is_array()))\n        {\n            // check if iterator pos fits to this JSON value\n            if (JSON_HEDLEY_UNLIKELY(pos.m_object != this))\n            {\n                JSON_THROW(invalid_iterator::create(202, \"iterator does not fit current value\", this));\n            }\n\n            // insert to array and return iterator\n            return insert_iterator(pos, cnt, val);\n        }\n\n        JSON_THROW(type_error::create(309, detail::concat(\"cannot use insert() with \", type_name()), this));\n    }\n\n    /// @brief inserts range of elements into array\n    /// @sa https://json.nlohmann.me/api/basic_json/insert/\n    iterator insert(const_iterator pos, const_iterator first, const_iterator last)\n    {\n        // insert only works for arrays\n        if (JSON_HEDLEY_UNLIKELY(!is_array()))\n        {\n            JSON_THROW(type_error::create(309, detail::concat(\"cannot use insert() with \", type_name()), this));\n        }\n\n        // check if iterator pos fits to this JSON value\n        if (JSON_HEDLEY_UNLIKELY(pos.m_object != this))\n        {\n            JSON_THROW(invalid_iterator::create(202, \"iterator does not fit current value\", this));\n        }\n\n        // check if range iterators belong to the same JSON object\n        if (JSON_HEDLEY_UNLIKELY(first.m_object != last.m_object))\n        {\n            JSON_THROW(invalid_iterator::create(210, \"iterators do not fit\", this));\n        }\n\n        if (JSON_HEDLEY_UNLIKELY(first.m_object == this))\n        {\n            JSON_THROW(invalid_iterator::create(211, \"passed iterators may not belong to container\", this));\n        }\n\n        // insert to array and return iterator\n        return insert_iterator(pos, first.m_it.array_iterator, last.m_it.array_iterator);\n    }\n\n    /// @brief inserts elements from initializer list into array\n    /// @sa https://json.nlohmann.me/api/basic_json/insert/\n    iterator insert(const_iterator pos, initializer_list_t ilist)\n    {\n        // insert only works for arrays\n        if (JSON_HEDLEY_UNLIKELY(!is_array()))\n        {\n            JSON_THROW(type_error::create(309, detail::concat(\"cannot use insert() with \", type_name()), this));\n        }\n\n        // check if iterator pos fits to this JSON value\n        if (JSON_HEDLEY_UNLIKELY(pos.m_object != this))\n        {\n            JSON_THROW(invalid_iterator::create(202, \"iterator does not fit current value\", this));\n        }\n\n        // insert to array and return iterator\n        return insert_iterator(pos, ilist.begin(), ilist.end());\n    }\n\n    /// @brief inserts range of elements into object\n    /// @sa https://json.nlohmann.me/api/basic_json/insert/\n    void insert(const_iterator first, const_iterator last)\n    {\n        // insert only works for objects\n        if (JSON_HEDLEY_UNLIKELY(!is_object()))\n        {\n            JSON_THROW(type_error::create(309, detail::concat(\"cannot use insert() with \", type_name()), this));\n        }\n\n        // check if range iterators belong to the same JSON object\n        if (JSON_HEDLEY_UNLIKELY(first.m_object != last.m_object))\n        {\n            JSON_THROW(invalid_iterator::create(210, \"iterators do not fit\", this));\n        }\n\n        // passed iterators must belong to objects\n        if (JSON_HEDLEY_UNLIKELY(!first.m_object->is_object()))\n        {\n            JSON_THROW(invalid_iterator::create(202, \"iterators first and last must point to objects\", this));\n        }\n\n        m_value.object->insert(first.m_it.object_iterator, last.m_it.object_iterator);\n    }\n\n    /// @brief updates a JSON object from another object, overwriting existing keys\n    /// @sa https://json.nlohmann.me/api/basic_json/update/\n    void update(const_reference j, bool merge_objects = false)\n    {\n        update(j.begin(), j.end(), merge_objects);\n    }\n\n    /// @brief updates a JSON object from another object, overwriting existing keys\n    /// @sa https://json.nlohmann.me/api/basic_json/update/\n    void update(const_iterator first, const_iterator last, bool merge_objects = false)\n    {\n        // implicitly convert null value to an empty object\n        if (is_null())\n        {\n            m_type = value_t::object;\n            m_value.object = create<object_t>();\n            assert_invariant();\n        }\n\n        if (JSON_HEDLEY_UNLIKELY(!is_object()))\n        {\n            JSON_THROW(type_error::create(312, detail::concat(\"cannot use update() with \", type_name()), this));\n        }\n\n        // check if range iterators belong to the same JSON object\n        if (JSON_HEDLEY_UNLIKELY(first.m_object != last.m_object))\n        {\n            JSON_THROW(invalid_iterator::create(210, \"iterators do not fit\", this));\n        }\n\n        // passed iterators must belong to objects\n        if (JSON_HEDLEY_UNLIKELY(!first.m_object->is_object()))\n        {\n            JSON_THROW(type_error::create(312, detail::concat(\"cannot use update() with \", first.m_object->type_name()), first.m_object));\n        }\n\n        for (auto it = first; it != last; ++it)\n        {\n            if (merge_objects && it.value().is_object())\n            {\n                auto it2 = m_value.object->find(it.key());\n                if (it2 != m_value.object->end())\n                {\n                    it2->second.update(it.value(), true);\n                    continue;\n                }\n            }\n            m_value.object->operator[](it.key()) = it.value();\n#if JSON_DIAGNOSTICS\n            m_value.object->operator[](it.key()).m_parent = this;\n#endif\n        }\n    }\n\n    /// @brief exchanges the values\n    /// @sa https://json.nlohmann.me/api/basic_json/swap/\n    void swap(reference other) noexcept (\n        std::is_nothrow_move_constructible<value_t>::value&&\n        std::is_nothrow_move_assignable<value_t>::value&&\n        std::is_nothrow_move_constructible<json_value>::value&&\n        std::is_nothrow_move_assignable<json_value>::value\n    )\n    {\n        std::swap(m_type, other.m_type);\n        std::swap(m_value, other.m_value);\n\n        set_parents();\n        other.set_parents();\n        assert_invariant();\n    }\n\n    /// @brief exchanges the values\n    /// @sa https://json.nlohmann.me/api/basic_json/swap/\n    friend void swap(reference left, reference right) noexcept (\n        std::is_nothrow_move_constructible<value_t>::value&&\n        std::is_nothrow_move_assignable<value_t>::value&&\n        std::is_nothrow_move_constructible<json_value>::value&&\n        std::is_nothrow_move_assignable<json_value>::value\n    )\n    {\n        left.swap(right);\n    }\n\n    /// @brief exchanges the values\n    /// @sa https://json.nlohmann.me/api/basic_json/swap/\n    void swap(array_t& other) // NOLINT(bugprone-exception-escape)\n    {\n        // swap only works for arrays\n        if (JSON_HEDLEY_LIKELY(is_array()))\n        {\n            using std::swap;\n            swap(*(m_value.array), other);\n        }\n        else\n        {\n            JSON_THROW(type_error::create(310, detail::concat(\"cannot use swap(array_t&) with \", type_name()), this));\n        }\n    }\n\n    /// @brief exchanges the values\n    /// @sa https://json.nlohmann.me/api/basic_json/swap/\n    void swap(object_t& other) // NOLINT(bugprone-exception-escape)\n    {\n        // swap only works for objects\n        if (JSON_HEDLEY_LIKELY(is_object()))\n        {\n            using std::swap;\n            swap(*(m_value.object), other);\n        }\n        else\n        {\n            JSON_THROW(type_error::create(310, detail::concat(\"cannot use swap(object_t&) with \", type_name()), this));\n        }\n    }\n\n    /// @brief exchanges the values\n    /// @sa https://json.nlohmann.me/api/basic_json/swap/\n    void swap(string_t& other) // NOLINT(bugprone-exception-escape)\n    {\n        // swap only works for strings\n        if (JSON_HEDLEY_LIKELY(is_string()))\n        {\n            using std::swap;\n            swap(*(m_value.string), other);\n        }\n        else\n        {\n            JSON_THROW(type_error::create(310, detail::concat(\"cannot use swap(string_t&) with \", type_name()), this));\n        }\n    }\n\n    /// @brief exchanges the values\n    /// @sa https://json.nlohmann.me/api/basic_json/swap/\n    void swap(binary_t& other) // NOLINT(bugprone-exception-escape)\n    {\n        // swap only works for strings\n        if (JSON_HEDLEY_LIKELY(is_binary()))\n        {\n            using std::swap;\n            swap(*(m_value.binary), other);\n        }\n        else\n        {\n            JSON_THROW(type_error::create(310, detail::concat(\"cannot use swap(binary_t&) with \", type_name()), this));\n        }\n    }\n\n    /// @brief exchanges the values\n    /// @sa https://json.nlohmann.me/api/basic_json/swap/\n    void swap(typename binary_t::container_type& other) // NOLINT(bugprone-exception-escape)\n    {\n        // swap only works for strings\n        if (JSON_HEDLEY_LIKELY(is_binary()))\n        {\n            using std::swap;\n            swap(*(m_value.binary), other);\n        }\n        else\n        {\n            JSON_THROW(type_error::create(310, detail::concat(\"cannot use swap(binary_t::container_type&) with \", type_name()), this));\n        }\n    }\n\n    /// @}\n\n    //////////////////////////////////////////\n    // lexicographical comparison operators //\n    //////////////////////////////////////////\n\n    /// @name lexicographical comparison operators\n    /// @{\n\n    // note parentheses around operands are necessary; see\n    // https://github.com/nlohmann/json/issues/1530\n#define JSON_IMPLEMENT_OPERATOR(op, null_result, unordered_result, default_result)                       \\\n    const auto lhs_type = lhs.type();                                                                    \\\n    const auto rhs_type = rhs.type();                                                                    \\\n    \\\n    if (lhs_type == rhs_type) /* NOLINT(readability/braces) */                                           \\\n    {                                                                                                    \\\n        switch (lhs_type)                                                                                \\\n        {                                                                                                \\\n            case value_t::array:                                                                         \\\n                return (*lhs.m_value.array) op (*rhs.m_value.array);                                     \\\n                \\\n            case value_t::object:                                                                        \\\n                return (*lhs.m_value.object) op (*rhs.m_value.object);                                   \\\n                \\\n            case value_t::null:                                                                          \\\n                return (null_result);                                                                    \\\n                \\\n            case value_t::string:                                                                        \\\n                return (*lhs.m_value.string) op (*rhs.m_value.string);                                   \\\n                \\\n            case value_t::boolean:                                                                       \\\n                return (lhs.m_value.boolean) op (rhs.m_value.boolean);                                   \\\n                \\\n            case value_t::number_integer:                                                                \\\n                return (lhs.m_value.number_integer) op (rhs.m_value.number_integer);                     \\\n                \\\n            case value_t::number_unsigned:                                                               \\\n                return (lhs.m_value.number_unsigned) op (rhs.m_value.number_unsigned);                   \\\n                \\\n            case value_t::number_float:                                                                  \\\n                return (lhs.m_value.number_float) op (rhs.m_value.number_float);                         \\\n                \\\n            case value_t::binary:                                                                        \\\n                return (*lhs.m_value.binary) op (*rhs.m_value.binary);                                   \\\n                \\\n            case value_t::discarded:                                                                     \\\n            default:                                                                                     \\\n                return (unordered_result);                                                               \\\n        }                                                                                                \\\n    }                                                                                                    \\\n    else if (lhs_type == value_t::number_integer && rhs_type == value_t::number_float)                   \\\n    {                                                                                                    \\\n        return static_cast<number_float_t>(lhs.m_value.number_integer) op rhs.m_value.number_float;      \\\n    }                                                                                                    \\\n    else if (lhs_type == value_t::number_float && rhs_type == value_t::number_integer)                   \\\n    {                                                                                                    \\\n        return lhs.m_value.number_float op static_cast<number_float_t>(rhs.m_value.number_integer);      \\\n    }                                                                                                    \\\n    else if (lhs_type == value_t::number_unsigned && rhs_type == value_t::number_float)                  \\\n    {                                                                                                    \\\n        return static_cast<number_float_t>(lhs.m_value.number_unsigned) op rhs.m_value.number_float;     \\\n    }                                                                                                    \\\n    else if (lhs_type == value_t::number_float && rhs_type == value_t::number_unsigned)                  \\\n    {                                                                                                    \\\n        return lhs.m_value.number_float op static_cast<number_float_t>(rhs.m_value.number_unsigned);     \\\n    }                                                                                                    \\\n    else if (lhs_type == value_t::number_unsigned && rhs_type == value_t::number_integer)                \\\n    {                                                                                                    \\\n        return static_cast<number_integer_t>(lhs.m_value.number_unsigned) op rhs.m_value.number_integer; \\\n    }                                                                                                    \\\n    else if (lhs_type == value_t::number_integer && rhs_type == value_t::number_unsigned)                \\\n    {                                                                                                    \\\n        return lhs.m_value.number_integer op static_cast<number_integer_t>(rhs.m_value.number_unsigned); \\\n    }                                                                                                    \\\n    else if(compares_unordered(lhs, rhs))\\\n    {\\\n        return (unordered_result);\\\n    }\\\n    \\\n    return (default_result);\n\n  JSON_PRIVATE_UNLESS_TESTED:\n    // returns true if:\n    // - any operand is NaN and the other operand is of number type\n    // - any operand is discarded\n    // in legacy mode, discarded values are considered ordered if\n    // an operation is computed as an odd number of inverses of others\n    static bool compares_unordered(const_reference lhs, const_reference rhs, bool inverse = false) noexcept\n    {\n        if ((lhs.is_number_float() && std::isnan(lhs.m_value.number_float) && rhs.is_number())\n                || (rhs.is_number_float() && std::isnan(rhs.m_value.number_float) && lhs.is_number()))\n        {\n            return true;\n        }\n#if JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON\n        return (lhs.is_discarded() || rhs.is_discarded()) && !inverse;\n#else\n        static_cast<void>(inverse);\n        return lhs.is_discarded() || rhs.is_discarded();\n#endif\n    }\n\n  private:\n    bool compares_unordered(const_reference rhs, bool inverse = false) const noexcept\n    {\n        return compares_unordered(*this, rhs, inverse);\n    }\n\n  public:\n#if JSON_HAS_THREE_WAY_COMPARISON\n    /// @brief comparison: equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_eq/\n    bool operator==(const_reference rhs) const noexcept\n    {\n#ifdef __GNUC__\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wfloat-equal\"\n#endif\n        const_reference lhs = *this;\n        JSON_IMPLEMENT_OPERATOR( ==, true, false, false)\n#ifdef __GNUC__\n#pragma GCC diagnostic pop\n#endif\n    }\n\n    /// @brief comparison: equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_eq/\n    template<typename ScalarType>\n    requires std::is_scalar_v<ScalarType>\n    bool operator==(ScalarType rhs) const noexcept\n    {\n        return *this == basic_json(rhs);\n    }\n\n    /// @brief comparison: not equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ne/\n    bool operator!=(const_reference rhs) const noexcept\n    {\n        if (compares_unordered(rhs, true))\n        {\n            return false;\n        }\n        return !operator==(rhs);\n    }\n\n    /// @brief comparison: 3-way\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_spaceship/\n    std::partial_ordering operator<=>(const_reference rhs) const noexcept // *NOPAD*\n    {\n        const_reference lhs = *this;\n        // default_result is used if we cannot compare values. In that case,\n        // we compare types.\n        JSON_IMPLEMENT_OPERATOR(<=>, // *NOPAD*\n                                std::partial_ordering::equivalent,\n                                std::partial_ordering::unordered,\n                                lhs_type <=> rhs_type) // *NOPAD*\n    }\n\n    /// @brief comparison: 3-way\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_spaceship/\n    template<typename ScalarType>\n    requires std::is_scalar_v<ScalarType>\n    std::partial_ordering operator<=>(ScalarType rhs) const noexcept // *NOPAD*\n    {\n        return *this <=> basic_json(rhs); // *NOPAD*\n    }\n\n#if JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON\n    // all operators that are computed as an odd number of inverses of others\n    // need to be overloaded to emulate the legacy comparison behavior\n\n    /// @brief comparison: less than or equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_le/\n    JSON_HEDLEY_DEPRECATED_FOR(3.11.0, undef JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON)\n    bool operator<=(const_reference rhs) const noexcept\n    {\n        if (compares_unordered(rhs, true))\n        {\n            return false;\n        }\n        return !(rhs < *this);\n    }\n\n    /// @brief comparison: less than or equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_le/\n    template<typename ScalarType>\n    requires std::is_scalar_v<ScalarType>\n    bool operator<=(ScalarType rhs) const noexcept\n    {\n        return *this <= basic_json(rhs);\n    }\n\n    /// @brief comparison: greater than or equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ge/\n    JSON_HEDLEY_DEPRECATED_FOR(3.11.0, undef JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON)\n    bool operator>=(const_reference rhs) const noexcept\n    {\n        if (compares_unordered(rhs, true))\n        {\n            return false;\n        }\n        return !(*this < rhs);\n    }\n\n    /// @brief comparison: greater than or equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ge/\n    template<typename ScalarType>\n    requires std::is_scalar_v<ScalarType>\n    bool operator>=(ScalarType rhs) const noexcept\n    {\n        return *this >= basic_json(rhs);\n    }\n#endif\n#else\n    /// @brief comparison: equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_eq/\n    friend bool operator==(const_reference lhs, const_reference rhs) noexcept\n    {\n#ifdef __GNUC__\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wfloat-equal\"\n#endif\n        JSON_IMPLEMENT_OPERATOR( ==, true, false, false)\n#ifdef __GNUC__\n#pragma GCC diagnostic pop\n#endif\n    }\n\n    /// @brief comparison: equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_eq/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator==(const_reference lhs, ScalarType rhs) noexcept\n    {\n        return lhs == basic_json(rhs);\n    }\n\n    /// @brief comparison: equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_eq/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator==(ScalarType lhs, const_reference rhs) noexcept\n    {\n        return basic_json(lhs) == rhs;\n    }\n\n    /// @brief comparison: not equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ne/\n    friend bool operator!=(const_reference lhs, const_reference rhs) noexcept\n    {\n        if (compares_unordered(lhs, rhs, true))\n        {\n            return false;\n        }\n        return !(lhs == rhs);\n    }\n\n    /// @brief comparison: not equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ne/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator!=(const_reference lhs, ScalarType rhs) noexcept\n    {\n        return lhs != basic_json(rhs);\n    }\n\n    /// @brief comparison: not equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ne/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator!=(ScalarType lhs, const_reference rhs) noexcept\n    {\n        return basic_json(lhs) != rhs;\n    }\n\n    /// @brief comparison: less than\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_lt/\n    friend bool operator<(const_reference lhs, const_reference rhs) noexcept\n    {\n        // default_result is used if we cannot compare values. In that case,\n        // we compare types. Note we have to call the operator explicitly,\n        // because MSVC has problems otherwise.\n        JSON_IMPLEMENT_OPERATOR( <, false, false, operator<(lhs_type, rhs_type))\n    }\n\n    /// @brief comparison: less than\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_lt/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator<(const_reference lhs, ScalarType rhs) noexcept\n    {\n        return lhs < basic_json(rhs);\n    }\n\n    /// @brief comparison: less than\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_lt/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator<(ScalarType lhs, const_reference rhs) noexcept\n    {\n        return basic_json(lhs) < rhs;\n    }\n\n    /// @brief comparison: less than or equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_le/\n    friend bool operator<=(const_reference lhs, const_reference rhs) noexcept\n    {\n        if (compares_unordered(lhs, rhs, true))\n        {\n            return false;\n        }\n        return !(rhs < lhs);\n    }\n\n    /// @brief comparison: less than or equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_le/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator<=(const_reference lhs, ScalarType rhs) noexcept\n    {\n        return lhs <= basic_json(rhs);\n    }\n\n    /// @brief comparison: less than or equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_le/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator<=(ScalarType lhs, const_reference rhs) noexcept\n    {\n        return basic_json(lhs) <= rhs;\n    }\n\n    /// @brief comparison: greater than\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_gt/\n    friend bool operator>(const_reference lhs, const_reference rhs) noexcept\n    {\n        // double inverse\n        if (compares_unordered(lhs, rhs))\n        {\n            return false;\n        }\n        return !(lhs <= rhs);\n    }\n\n    /// @brief comparison: greater than\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_gt/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator>(const_reference lhs, ScalarType rhs) noexcept\n    {\n        return lhs > basic_json(rhs);\n    }\n\n    /// @brief comparison: greater than\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_gt/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator>(ScalarType lhs, const_reference rhs) noexcept\n    {\n        return basic_json(lhs) > rhs;\n    }\n\n    /// @brief comparison: greater than or equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ge/\n    friend bool operator>=(const_reference lhs, const_reference rhs) noexcept\n    {\n        if (compares_unordered(lhs, rhs, true))\n        {\n            return false;\n        }\n        return !(lhs < rhs);\n    }\n\n    /// @brief comparison: greater than or equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ge/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator>=(const_reference lhs, ScalarType rhs) noexcept\n    {\n        return lhs >= basic_json(rhs);\n    }\n\n    /// @brief comparison: greater than or equal\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ge/\n    template<typename ScalarType, typename std::enable_if<\n                 std::is_scalar<ScalarType>::value, int>::type = 0>\n    friend bool operator>=(ScalarType lhs, const_reference rhs) noexcept\n    {\n        return basic_json(lhs) >= rhs;\n    }\n#endif\n\n#undef JSON_IMPLEMENT_OPERATOR\n\n    /// @}\n\n    ///////////////////\n    // serialization //\n    ///////////////////\n\n    /// @name serialization\n    /// @{\n#ifndef JSON_NO_IO\n    /// @brief serialize to stream\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ltlt/\n    friend std::ostream& operator<<(std::ostream& o, const basic_json& j)\n    {\n        // read width member and use it as indentation parameter if nonzero\n        const bool pretty_print = o.width() > 0;\n        const auto indentation = pretty_print ? o.width() : 0;\n\n        // reset width to 0 for subsequent calls to this stream\n        o.width(0);\n\n        // do the actual serialization\n        serializer s(detail::output_adapter<char>(o), o.fill());\n        s.dump(j, pretty_print, false, static_cast<unsigned int>(indentation));\n        return o;\n    }\n\n    /// @brief serialize to stream\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_ltlt/\n    /// @deprecated This function is deprecated since 3.0.0 and will be removed in\n    ///             version 4.0.0 of the library. Please use\n    ///             operator<<(std::ostream&, const basic_json&) instead; that is,\n    ///             replace calls like `j >> o;` with `o << j;`.\n    JSON_HEDLEY_DEPRECATED_FOR(3.0.0, operator<<(std::ostream&, const basic_json&))\n    friend std::ostream& operator>>(const basic_json& j, std::ostream& o)\n    {\n        return o << j;\n    }\n#endif  // JSON_NO_IO\n    /// @}\n\n\n    /////////////////////\n    // deserialization //\n    /////////////////////\n\n    /// @name deserialization\n    /// @{\n\n    /// @brief deserialize from a compatible input\n    /// @sa https://json.nlohmann.me/api/basic_json/parse/\n    template<typename InputType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json parse(InputType&& i,\n                            const parser_callback_t cb = nullptr,\n                            const bool allow_exceptions = true,\n                            const bool ignore_comments = false)\n    {\n        basic_json result;\n        parser(detail::input_adapter(std::forward<InputType>(i)), cb, allow_exceptions, ignore_comments).parse(true, result);\n        return result;\n    }\n\n    /// @brief deserialize from a pair of character iterators\n    /// @sa https://json.nlohmann.me/api/basic_json/parse/\n    template<typename IteratorType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json parse(IteratorType first,\n                            IteratorType last,\n                            const parser_callback_t cb = nullptr,\n                            const bool allow_exceptions = true,\n                            const bool ignore_comments = false)\n    {\n        basic_json result;\n        parser(detail::input_adapter(std::move(first), std::move(last)), cb, allow_exceptions, ignore_comments).parse(true, result);\n        return result;\n    }\n\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    JSON_HEDLEY_DEPRECATED_FOR(3.8.0, parse(ptr, ptr + len))\n    static basic_json parse(detail::span_input_adapter&& i,\n                            const parser_callback_t cb = nullptr,\n                            const bool allow_exceptions = true,\n                            const bool ignore_comments = false)\n    {\n        basic_json result;\n        parser(i.get(), cb, allow_exceptions, ignore_comments).parse(true, result);\n        return result;\n    }\n\n    /// @brief check if the input is valid JSON\n    /// @sa https://json.nlohmann.me/api/basic_json/accept/\n    template<typename InputType>\n    static bool accept(InputType&& i,\n                       const bool ignore_comments = false)\n    {\n        return parser(detail::input_adapter(std::forward<InputType>(i)), nullptr, false, ignore_comments).accept(true);\n    }\n\n    /// @brief check if the input is valid JSON\n    /// @sa https://json.nlohmann.me/api/basic_json/accept/\n    template<typename IteratorType>\n    static bool accept(IteratorType first, IteratorType last,\n                       const bool ignore_comments = false)\n    {\n        return parser(detail::input_adapter(std::move(first), std::move(last)), nullptr, false, ignore_comments).accept(true);\n    }\n\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    JSON_HEDLEY_DEPRECATED_FOR(3.8.0, accept(ptr, ptr + len))\n    static bool accept(detail::span_input_adapter&& i,\n                       const bool ignore_comments = false)\n    {\n        return parser(i.get(), nullptr, false, ignore_comments).accept(true);\n    }\n\n    /// @brief generate SAX events\n    /// @sa https://json.nlohmann.me/api/basic_json/sax_parse/\n    template <typename InputType, typename SAX>\n    JSON_HEDLEY_NON_NULL(2)\n    static bool sax_parse(InputType&& i, SAX* sax,\n                          input_format_t format = input_format_t::json,\n                          const bool strict = true,\n                          const bool ignore_comments = false)\n    {\n        auto ia = detail::input_adapter(std::forward<InputType>(i));\n        return format == input_format_t::json\n               ? parser(std::move(ia), nullptr, true, ignore_comments).sax_parse(sax, strict)\n               : detail::binary_reader<basic_json, decltype(ia), SAX>(std::move(ia), format).sax_parse(format, sax, strict);\n    }\n\n    /// @brief generate SAX events\n    /// @sa https://json.nlohmann.me/api/basic_json/sax_parse/\n    template<class IteratorType, class SAX>\n    JSON_HEDLEY_NON_NULL(3)\n    static bool sax_parse(IteratorType first, IteratorType last, SAX* sax,\n                          input_format_t format = input_format_t::json,\n                          const bool strict = true,\n                          const bool ignore_comments = false)\n    {\n        auto ia = detail::input_adapter(std::move(first), std::move(last));\n        return format == input_format_t::json\n               ? parser(std::move(ia), nullptr, true, ignore_comments).sax_parse(sax, strict)\n               : detail::binary_reader<basic_json, decltype(ia), SAX>(std::move(ia), format).sax_parse(format, sax, strict);\n    }\n\n    /// @brief generate SAX events\n    /// @sa https://json.nlohmann.me/api/basic_json/sax_parse/\n    /// @deprecated This function is deprecated since 3.8.0 and will be removed in\n    ///             version 4.0.0 of the library. Please use\n    ///             sax_parse(ptr, ptr + len) instead.\n    template <typename SAX>\n    JSON_HEDLEY_DEPRECATED_FOR(3.8.0, sax_parse(ptr, ptr + len, ...))\n    JSON_HEDLEY_NON_NULL(2)\n    static bool sax_parse(detail::span_input_adapter&& i, SAX* sax,\n                          input_format_t format = input_format_t::json,\n                          const bool strict = true,\n                          const bool ignore_comments = false)\n    {\n        auto ia = i.get();\n        return format == input_format_t::json\n               // NOLINTNEXTLINE(hicpp-move-const-arg,performance-move-const-arg)\n               ? parser(std::move(ia), nullptr, true, ignore_comments).sax_parse(sax, strict)\n               // NOLINTNEXTLINE(hicpp-move-const-arg,performance-move-const-arg)\n               : detail::binary_reader<basic_json, decltype(ia), SAX>(std::move(ia), format).sax_parse(format, sax, strict);\n    }\n#ifndef JSON_NO_IO\n    /// @brief deserialize from stream\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_gtgt/\n    /// @deprecated This stream operator is deprecated since 3.0.0 and will be removed in\n    ///             version 4.0.0 of the library. Please use\n    ///             operator>>(std::istream&, basic_json&) instead; that is,\n    ///             replace calls like `j << i;` with `i >> j;`.\n    JSON_HEDLEY_DEPRECATED_FOR(3.0.0, operator>>(std::istream&, basic_json&))\n    friend std::istream& operator<<(basic_json& j, std::istream& i)\n    {\n        return operator>>(i, j);\n    }\n\n    /// @brief deserialize from stream\n    /// @sa https://json.nlohmann.me/api/basic_json/operator_gtgt/\n    friend std::istream& operator>>(std::istream& i, basic_json& j)\n    {\n        parser(detail::input_adapter(i)).parse(false, j);\n        return i;\n    }\n#endif  // JSON_NO_IO\n    /// @}\n\n    ///////////////////////////\n    // convenience functions //\n    ///////////////////////////\n\n    /// @brief return the type as string\n    /// @sa https://json.nlohmann.me/api/basic_json/type_name/\n    JSON_HEDLEY_RETURNS_NON_NULL\n    const char* type_name() const noexcept\n    {\n        switch (m_type)\n        {\n            case value_t::null:\n                return \"null\";\n            case value_t::object:\n                return \"object\";\n            case value_t::array:\n                return \"array\";\n            case value_t::string:\n                return \"string\";\n            case value_t::boolean:\n                return \"boolean\";\n            case value_t::binary:\n                return \"binary\";\n            case value_t::discarded:\n                return \"discarded\";\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            default:\n                return \"number\";\n        }\n    }\n\n\n  JSON_PRIVATE_UNLESS_TESTED:\n    //////////////////////\n    // member variables //\n    //////////////////////\n\n    /// the type of the current element\n    value_t m_type = value_t::null;\n\n    /// the value of the current element\n    json_value m_value = {};\n\n#if JSON_DIAGNOSTICS\n    /// a pointer to a parent value (for debugging purposes)\n    basic_json* m_parent = nullptr;\n#endif\n\n    //////////////////////////////////////////\n    // binary serialization/deserialization //\n    //////////////////////////////////////////\n\n    /// @name binary serialization/deserialization support\n    /// @{\n\n  public:\n    /// @brief create a CBOR serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_cbor/\n    static std::vector<std::uint8_t> to_cbor(const basic_json& j)\n    {\n        std::vector<std::uint8_t> result;\n        to_cbor(j, result);\n        return result;\n    }\n\n    /// @brief create a CBOR serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_cbor/\n    static void to_cbor(const basic_json& j, detail::output_adapter<std::uint8_t> o)\n    {\n        binary_writer<std::uint8_t>(o).write_cbor(j);\n    }\n\n    /// @brief create a CBOR serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_cbor/\n    static void to_cbor(const basic_json& j, detail::output_adapter<char> o)\n    {\n        binary_writer<char>(o).write_cbor(j);\n    }\n\n    /// @brief create a MessagePack serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_msgpack/\n    static std::vector<std::uint8_t> to_msgpack(const basic_json& j)\n    {\n        std::vector<std::uint8_t> result;\n        to_msgpack(j, result);\n        return result;\n    }\n\n    /// @brief create a MessagePack serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_msgpack/\n    static void to_msgpack(const basic_json& j, detail::output_adapter<std::uint8_t> o)\n    {\n        binary_writer<std::uint8_t>(o).write_msgpack(j);\n    }\n\n    /// @brief create a MessagePack serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_msgpack/\n    static void to_msgpack(const basic_json& j, detail::output_adapter<char> o)\n    {\n        binary_writer<char>(o).write_msgpack(j);\n    }\n\n    /// @brief create a UBJSON serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_ubjson/\n    static std::vector<std::uint8_t> to_ubjson(const basic_json& j,\n            const bool use_size = false,\n            const bool use_type = false)\n    {\n        std::vector<std::uint8_t> result;\n        to_ubjson(j, result, use_size, use_type);\n        return result;\n    }\n\n    /// @brief create a UBJSON serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_ubjson/\n    static void to_ubjson(const basic_json& j, detail::output_adapter<std::uint8_t> o,\n                          const bool use_size = false, const bool use_type = false)\n    {\n        binary_writer<std::uint8_t>(o).write_ubjson(j, use_size, use_type);\n    }\n\n    /// @brief create a UBJSON serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_ubjson/\n    static void to_ubjson(const basic_json& j, detail::output_adapter<char> o,\n                          const bool use_size = false, const bool use_type = false)\n    {\n        binary_writer<char>(o).write_ubjson(j, use_size, use_type);\n    }\n\n    /// @brief create a BJData serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_bjdata/\n    static std::vector<std::uint8_t> to_bjdata(const basic_json& j,\n            const bool use_size = false,\n            const bool use_type = false)\n    {\n        std::vector<std::uint8_t> result;\n        to_bjdata(j, result, use_size, use_type);\n        return result;\n    }\n\n    /// @brief create a BJData serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_bjdata/\n    static void to_bjdata(const basic_json& j, detail::output_adapter<std::uint8_t> o,\n                          const bool use_size = false, const bool use_type = false)\n    {\n        binary_writer<std::uint8_t>(o).write_ubjson(j, use_size, use_type, true, true);\n    }\n\n    /// @brief create a BJData serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_bjdata/\n    static void to_bjdata(const basic_json& j, detail::output_adapter<char> o,\n                          const bool use_size = false, const bool use_type = false)\n    {\n        binary_writer<char>(o).write_ubjson(j, use_size, use_type, true, true);\n    }\n\n    /// @brief create a BSON serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_bson/\n    static std::vector<std::uint8_t> to_bson(const basic_json& j)\n    {\n        std::vector<std::uint8_t> result;\n        to_bson(j, result);\n        return result;\n    }\n\n    /// @brief create a BSON serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_bson/\n    static void to_bson(const basic_json& j, detail::output_adapter<std::uint8_t> o)\n    {\n        binary_writer<std::uint8_t>(o).write_bson(j);\n    }\n\n    /// @brief create a BSON serialization of a given JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/to_bson/\n    static void to_bson(const basic_json& j, detail::output_adapter<char> o)\n    {\n        binary_writer<char>(o).write_bson(j);\n    }\n\n    /// @brief create a JSON value from an input in CBOR format\n    /// @sa https://json.nlohmann.me/api/basic_json/from_cbor/\n    template<typename InputType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json from_cbor(InputType&& i,\n                                const bool strict = true,\n                                const bool allow_exceptions = true,\n                                const cbor_tag_handler_t tag_handler = cbor_tag_handler_t::error)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = detail::input_adapter(std::forward<InputType>(i));\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::cbor).sax_parse(input_format_t::cbor, &sdp, strict, tag_handler);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    /// @brief create a JSON value from an input in CBOR format\n    /// @sa https://json.nlohmann.me/api/basic_json/from_cbor/\n    template<typename IteratorType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json from_cbor(IteratorType first, IteratorType last,\n                                const bool strict = true,\n                                const bool allow_exceptions = true,\n                                const cbor_tag_handler_t tag_handler = cbor_tag_handler_t::error)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = detail::input_adapter(std::move(first), std::move(last));\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::cbor).sax_parse(input_format_t::cbor, &sdp, strict, tag_handler);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    template<typename T>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    JSON_HEDLEY_DEPRECATED_FOR(3.8.0, from_cbor(ptr, ptr + len))\n    static basic_json from_cbor(const T* ptr, std::size_t len,\n                                const bool strict = true,\n                                const bool allow_exceptions = true,\n                                const cbor_tag_handler_t tag_handler = cbor_tag_handler_t::error)\n    {\n        return from_cbor(ptr, ptr + len, strict, allow_exceptions, tag_handler);\n    }\n\n\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    JSON_HEDLEY_DEPRECATED_FOR(3.8.0, from_cbor(ptr, ptr + len))\n    static basic_json from_cbor(detail::span_input_adapter&& i,\n                                const bool strict = true,\n                                const bool allow_exceptions = true,\n                                const cbor_tag_handler_t tag_handler = cbor_tag_handler_t::error)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = i.get();\n        // NOLINTNEXTLINE(hicpp-move-const-arg,performance-move-const-arg)\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::cbor).sax_parse(input_format_t::cbor, &sdp, strict, tag_handler);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    /// @brief create a JSON value from an input in MessagePack format\n    /// @sa https://json.nlohmann.me/api/basic_json/from_msgpack/\n    template<typename InputType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json from_msgpack(InputType&& i,\n                                   const bool strict = true,\n                                   const bool allow_exceptions = true)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = detail::input_adapter(std::forward<InputType>(i));\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::msgpack).sax_parse(input_format_t::msgpack, &sdp, strict);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    /// @brief create a JSON value from an input in MessagePack format\n    /// @sa https://json.nlohmann.me/api/basic_json/from_msgpack/\n    template<typename IteratorType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json from_msgpack(IteratorType first, IteratorType last,\n                                   const bool strict = true,\n                                   const bool allow_exceptions = true)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = detail::input_adapter(std::move(first), std::move(last));\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::msgpack).sax_parse(input_format_t::msgpack, &sdp, strict);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    template<typename T>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    JSON_HEDLEY_DEPRECATED_FOR(3.8.0, from_msgpack(ptr, ptr + len))\n    static basic_json from_msgpack(const T* ptr, std::size_t len,\n                                   const bool strict = true,\n                                   const bool allow_exceptions = true)\n    {\n        return from_msgpack(ptr, ptr + len, strict, allow_exceptions);\n    }\n\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    JSON_HEDLEY_DEPRECATED_FOR(3.8.0, from_msgpack(ptr, ptr + len))\n    static basic_json from_msgpack(detail::span_input_adapter&& i,\n                                   const bool strict = true,\n                                   const bool allow_exceptions = true)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = i.get();\n        // NOLINTNEXTLINE(hicpp-move-const-arg,performance-move-const-arg)\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::msgpack).sax_parse(input_format_t::msgpack, &sdp, strict);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    /// @brief create a JSON value from an input in UBJSON format\n    /// @sa https://json.nlohmann.me/api/basic_json/from_ubjson/\n    template<typename InputType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json from_ubjson(InputType&& i,\n                                  const bool strict = true,\n                                  const bool allow_exceptions = true)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = detail::input_adapter(std::forward<InputType>(i));\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::ubjson).sax_parse(input_format_t::ubjson, &sdp, strict);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    /// @brief create a JSON value from an input in UBJSON format\n    /// @sa https://json.nlohmann.me/api/basic_json/from_ubjson/\n    template<typename IteratorType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json from_ubjson(IteratorType first, IteratorType last,\n                                  const bool strict = true,\n                                  const bool allow_exceptions = true)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = detail::input_adapter(std::move(first), std::move(last));\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::ubjson).sax_parse(input_format_t::ubjson, &sdp, strict);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    template<typename T>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    JSON_HEDLEY_DEPRECATED_FOR(3.8.0, from_ubjson(ptr, ptr + len))\n    static basic_json from_ubjson(const T* ptr, std::size_t len,\n                                  const bool strict = true,\n                                  const bool allow_exceptions = true)\n    {\n        return from_ubjson(ptr, ptr + len, strict, allow_exceptions);\n    }\n\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    JSON_HEDLEY_DEPRECATED_FOR(3.8.0, from_ubjson(ptr, ptr + len))\n    static basic_json from_ubjson(detail::span_input_adapter&& i,\n                                  const bool strict = true,\n                                  const bool allow_exceptions = true)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = i.get();\n        // NOLINTNEXTLINE(hicpp-move-const-arg,performance-move-const-arg)\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::ubjson).sax_parse(input_format_t::ubjson, &sdp, strict);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n\n    /// @brief create a JSON value from an input in BJData format\n    /// @sa https://json.nlohmann.me/api/basic_json/from_bjdata/\n    template<typename InputType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json from_bjdata(InputType&& i,\n                                  const bool strict = true,\n                                  const bool allow_exceptions = true)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = detail::input_adapter(std::forward<InputType>(i));\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::bjdata).sax_parse(input_format_t::bjdata, &sdp, strict);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    /// @brief create a JSON value from an input in BJData format\n    /// @sa https://json.nlohmann.me/api/basic_json/from_bjdata/\n    template<typename IteratorType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json from_bjdata(IteratorType first, IteratorType last,\n                                  const bool strict = true,\n                                  const bool allow_exceptions = true)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = detail::input_adapter(std::move(first), std::move(last));\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::bjdata).sax_parse(input_format_t::bjdata, &sdp, strict);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    /// @brief create a JSON value from an input in BSON format\n    /// @sa https://json.nlohmann.me/api/basic_json/from_bson/\n    template<typename InputType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json from_bson(InputType&& i,\n                                const bool strict = true,\n                                const bool allow_exceptions = true)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = detail::input_adapter(std::forward<InputType>(i));\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::bson).sax_parse(input_format_t::bson, &sdp, strict);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    /// @brief create a JSON value from an input in BSON format\n    /// @sa https://json.nlohmann.me/api/basic_json/from_bson/\n    template<typename IteratorType>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json from_bson(IteratorType first, IteratorType last,\n                                const bool strict = true,\n                                const bool allow_exceptions = true)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = detail::input_adapter(std::move(first), std::move(last));\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::bson).sax_parse(input_format_t::bson, &sdp, strict);\n        return res ? result : basic_json(value_t::discarded);\n    }\n\n    template<typename T>\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    JSON_HEDLEY_DEPRECATED_FOR(3.8.0, from_bson(ptr, ptr + len))\n    static basic_json from_bson(const T* ptr, std::size_t len,\n                                const bool strict = true,\n                                const bool allow_exceptions = true)\n    {\n        return from_bson(ptr, ptr + len, strict, allow_exceptions);\n    }\n\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    JSON_HEDLEY_DEPRECATED_FOR(3.8.0, from_bson(ptr, ptr + len))\n    static basic_json from_bson(detail::span_input_adapter&& i,\n                                const bool strict = true,\n                                const bool allow_exceptions = true)\n    {\n        basic_json result;\n        detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions);\n        auto ia = i.get();\n        // NOLINTNEXTLINE(hicpp-move-const-arg,performance-move-const-arg)\n        const bool res = binary_reader<decltype(ia)>(std::move(ia), input_format_t::bson).sax_parse(input_format_t::bson, &sdp, strict);\n        return res ? result : basic_json(value_t::discarded);\n    }\n    /// @}\n\n    //////////////////////////\n    // JSON Pointer support //\n    //////////////////////////\n\n    /// @name JSON Pointer functions\n    /// @{\n\n    /// @brief access specified element via JSON Pointer\n    /// @sa https://json.nlohmann.me/api/basic_json/operator%5B%5D/\n    reference operator[](const json_pointer& ptr)\n    {\n        return ptr.get_unchecked(this);\n    }\n\n    template<typename BasicJsonType, detail::enable_if_t<detail::is_basic_json<BasicJsonType>::value, int> = 0>\n    JSON_HEDLEY_DEPRECATED_FOR(3.11.0, basic_json::json_pointer or nlohmann::json_pointer<basic_json::string_t>) // NOLINT(readability/alt_tokens)\n    reference operator[](const ::nlohmann::json_pointer<BasicJsonType>& ptr)\n    {\n        return ptr.get_unchecked(this);\n    }\n\n    /// @brief access specified element via JSON Pointer\n    /// @sa https://json.nlohmann.me/api/basic_json/operator%5B%5D/\n    const_reference operator[](const json_pointer& ptr) const\n    {\n        return ptr.get_unchecked(this);\n    }\n\n    template<typename BasicJsonType, detail::enable_if_t<detail::is_basic_json<BasicJsonType>::value, int> = 0>\n    JSON_HEDLEY_DEPRECATED_FOR(3.11.0, basic_json::json_pointer or nlohmann::json_pointer<basic_json::string_t>) // NOLINT(readability/alt_tokens)\n    const_reference operator[](const ::nlohmann::json_pointer<BasicJsonType>& ptr) const\n    {\n        return ptr.get_unchecked(this);\n    }\n\n    /// @brief access specified element via JSON Pointer\n    /// @sa https://json.nlohmann.me/api/basic_json/at/\n    reference at(const json_pointer& ptr)\n    {\n        return ptr.get_checked(this);\n    }\n\n    template<typename BasicJsonType, detail::enable_if_t<detail::is_basic_json<BasicJsonType>::value, int> = 0>\n    JSON_HEDLEY_DEPRECATED_FOR(3.11.0, basic_json::json_pointer or nlohmann::json_pointer<basic_json::string_t>) // NOLINT(readability/alt_tokens)\n    reference at(const ::nlohmann::json_pointer<BasicJsonType>& ptr)\n    {\n        return ptr.get_checked(this);\n    }\n\n    /// @brief access specified element via JSON Pointer\n    /// @sa https://json.nlohmann.me/api/basic_json/at/\n    const_reference at(const json_pointer& ptr) const\n    {\n        return ptr.get_checked(this);\n    }\n\n    template<typename BasicJsonType, detail::enable_if_t<detail::is_basic_json<BasicJsonType>::value, int> = 0>\n    JSON_HEDLEY_DEPRECATED_FOR(3.11.0, basic_json::json_pointer or nlohmann::json_pointer<basic_json::string_t>) // NOLINT(readability/alt_tokens)\n    const_reference at(const ::nlohmann::json_pointer<BasicJsonType>& ptr) const\n    {\n        return ptr.get_checked(this);\n    }\n\n    /// @brief return flattened JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/flatten/\n    basic_json flatten() const\n    {\n        basic_json result(value_t::object);\n        json_pointer::flatten(\"\", *this, result);\n        return result;\n    }\n\n    /// @brief unflatten a previously flattened JSON value\n    /// @sa https://json.nlohmann.me/api/basic_json/unflatten/\n    basic_json unflatten() const\n    {\n        return json_pointer::unflatten(*this);\n    }\n\n    /// @}\n\n    //////////////////////////\n    // JSON Patch functions //\n    //////////////////////////\n\n    /// @name JSON Patch functions\n    /// @{\n\n    /// @brief applies a JSON patch in-place without copying the object\n    /// @sa https://json.nlohmann.me/api/basic_json/patch/\n    void patch_inplace(const basic_json& json_patch)\n    {\n        basic_json& result = *this;\n        // the valid JSON Patch operations\n        enum class patch_operations {add, remove, replace, move, copy, test, invalid};\n\n        const auto get_op = [](const std::string & op)\n        {\n            if (op == \"add\")\n            {\n                return patch_operations::add;\n            }\n            if (op == \"remove\")\n            {\n                return patch_operations::remove;\n            }\n            if (op == \"replace\")\n            {\n                return patch_operations::replace;\n            }\n            if (op == \"move\")\n            {\n                return patch_operations::move;\n            }\n            if (op == \"copy\")\n            {\n                return patch_operations::copy;\n            }\n            if (op == \"test\")\n            {\n                return patch_operations::test;\n            }\n\n            return patch_operations::invalid;\n        };\n\n        // wrapper for \"add\" operation; add value at ptr\n        const auto operation_add = [&result](json_pointer & ptr, basic_json val)\n        {\n            // adding to the root of the target document means replacing it\n            if (ptr.empty())\n            {\n                result = val;\n                return;\n            }\n\n            // make sure the top element of the pointer exists\n            json_pointer top_pointer = ptr.top();\n            if (top_pointer != ptr)\n            {\n                result.at(top_pointer);\n            }\n\n            // get reference to parent of JSON pointer ptr\n            const auto last_path = ptr.back();\n            ptr.pop_back();\n            // parent must exist when performing patch add per RFC6902 specs\n            basic_json& parent = result.at(ptr);\n\n            switch (parent.m_type)\n            {\n                case value_t::null:\n                case value_t::object:\n                {\n                    // use operator[] to add value\n                    parent[last_path] = val;\n                    break;\n                }\n\n                case value_t::array:\n                {\n                    if (last_path == \"-\")\n                    {\n                        // special case: append to back\n                        parent.push_back(val);\n                    }\n                    else\n                    {\n                        const auto idx = json_pointer::template array_index<basic_json_t>(last_path);\n                        if (JSON_HEDLEY_UNLIKELY(idx > parent.size()))\n                        {\n                            // avoid undefined behavior\n                            JSON_THROW(out_of_range::create(401, detail::concat(\"array index \", std::to_string(idx), \" is out of range\"), &parent));\n                        }\n\n                        // default case: insert add offset\n                        parent.insert(parent.begin() + static_cast<difference_type>(idx), val);\n                    }\n                    break;\n                }\n\n                // if there exists a parent it cannot be primitive\n                case value_t::string: // LCOV_EXCL_LINE\n                case value_t::boolean: // LCOV_EXCL_LINE\n                case value_t::number_integer: // LCOV_EXCL_LINE\n                case value_t::number_unsigned: // LCOV_EXCL_LINE\n                case value_t::number_float: // LCOV_EXCL_LINE\n                case value_t::binary: // LCOV_EXCL_LINE\n                case value_t::discarded: // LCOV_EXCL_LINE\n                default:            // LCOV_EXCL_LINE\n                    JSON_ASSERT(false); // NOLINT(cert-dcl03-c,hicpp-static-assert,misc-static-assert) LCOV_EXCL_LINE\n            }\n        };\n\n        // wrapper for \"remove\" operation; remove value at ptr\n        const auto operation_remove = [this, &result](json_pointer & ptr)\n        {\n            // get reference to parent of JSON pointer ptr\n            const auto last_path = ptr.back();\n            ptr.pop_back();\n            basic_json& parent = result.at(ptr);\n\n            // remove child\n            if (parent.is_object())\n            {\n                // perform range check\n                auto it = parent.find(last_path);\n                if (JSON_HEDLEY_LIKELY(it != parent.end()))\n                {\n                    parent.erase(it);\n                }\n                else\n                {\n                    JSON_THROW(out_of_range::create(403, detail::concat(\"key '\", last_path, \"' not found\"), this));\n                }\n            }\n            else if (parent.is_array())\n            {\n                // note erase performs range check\n                parent.erase(json_pointer::template array_index<basic_json_t>(last_path));\n            }\n        };\n\n        // type check: top level value must be an array\n        if (JSON_HEDLEY_UNLIKELY(!json_patch.is_array()))\n        {\n            JSON_THROW(parse_error::create(104, 0, \"JSON patch must be an array of objects\", &json_patch));\n        }\n\n        // iterate and apply the operations\n        for (const auto& val : json_patch)\n        {\n            // wrapper to get a value for an operation\n            const auto get_value = [&val](const std::string & op,\n                                          const std::string & member,\n                                          bool string_type) -> basic_json &\n            {\n                // find value\n                auto it = val.m_value.object->find(member);\n\n                // context-sensitive error message\n                const auto error_msg = (op == \"op\") ? \"operation\" : detail::concat(\"operation '\", op, '\\'');\n\n                // check if desired value is present\n                if (JSON_HEDLEY_UNLIKELY(it == val.m_value.object->end()))\n                {\n                    // NOLINTNEXTLINE(performance-inefficient-string-concatenation)\n                    JSON_THROW(parse_error::create(105, 0, detail::concat(error_msg, \" must have member '\", member, \"'\"), &val));\n                }\n\n                // check if result is of type string\n                if (JSON_HEDLEY_UNLIKELY(string_type && !it->second.is_string()))\n                {\n                    // NOLINTNEXTLINE(performance-inefficient-string-concatenation)\n                    JSON_THROW(parse_error::create(105, 0, detail::concat(error_msg, \" must have string member '\", member, \"'\"), &val));\n                }\n\n                // no error: return value\n                return it->second;\n            };\n\n            // type check: every element of the array must be an object\n            if (JSON_HEDLEY_UNLIKELY(!val.is_object()))\n            {\n                JSON_THROW(parse_error::create(104, 0, \"JSON patch must be an array of objects\", &val));\n            }\n\n            // collect mandatory members\n            const auto op = get_value(\"op\", \"op\", true).template get<std::string>();\n            const auto path = get_value(op, \"path\", true).template get<std::string>();\n            json_pointer ptr(path);\n\n            switch (get_op(op))\n            {\n                case patch_operations::add:\n                {\n                    operation_add(ptr, get_value(\"add\", \"value\", false));\n                    break;\n                }\n\n                case patch_operations::remove:\n                {\n                    operation_remove(ptr);\n                    break;\n                }\n\n                case patch_operations::replace:\n                {\n                    // the \"path\" location must exist - use at()\n                    result.at(ptr) = get_value(\"replace\", \"value\", false);\n                    break;\n                }\n\n                case patch_operations::move:\n                {\n                    const auto from_path = get_value(\"move\", \"from\", true).template get<std::string>();\n                    json_pointer from_ptr(from_path);\n\n                    // the \"from\" location must exist - use at()\n                    basic_json v = result.at(from_ptr);\n\n                    // The move operation is functionally identical to a\n                    // \"remove\" operation on the \"from\" location, followed\n                    // immediately by an \"add\" operation at the target\n                    // location with the value that was just removed.\n                    operation_remove(from_ptr);\n                    operation_add(ptr, v);\n                    break;\n                }\n\n                case patch_operations::copy:\n                {\n                    const auto from_path = get_value(\"copy\", \"from\", true).template get<std::string>();\n                    const json_pointer from_ptr(from_path);\n\n                    // the \"from\" location must exist - use at()\n                    basic_json v = result.at(from_ptr);\n\n                    // The copy is functionally identical to an \"add\"\n                    // operation at the target location using the value\n                    // specified in the \"from\" member.\n                    operation_add(ptr, v);\n                    break;\n                }\n\n                case patch_operations::test:\n                {\n                    bool success = false;\n                    JSON_TRY\n                    {\n                        // check if \"value\" matches the one at \"path\"\n                        // the \"path\" location must exist - use at()\n                        success = (result.at(ptr) == get_value(\"test\", \"value\", false));\n                    }\n                    JSON_INTERNAL_CATCH (out_of_range&)\n                    {\n                        // ignore out of range errors: success remains false\n                    }\n\n                    // throw an exception if test fails\n                    if (JSON_HEDLEY_UNLIKELY(!success))\n                    {\n                        JSON_THROW(other_error::create(501, detail::concat(\"unsuccessful: \", val.dump()), &val));\n                    }\n\n                    break;\n                }\n\n                case patch_operations::invalid:\n                default:\n                {\n                    // op must be \"add\", \"remove\", \"replace\", \"move\", \"copy\", or\n                    // \"test\"\n                    JSON_THROW(parse_error::create(105, 0, detail::concat(\"operation value '\", op, \"' is invalid\"), &val));\n                }\n            }\n        }\n    }\n\n    /// @brief applies a JSON patch to a copy of the current object\n    /// @sa https://json.nlohmann.me/api/basic_json/patch/\n    basic_json patch(const basic_json& json_patch) const\n    {\n        basic_json result = *this;\n        result.patch_inplace(json_patch);\n        return result;\n    }\n\n    /// @brief creates a diff as a JSON patch\n    /// @sa https://json.nlohmann.me/api/basic_json/diff/\n    JSON_HEDLEY_WARN_UNUSED_RESULT\n    static basic_json diff(const basic_json& source, const basic_json& target,\n                           const std::string& path = \"\")\n    {\n        // the patch\n        basic_json result(value_t::array);\n\n        // if the values are the same, return empty patch\n        if (source == target)\n        {\n            return result;\n        }\n\n        if (source.type() != target.type())\n        {\n            // different types: replace value\n            result.push_back(\n            {\n                {\"op\", \"replace\"}, {\"path\", path}, {\"value\", target}\n            });\n            return result;\n        }\n\n        switch (source.type())\n        {\n            case value_t::array:\n            {\n                // first pass: traverse common elements\n                std::size_t i = 0;\n                while (i < source.size() && i < target.size())\n                {\n                    // recursive call to compare array values at index i\n                    auto temp_diff = diff(source[i], target[i], detail::concat(path, '/', std::to_string(i)));\n                    result.insert(result.end(), temp_diff.begin(), temp_diff.end());\n                    ++i;\n                }\n\n                // We now reached the end of at least one array\n                // in a second pass, traverse the remaining elements\n\n                // remove my remaining elements\n                const auto end_index = static_cast<difference_type>(result.size());\n                while (i < source.size())\n                {\n                    // add operations in reverse order to avoid invalid\n                    // indices\n                    result.insert(result.begin() + end_index, object(\n                    {\n                        {\"op\", \"remove\"},\n                        {\"path\", detail::concat(path, '/', std::to_string(i))}\n                    }));\n                    ++i;\n                }\n\n                // add other remaining elements\n                while (i < target.size())\n                {\n                    result.push_back(\n                    {\n                        {\"op\", \"add\"},\n                        {\"path\", detail::concat(path, \"/-\")},\n                        {\"value\", target[i]}\n                    });\n                    ++i;\n                }\n\n                break;\n            }\n\n            case value_t::object:\n            {\n                // first pass: traverse this object's elements\n                for (auto it = source.cbegin(); it != source.cend(); ++it)\n                {\n                    // escape the key name to be used in a JSON patch\n                    const auto path_key = detail::concat(path, '/', detail::escape(it.key()));\n\n                    if (target.find(it.key()) != target.end())\n                    {\n                        // recursive call to compare object values at key it\n                        auto temp_diff = diff(it.value(), target[it.key()], path_key);\n                        result.insert(result.end(), temp_diff.begin(), temp_diff.end());\n                    }\n                    else\n                    {\n                        // found a key that is not in o -> remove it\n                        result.push_back(object(\n                        {\n                            {\"op\", \"remove\"}, {\"path\", path_key}\n                        }));\n                    }\n                }\n\n                // second pass: traverse other object's elements\n                for (auto it = target.cbegin(); it != target.cend(); ++it)\n                {\n                    if (source.find(it.key()) == source.end())\n                    {\n                        // found a key that is not in this -> add it\n                        const auto path_key = detail::concat(path, '/', detail::escape(it.key()));\n                        result.push_back(\n                        {\n                            {\"op\", \"add\"}, {\"path\", path_key},\n                            {\"value\", it.value()}\n                        });\n                    }\n                }\n\n                break;\n            }\n\n            case value_t::null:\n            case value_t::string:\n            case value_t::boolean:\n            case value_t::number_integer:\n            case value_t::number_unsigned:\n            case value_t::number_float:\n            case value_t::binary:\n            case value_t::discarded:\n            default:\n            {\n                // both primitive type: replace value\n                result.push_back(\n                {\n                    {\"op\", \"replace\"}, {\"path\", path}, {\"value\", target}\n                });\n                break;\n            }\n        }\n\n        return result;\n    }\n    /// @}\n\n    ////////////////////////////////\n    // JSON Merge Patch functions //\n    ////////////////////////////////\n\n    /// @name JSON Merge Patch functions\n    /// @{\n\n    /// @brief applies a JSON Merge Patch\n    /// @sa https://json.nlohmann.me/api/basic_json/merge_patch/\n    void merge_patch(const basic_json& apply_patch)\n    {\n        if (apply_patch.is_object())\n        {\n            if (!is_object())\n            {\n                *this = object();\n            }\n            for (auto it = apply_patch.begin(); it != apply_patch.end(); ++it)\n            {\n                if (it.value().is_null())\n                {\n                    erase(it.key());\n                }\n                else\n                {\n                    operator[](it.key()).merge_patch(it.value());\n                }\n            }\n        }\n        else\n        {\n            *this = apply_patch;\n        }\n    }\n\n    /// @}\n};\n\n/// @brief user-defined to_string function for JSON values\n/// @sa https://json.nlohmann.me/api/basic_json/to_string/\nNLOHMANN_BASIC_JSON_TPL_DECLARATION\nstd::string to_string(const NLOHMANN_BASIC_JSON_TPL& j)\n{\n    return j.dump();\n}\n\ninline namespace literals\n{\ninline namespace json_literals\n{\n\n/// @brief user-defined string literal for JSON values\n/// @sa https://json.nlohmann.me/api/basic_json/operator_literal_json/\nJSON_HEDLEY_NON_NULL(1)\ninline nlohmann::json operator \"\" _json(const char* s, std::size_t n)\n{\n    return nlohmann::json::parse(s, s + n);\n}\n\n/// @brief user-defined string literal for JSON pointer\n/// @sa https://json.nlohmann.me/api/basic_json/operator_literal_json_pointer/\nJSON_HEDLEY_NON_NULL(1)\ninline nlohmann::json::json_pointer operator \"\" _json_pointer(const char* s, std::size_t n)\n{\n    return nlohmann::json::json_pointer(std::string(s, n));\n}\n\n}  // namespace json_literals\n}  // namespace literals\nNLOHMANN_JSON_NAMESPACE_END\n\n///////////////////////\n// nonmember support //\n///////////////////////\n\nnamespace std // NOLINT(cert-dcl58-cpp)\n{\n\n/// @brief hash value for JSON objects\n/// @sa https://json.nlohmann.me/api/basic_json/std_hash/\nNLOHMANN_BASIC_JSON_TPL_DECLARATION\nstruct hash<nlohmann::NLOHMANN_BASIC_JSON_TPL>\n{\n    std::size_t operator()(const nlohmann::NLOHMANN_BASIC_JSON_TPL& j) const\n    {\n        return nlohmann::detail::hash(j);\n    }\n};\n\n// specialization for std::less<value_t>\ntemplate<>\nstruct less< ::nlohmann::detail::value_t> // do not remove the space after '<', see https://github.com/nlohmann/json/pull/679\n{\n    /*!\n    @brief compare two value_t enum values\n    @since version 3.0.0\n    */\n    bool operator()(::nlohmann::detail::value_t lhs,\n                    ::nlohmann::detail::value_t rhs) const noexcept\n    {\n#if JSON_HAS_THREE_WAY_COMPARISON\n        return std::is_lt(lhs <=> rhs); // *NOPAD*\n#else\n        return ::nlohmann::detail::operator<(lhs, rhs);\n#endif\n    }\n};\n\n// C++20 prohibit function specialization in the std namespace.\n#ifndef JSON_HAS_CPP_20\n\n/// @brief exchanges the values of two JSON objects\n/// @sa https://json.nlohmann.me/api/basic_json/std_swap/\nNLOHMANN_BASIC_JSON_TPL_DECLARATION\ninline void swap(nlohmann::NLOHMANN_BASIC_JSON_TPL& j1, nlohmann::NLOHMANN_BASIC_JSON_TPL& j2) noexcept(  // NOLINT(readability-inconsistent-declaration-parameter-name)\n    is_nothrow_move_constructible<nlohmann::NLOHMANN_BASIC_JSON_TPL>::value&&                          // NOLINT(misc-redundant-expression)\n    is_nothrow_move_assignable<nlohmann::NLOHMANN_BASIC_JSON_TPL>::value)\n{\n    j1.swap(j2);\n}\n\n#endif\n\n}  // namespace std\n\n#if JSON_USE_GLOBAL_UDLS\n    using nlohmann::literals::json_literals::operator \"\" _json; // NOLINT(misc-unused-using-decls,google-global-names-in-headers)\n    using nlohmann::literals::json_literals::operator \"\" _json_pointer; //NOLINT(misc-unused-using-decls,google-global-names-in-headers)\n#endif\n\n// #include <nlohmann/detail/macro_unscope.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n// restore clang diagnostic settings\n#if defined(__clang__)\n    #pragma clang diagnostic pop\n#endif\n\n// clean up\n#undef JSON_ASSERT\n#undef JSON_INTERNAL_CATCH\n#undef JSON_THROW\n#undef JSON_PRIVATE_UNLESS_TESTED\n#undef NLOHMANN_BASIC_JSON_TPL_DECLARATION\n#undef NLOHMANN_BASIC_JSON_TPL\n#undef JSON_EXPLICIT\n#undef NLOHMANN_CAN_CALL_STD_FUNC_IMPL\n#undef JSON_INLINE_VARIABLE\n#undef JSON_NO_UNIQUE_ADDRESS\n#undef JSON_DISABLE_ENUM_SERIALIZATION\n#undef JSON_USE_GLOBAL_UDLS\n\n#ifndef JSON_TEST_KEEP_MACROS\n    #undef JSON_CATCH\n    #undef JSON_TRY\n    #undef JSON_HAS_CPP_11\n    #undef JSON_HAS_CPP_14\n    #undef JSON_HAS_CPP_17\n    #undef JSON_HAS_CPP_20\n    #undef JSON_HAS_FILESYSTEM\n    #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM\n    #undef JSON_HAS_THREE_WAY_COMPARISON\n    #undef JSON_HAS_RANGES\n    #undef JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON\n#endif\n\n// #include <nlohmann/thirdparty/hedley/hedley_undef.hpp>\n//     __ _____ _____ _____\n//  __|  |   __|     |   | |  JSON for Modern C++\n// |  |  |__   |  |  | | | |  version 3.11.2\n// |_____|_____|_____|_|___|  https://github.com/nlohmann/json\n//\n// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann <https://nlohmann.me>\n// SPDX-License-Identifier: MIT\n\n\n\n#undef JSON_HEDLEY_ALWAYS_INLINE\n#undef JSON_HEDLEY_ARM_VERSION\n#undef JSON_HEDLEY_ARM_VERSION_CHECK\n#undef JSON_HEDLEY_ARRAY_PARAM\n#undef JSON_HEDLEY_ASSUME\n#undef JSON_HEDLEY_BEGIN_C_DECLS\n#undef JSON_HEDLEY_CLANG_HAS_ATTRIBUTE\n#undef JSON_HEDLEY_CLANG_HAS_BUILTIN\n#undef JSON_HEDLEY_CLANG_HAS_CPP_ATTRIBUTE\n#undef JSON_HEDLEY_CLANG_HAS_DECLSPEC_DECLSPEC_ATTRIBUTE\n#undef JSON_HEDLEY_CLANG_HAS_EXTENSION\n#undef JSON_HEDLEY_CLANG_HAS_FEATURE\n#undef JSON_HEDLEY_CLANG_HAS_WARNING\n#undef JSON_HEDLEY_COMPCERT_VERSION\n#undef JSON_HEDLEY_COMPCERT_VERSION_CHECK\n#undef JSON_HEDLEY_CONCAT\n#undef JSON_HEDLEY_CONCAT3\n#undef JSON_HEDLEY_CONCAT3_EX\n#undef JSON_HEDLEY_CONCAT_EX\n#undef JSON_HEDLEY_CONST\n#undef JSON_HEDLEY_CONSTEXPR\n#undef JSON_HEDLEY_CONST_CAST\n#undef JSON_HEDLEY_CPP_CAST\n#undef JSON_HEDLEY_CRAY_VERSION\n#undef JSON_HEDLEY_CRAY_VERSION_CHECK\n#undef JSON_HEDLEY_C_DECL\n#undef JSON_HEDLEY_DEPRECATED\n#undef JSON_HEDLEY_DEPRECATED_FOR\n#undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_CAST_QUAL\n#undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_CPP98_COMPAT_WRAP_\n#undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_DEPRECATED\n#undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_CPP_ATTRIBUTES\n#undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNKNOWN_PRAGMAS\n#undef JSON_HEDLEY_DIAGNOSTIC_DISABLE_UNUSED_FUNCTION\n#undef JSON_HEDLEY_DIAGNOSTIC_POP\n#undef JSON_HEDLEY_DIAGNOSTIC_PUSH\n#undef JSON_HEDLEY_DMC_VERSION\n#undef JSON_HEDLEY_DMC_VERSION_CHECK\n#undef JSON_HEDLEY_EMPTY_BASES\n#undef JSON_HEDLEY_EMSCRIPTEN_VERSION\n#undef JSON_HEDLEY_EMSCRIPTEN_VERSION_CHECK\n#undef JSON_HEDLEY_END_C_DECLS\n#undef JSON_HEDLEY_FLAGS\n#undef JSON_HEDLEY_FLAGS_CAST\n#undef JSON_HEDLEY_GCC_HAS_ATTRIBUTE\n#undef JSON_HEDLEY_GCC_HAS_BUILTIN\n#undef JSON_HEDLEY_GCC_HAS_CPP_ATTRIBUTE\n#undef JSON_HEDLEY_GCC_HAS_DECLSPEC_ATTRIBUTE\n#undef JSON_HEDLEY_GCC_HAS_EXTENSION\n#undef JSON_HEDLEY_GCC_HAS_FEATURE\n#undef JSON_HEDLEY_GCC_HAS_WARNING\n#undef JSON_HEDLEY_GCC_NOT_CLANG_VERSION_CHECK\n#undef JSON_HEDLEY_GCC_VERSION\n#undef JSON_HEDLEY_GCC_VERSION_CHECK\n#undef JSON_HEDLEY_GNUC_HAS_ATTRIBUTE\n#undef JSON_HEDLEY_GNUC_HAS_BUILTIN\n#undef JSON_HEDLEY_GNUC_HAS_CPP_ATTRIBUTE\n#undef JSON_HEDLEY_GNUC_HAS_DECLSPEC_ATTRIBUTE\n#undef JSON_HEDLEY_GNUC_HAS_EXTENSION\n#undef JSON_HEDLEY_GNUC_HAS_FEATURE\n#undef JSON_HEDLEY_GNUC_HAS_WARNING\n#undef JSON_HEDLEY_GNUC_VERSION\n#undef JSON_HEDLEY_GNUC_VERSION_CHECK\n#undef JSON_HEDLEY_HAS_ATTRIBUTE\n#undef JSON_HEDLEY_HAS_BUILTIN\n#undef JSON_HEDLEY_HAS_CPP_ATTRIBUTE\n#undef JSON_HEDLEY_HAS_CPP_ATTRIBUTE_NS\n#undef JSON_HEDLEY_HAS_DECLSPEC_ATTRIBUTE\n#undef JSON_HEDLEY_HAS_EXTENSION\n#undef JSON_HEDLEY_HAS_FEATURE\n#undef JSON_HEDLEY_HAS_WARNING\n#undef JSON_HEDLEY_IAR_VERSION\n#undef JSON_HEDLEY_IAR_VERSION_CHECK\n#undef JSON_HEDLEY_IBM_VERSION\n#undef JSON_HEDLEY_IBM_VERSION_CHECK\n#undef JSON_HEDLEY_IMPORT\n#undef JSON_HEDLEY_INLINE\n#undef JSON_HEDLEY_INTEL_CL_VERSION\n#undef JSON_HEDLEY_INTEL_CL_VERSION_CHECK\n#undef JSON_HEDLEY_INTEL_VERSION\n#undef JSON_HEDLEY_INTEL_VERSION_CHECK\n#undef JSON_HEDLEY_IS_CONSTANT\n#undef JSON_HEDLEY_IS_CONSTEXPR_\n#undef JSON_HEDLEY_LIKELY\n#undef JSON_HEDLEY_MALLOC\n#undef JSON_HEDLEY_MCST_LCC_VERSION\n#undef JSON_HEDLEY_MCST_LCC_VERSION_CHECK\n#undef JSON_HEDLEY_MESSAGE\n#undef JSON_HEDLEY_MSVC_VERSION\n#undef JSON_HEDLEY_MSVC_VERSION_CHECK\n#undef JSON_HEDLEY_NEVER_INLINE\n#undef JSON_HEDLEY_NON_NULL\n#undef JSON_HEDLEY_NO_ESCAPE\n#undef JSON_HEDLEY_NO_RETURN\n#undef JSON_HEDLEY_NO_THROW\n#undef JSON_HEDLEY_NULL\n#undef JSON_HEDLEY_PELLES_VERSION\n#undef JSON_HEDLEY_PELLES_VERSION_CHECK\n#undef JSON_HEDLEY_PGI_VERSION\n#undef JSON_HEDLEY_PGI_VERSION_CHECK\n#undef JSON_HEDLEY_PREDICT\n#undef JSON_HEDLEY_PRINTF_FORMAT\n#undef JSON_HEDLEY_PRIVATE\n#undef JSON_HEDLEY_PUBLIC\n#undef JSON_HEDLEY_PURE\n#undef JSON_HEDLEY_REINTERPRET_CAST\n#undef JSON_HEDLEY_REQUIRE\n#undef JSON_HEDLEY_REQUIRE_CONSTEXPR\n#undef JSON_HEDLEY_REQUIRE_MSG\n#undef JSON_HEDLEY_RESTRICT\n#undef JSON_HEDLEY_RETURNS_NON_NULL\n#undef JSON_HEDLEY_SENTINEL\n#undef JSON_HEDLEY_STATIC_ASSERT\n#undef JSON_HEDLEY_STATIC_CAST\n#undef JSON_HEDLEY_STRINGIFY\n#undef JSON_HEDLEY_STRINGIFY_EX\n#undef JSON_HEDLEY_SUNPRO_VERSION\n#undef JSON_HEDLEY_SUNPRO_VERSION_CHECK\n#undef JSON_HEDLEY_TINYC_VERSION\n#undef JSON_HEDLEY_TINYC_VERSION_CHECK\n#undef JSON_HEDLEY_TI_ARMCL_VERSION\n#undef JSON_HEDLEY_TI_ARMCL_VERSION_CHECK\n#undef JSON_HEDLEY_TI_CL2000_VERSION\n#undef JSON_HEDLEY_TI_CL2000_VERSION_CHECK\n#undef JSON_HEDLEY_TI_CL430_VERSION\n#undef JSON_HEDLEY_TI_CL430_VERSION_CHECK\n#undef JSON_HEDLEY_TI_CL6X_VERSION\n#undef JSON_HEDLEY_TI_CL6X_VERSION_CHECK\n#undef JSON_HEDLEY_TI_CL7X_VERSION\n#undef JSON_HEDLEY_TI_CL7X_VERSION_CHECK\n#undef JSON_HEDLEY_TI_CLPRU_VERSION\n#undef JSON_HEDLEY_TI_CLPRU_VERSION_CHECK\n#undef JSON_HEDLEY_TI_VERSION\n#undef JSON_HEDLEY_TI_VERSION_CHECK\n#undef JSON_HEDLEY_UNAVAILABLE\n#undef JSON_HEDLEY_UNLIKELY\n#undef JSON_HEDLEY_UNPREDICTABLE\n#undef JSON_HEDLEY_UNREACHABLE\n#undef JSON_HEDLEY_UNREACHABLE_RETURN\n#undef JSON_HEDLEY_VERSION\n#undef JSON_HEDLEY_VERSION_DECODE_MAJOR\n#undef JSON_HEDLEY_VERSION_DECODE_MINOR\n#undef JSON_HEDLEY_VERSION_DECODE_REVISION\n#undef JSON_HEDLEY_VERSION_ENCODE\n#undef JSON_HEDLEY_WARNING\n#undef JSON_HEDLEY_WARN_UNUSED_RESULT\n#undef JSON_HEDLEY_WARN_UNUSED_RESULT_MSG\n#undef JSON_HEDLEY_FALL_THROUGH\n\n\n\n#endif  // INCLUDE_NLOHMANN_JSON_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gems/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_gems</name>\n  <version>4.4.0</version>\n  <description>Headers that are used by Isaac GXF extensions.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gxf_helpers/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_gxf_helpers LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gxf_helpers/gxf/extensions/gxf_helpers/parameter_parser_isaac.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"gems/core/constants.hpp\"\n#include \"gems/core/math/pose2.hpp\"\n#include \"gems/core/math/pose3.hpp\"\n#include \"gems/core/math/types.hpp\"\n#include \"gems/geometry/n_cuboid.hpp\"\n#include \"gems/geometry/n_sphere.hpp\"\n#include \"gems/geometry/polygon.hpp\"\n#include \"gems/uuid/uuid.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/core/parameter_parser.hpp\"\n#include \"gxf/core/parameter_parser_std.hpp\"\n#include \"yaml-cpp/yaml.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// Parameter support for std::pair.\n// Example format: [object1, object2]\ntemplate <typename T, typename N>\nstruct ParameterParser<std::pair<T, N>> {\n  static ::nvidia::gxf::Expected<std::pair<T, N>> Parse(\n      gxf_context_t context, gxf_uid_t component_uid,\n      const char* key, const YAML::Node& node,\n      const std::string& prefix) {\n    constexpr int kPairSize = 2;\n    if (!node.IsSequence()) {\n      GXF_LOG_ERROR(\"'%s' needs to be a sequence\", key);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    const size_t yaml_size = node.size();\n    if (static_cast<int>(yaml_size) != kPairSize) {\n      GXF_LOG_ERROR(\"'%s' is a sequence of %zu elements. Expected %d in pair.\", key, yaml_size,\n          kPairSize);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    std::pair<T, N> result;\n    const auto maybe_first = ParameterParser<T>::Parse(context, component_uid, key, node[0],\n        prefix);\n    if (!maybe_first) {\n      return ForwardError(maybe_first);\n    }\n    result.first = std::move(maybe_first.value());\n    const auto maybe_second = ParameterParser<N>::Parse(context, component_uid, key, node[1],\n        prefix);\n    if (!maybe_second) {\n      return ForwardError(maybe_second);\n    }\n    result.second = std::move(maybe_second.value());\n    return result;\n  }\n};\n\n// Parameter support for Vectors.\n// Example format: [1.0, 2.2, -3.7]\ntemplate <typename T, int N>\nstruct ParameterParser<::nvidia::isaac::Vector<T, N>> {\n  static ::nvidia::gxf::Expected<::nvidia::isaac::Vector<T, N>> Parse(\n      gxf_context_t context, gxf_uid_t component_uid,\n      const char* key, const YAML::Node& node,\n      const std::string& prefix) {\n    if (!node.IsSequence()) {\n      GXF_LOG_ERROR(\"'%s' needs to be a sequence\", key);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    const size_t yaml_size = node.size();\n    if (N != Eigen::Dynamic && static_cast<int>(yaml_size) != N) {\n      GXF_LOG_ERROR(\"'%s' is a sequence of %zu elements. Expected %d\", key, yaml_size, N);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    ::nvidia::isaac::Vector<T, N> result(yaml_size);\n    for (size_t i = 0; i < yaml_size; i++) {\n      const auto maybe = ParameterParser<T>::Parse(context, component_uid, key, node[i], prefix);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      result[i] = std::move(maybe.value());\n    }\n    return result;\n  }\n};\n\n// Parameter support for matrices (row-major specification)\n// Example format: [[1.0, 2.2, -3.7], [0.3, -1.1, 2.7]]\ntemplate <typename T, int N, int M>\nstruct ParameterParser<::nvidia::isaac::Matrix<T, N, M>> {\n  static ::nvidia::gxf::Expected<::nvidia::isaac::Matrix<T, N, M>> Parse(\n      gxf_context_t context, gxf_uid_t component_uid,\n      const char* key, const YAML::Node& node,\n      const std::string& prefix) {\n    // Get the number of columns\n    if (!node.IsSequence()) {\n      GXF_LOG_ERROR(\"'%s' needs to be a sequence\", key);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    const int32_t rows = node.size();\n    if (N != Eigen::Dynamic && rows != N) {\n      GXF_LOG_ERROR(\"'%s' is a sequence of %d elements. Expected %d\", key, rows, N);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    if (rows <= 0) {\n      GXF_LOG_ERROR(\"Number of rows (%d) must be greater than 0.\", rows);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n\n    // Get and check the number of columns\n    int32_t cols = 0;\n    for (int32_t i = 0; i < rows; i++) {\n      const auto& sub_node = node[i];\n      if (!sub_node.IsSequence()) {\n        GXF_LOG_ERROR(\"'%s' needs to be a sequence of sequences\", key);\n        return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n      }\n      const int32_t current_cols = sub_node.size();\n      if (M != Eigen::Dynamic && current_cols != M) {\n        GXF_LOG_ERROR(\"'%s' has a sub sequence of %d elements. Expected %d\",\n                      key, current_cols, M);\n        return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n      }\n      if (current_cols <= 0) {\n        GXF_LOG_ERROR(\"Number of columns (%d) must be greater than 0.\", current_cols);\n        return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n      }\n      if (i == 0) {\n        cols = current_cols;\n      }\n      if (current_cols != cols) {\n        GXF_LOG_ERROR(\"All rows must have the same length: %d vs %d\", current_cols, cols);\n        return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n      }\n    }\n\n    // Parse elements\n    ::nvidia::isaac::Matrix<T, N, M> result(rows, cols);\n    for (int32_t i = 0; i < rows; i++) {\n      for (int32_t j = 0; j < cols; j++) {\n        auto maybe = ParameterParser<T>::Parse(context, component_uid, key, node[i][j], prefix);\n        if (!maybe) {\n          return ForwardError(maybe);\n        }\n        result(i, j) = std::move(maybe.value());\n      }\n    }\n    return result;\n  }\n};\n\n// Parameter support for 2D pose.\n// Example formats:\n//      translation: [12.2, 8.7]\n//      rotation: 3.14 # radians\n// or\n//      translation: [12.2, 8.7]\n//      rotation_deg: 90.0 # degrees\ntemplate <typename T>\nstruct ParameterParser<::nvidia::isaac::Pose2<T>> {\n  static ::nvidia::gxf::Expected<::nvidia::isaac::Pose2<T>> Parse(\n      gxf_context_t context, gxf_uid_t component_uid, const char* key, const YAML::Node& node,\n      const std::string& prefix) {\n    if (!node.IsMap()) {\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n\n    // translation\n    constexpr char kTranslation[] = \"translation\";\n    const auto maybe_node_translation = node[kTranslation];\n    if (!maybe_node_translation) {\n      GXF_LOG_ERROR(\"Could not find '%s' in '%s' parameter\", kTranslation, key);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    const auto maybe_translation = ParameterParser<::nvidia::isaac::Vector2<T>>::Parse(\n        context, component_uid, kTranslation, maybe_node_translation, prefix);\n    if (!maybe_translation) {\n      return ForwardError(maybe_translation);\n    }\n    const ::nvidia::isaac::Vector2<T>& translation = maybe_translation.value();\n\n    // angle\n    constexpr char kRotation[] = \"rotation\";\n    constexpr char kRotationDeg[] = \"rotation_deg\";\n    T angle = T(0);\n    if (const auto maybe_node = node[kRotation]) {\n      const auto maybe =\n          ParameterParser<T>::Parse(context, component_uid, kRotation, maybe_node, prefix);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      angle = maybe.value();\n    } else if (const auto maybe_node = node[kRotationDeg]) {\n      const auto maybe =\n          ParameterParser<T>::Parse(context, component_uid, kRotationDeg, maybe_node, prefix);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      angle = ::nvidia::isaac::DegToRad(maybe.value());\n    } else {\n      GXF_LOG_ERROR(\"Could not find '%s' or '%s' in '%s' parameter\", kRotation, kRotationDeg, key);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n\n    return ::nvidia::isaac::Pose2<T>::FromXYA(translation.x(), translation.y(), angle);\n  }\n};\n\n// Parameter support for 3D pose.\n// Example formats:\n//      translation: [2.2, 8.7, 0.0]\n//      rotation_rpy: [0.0, 90.0, -180.0] # degrees\n// or\n//      translation: [2.2, 8.7, 0.0]\n//      rotation: [-0.393, -0.469, -0.725, 0.314] # (w, x, y, z) values forming the quaternion\ntemplate <typename T>\nstruct ParameterParser<::nvidia::isaac::Pose3<T>> {\n  static ::nvidia::gxf::Expected<::nvidia::isaac::Pose3<T>> Parse(\n      gxf_context_t context, gxf_uid_t component_uid, const char* key, const YAML::Node& node,\n      const std::string& prefix) {\n    if (!node.IsMap()) {\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n\n    // translation\n    constexpr char kTranslation[] = \"translation\";\n    const auto maybe_node_translation = node[kTranslation];\n    if (!maybe_node_translation) {\n      GXF_LOG_ERROR(\"Could not find '%s' in '%s' parameter\", kTranslation, key);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    const auto maybe_translation = ParameterParser<::nvidia::isaac::Vector3<T>>::Parse(\n        context, component_uid, kTranslation, maybe_node_translation, prefix);\n    if (!maybe_translation) {\n      return ForwardError(maybe_translation);\n    }\n    const ::nvidia::isaac::Vector3<T>& translation = maybe_translation.value();\n\n    // rotation\n    constexpr char kRotation[] = \"rotation\";\n    constexpr char kRotationRpy[] = \"rotation_rpy\";\n    ::nvidia::isaac::SO3<T> rotation = ::nvidia::isaac::SO3<T>::Identity();\n    if (const auto maybe_node = node[kRotation]) {\n      const auto maybe = ParameterParser<::nvidia::isaac::Vector4<T>>::Parse(context, component_uid,\n                                                                     kRotation, maybe_node,\n                                                                     prefix);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      rotation = ::nvidia::isaac::SO3<T>::FromQuaternion(\n          {maybe.value()[0], maybe.value()[1], maybe.value()[2], maybe.value()[3]});\n    } else if (const auto maybe_node = node[kRotationRpy]) {\n      const auto maybe = ParameterParser<::nvidia::isaac::Vector3<T>>::Parse(context, component_uid,\n                                                                     kRotationRpy, maybe_node,\n                                                                     prefix);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      rotation = ::nvidia::isaac::SO3<T>::FromEulerAnglesRPY(\n          ::nvidia::isaac::DegToRad(maybe.value()[0]),\n          ::nvidia::isaac::DegToRad(maybe.value()[1]),\n          ::nvidia::isaac::DegToRad(maybe.value()[2]));\n    } else {\n      GXF_LOG_ERROR(\"Could not find '%s' or '%s' in '%s' parameter\", kRotation, kRotationRpy, key);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n\n    return ::nvidia::isaac::Pose3<T>{rotation, translation};\n  }\n};\n\n// Parameter support for NSphere.\n// Example formats:\n//      center: [X, Y [, ...]]\n//      radius: 3.0\ntemplate <typename T, int N>\nstruct ParameterParser<::nvidia::isaac::geometry::NSphere<T, N>> {\n  static ::nvidia::gxf::Expected<::nvidia::isaac::geometry::NSphere<T, N>> Parse(\n      gxf_context_t context, gxf_uid_t component_uid,\n      const char* key, const YAML::Node& node, const std::string& prefix) {\n    if (!node.IsMap()) {\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n\n    // center\n    constexpr char kCenter[] = \"center\";\n    const auto maybe_node_center = node[kCenter];\n    if (!maybe_node_center) {\n      GXF_LOG_ERROR(\"Could not find '%s' in '%s' parameter\", kCenter, key);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    const auto maybe_center = ParameterParser<::nvidia::isaac::Vector<T, N>>::Parse(\n        context, component_uid, kCenter, maybe_node_center, prefix);\n    if (!maybe_center) {\n      return ForwardError(maybe_center);\n    }\n    const ::nvidia::isaac::Vector<T, N>& center = maybe_center.value();\n\n    // radius\n    constexpr char kRadius[] = \"radius\";\n    const auto maybe_node_radius = node[kRadius];\n    if (!maybe_node_radius) {\n      GXF_LOG_ERROR(\"Could not find '%s' in '%s' parameter\", kRadius, key);\n      return Unexpected{GXF_PARAMETER_PARSER_ERROR};\n    }\n    const auto maybe_radius =\n        ParameterParser<T>::Parse(context, component_uid, kRadius, maybe_node_radius, prefix);\n    if (!maybe_radius) {\n      return ForwardError(maybe_radius);\n    }\n    const T radius = maybe_radius.value();\n\n    return ::nvidia::isaac::geometry::NSphere<T, N>{center, radius};\n  }\n};\n\n// Parameter support for NCuboid (where N is the dimension of the cuboid). The input is a std::array\n// of length N, for which each component contains an the minimum and maximum bound for Nth dimension\n// Example: [[-1.0, 1.0], [-2.0, 2.0]] represents a rectangle with the x dimension spanning -1.0 to\n// 1.0 and the y dimension spanning -2.0 to 2.0.\ntemplate <typename T, int N>\nstruct ParameterParser<::nvidia::isaac::geometry::NCuboid<T, N>> {\n  static ::nvidia::gxf::Expected<::nvidia::isaac::geometry::NCuboid<T, N>> Parse(\n    gxf_context_t context, gxf_uid_t component_uid, const char* key, const YAML::Node& node,\n    const std::string& prefix) {\n    const auto maybe = ParameterParser<std::array<::nvidia::isaac::Vector2<T>, N>>::Parse(\n        context, component_uid, key, node, prefix);\n    if (!maybe) {\n      return ForwardError(maybe);\n    }\n    return ::nvidia::isaac::geometry::NCuboid<T, N>::FromBoundingCuboid(maybe.value());\n  }\n};\n\n// Parameter support for Polygon2. Example format:\n// [[60, -104], [32, -96], [-6, -88], [-52, -81], [-95, -82]]\n// This would form a polygon with 5 points in 2D. Each point consist of [x, y] values.\ntemplate <typename T>\nstruct ParameterParser<::nvidia::isaac::geometry::Polygon2<T>> {\n  static ::nvidia::gxf::Expected<::nvidia::isaac::geometry::Polygon2<T>> Parse(\n    gxf_context_t context, gxf_uid_t component_uid, const char* key, const YAML::Node& node,\n    const std::string& prefix) {\n    const auto maybe = ParameterParser<std::vector<::nvidia::isaac::Vector2<T>>>::Parse(\n        context, component_uid, key, node, prefix);\n    if (!maybe) {\n      return ForwardError(maybe);\n    }\n    return ::nvidia::isaac::geometry::Polygon2<T>{maybe.value()};\n  }\n};\n\n// Parameter support for LineSegment. Example format:\n//   [ [-100.0, 0.0], [20.0, 5.0] ]\n// This would form a line segment between (-100.0, 0.0) and (20.0, 5.0).\n// Each point consist of (x, y) values.\ntemplate <typename T, int N>\nstruct ParameterParser<::nvidia::isaac::geometry::LineSegment<T, N>> {\n  static ::nvidia::gxf::Expected<::nvidia::isaac::geometry::LineSegment<T, N>> Parse(\n    gxf_context_t context, gxf_uid_t component_uid, const char* key, const YAML::Node& node,\n    const std::string& prefix) {\n    const auto maybe = ParameterParser<std::array<::nvidia::isaac::Vector<T, N>, 2>>::Parse(\n        context, component_uid, key, node, prefix);\n    if (!maybe) {\n      return ForwardError(maybe);\n    }\n    return ::nvidia::isaac::geometry::LineSegment<T, N>::FromPoints(\n        maybe.value()[0], maybe.value()[1]);\n  }\n};\n\n// Parameter support for Uuid.\ntemplate <>\nstruct ParameterParser<::nvidia::isaac::Uuid> {\n  static ::nvidia::gxf::Expected<::nvidia::isaac::Uuid> Parse(\n      gxf_context_t context, gxf_uid_t component_uid, const char* key,\n      const YAML::Node& node, const std::string& prefix) {\n    auto str = ParameterParser<std::string>::Parse(context, component_uid, key, node, prefix);\n    if (!str) {\n      return ForwardError(str);\n    }\n    return ::nvidia::isaac::Uuid::FromString(str.value());\n  }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gxf_helpers/gxf/extensions/gxf_helpers/parameter_wrapper_isaac.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n#include <unordered_map>\n#include <utility>\n#include <vector>\n\n#include \"gems/core/constants.hpp\"\n#include \"gems/core/math/pose2.hpp\"\n#include \"gems/core/math/pose3.hpp\"\n#include \"gems/core/math/types.hpp\"\n#include \"gems/geometry/n_cuboid.hpp\"\n#include \"gems/geometry/n_sphere.hpp\"\n#include \"gems/geometry/polygon.hpp\"\n#include \"gems/uuid/uuid.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/core/parameter_wrapper.hpp\"\n#include \"yaml-cpp/yaml.h\"\n\nnamespace nvidia {\nnamespace gxf {\n\n// uint8_t by default is interpreted as unsigned char, however value above 127 are not valid\n// and json failed to serialize as the string is not a valid utf-8 string.\ntemplate<>\nstruct ParameterWrapper<uint8_t> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context, uint8_t value) {\n    YAML::Node node(static_cast<int>(value));\n    return node;\n  }\n};\n\ntemplate <typename T, typename N>\nstruct ParameterWrapper<std::pair<T, N>> {\n  static Expected<YAML::Node> Wrap(\n      gxf_context_t context, const std::pair<T, N>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n\n    const auto maybe_first = ParameterWrapper<T>::Wrap(context, value.first);\n    if (!maybe_first) {\n      return ForwardError(maybe_first);\n    }\n    node.push_back(maybe_first.value());\n\n    const auto maybe_second = ParameterWrapper<N>::Wrap(context, value.second);\n    if (!maybe_second) {\n      return ForwardError(maybe_second);\n    }\n    node.push_back(maybe_second.value());\n\n    return node;\n  }\n};\n\ntemplate <typename T, int N>\nstruct ParameterWrapper<::nvidia::isaac::Vector<T, N>> {\n  static Expected<YAML::Node> Wrap(\n      gxf_context_t context, const ::nvidia::isaac::Vector<T, N>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n\n    for (uint32_t i = 0; i < value.size(); i++) {\n      const auto maybe = ParameterWrapper<T>::Wrap(context, value[i]);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n// Parameter support for matrices (row-major specification)\n// Example format: [[1.0, 2.2, -3.7], [0.3, -1.1, 2.7]]\ntemplate <typename T, int N, int M>\nstruct ParameterWrapper<::nvidia::isaac::Matrix<T, N, M>> {\n  static Expected<YAML::Node> Wrap(\n      gxf_context_t context, const ::nvidia::isaac::Matrix<T, N, M>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n\n    for (int32_t i = 0; i < N; i++) {\n      for (int32_t j = 0; j < M; j++) {\n        auto maybe = ParameterWrapper<T>::Wrap(context, value(i, j));\n        if (!maybe) {\n          return ForwardError(maybe);\n        }\n        node.push_back(maybe.value());\n      }\n    }\n    return node;\n  }\n};\n\n// Parameter support for matrices (row-major specification)\n// Example format: [[1.0, 2.2, -3.7], [0.3, -1.1, 2.7]]\ntemplate <typename T, int N, int M>\nstruct ParameterWrapper<std::vector<::nvidia::isaac::Matrix<T, N, M>>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context,\n                                   const std::vector<::nvidia::isaac::Matrix<T, N, M>>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n\n    for (auto &v : value) {\n      const auto maybe = ParameterWrapper<::nvidia::isaac::Matrix<T, N, M>>::Wrap(context, v);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n// Parameter support for 2D pose.\n// Example formats:\n//      translation: [12.2, 8.7]\n//      rotation: 3.14 # radians\n// or\n//      translation: [12.2, 8.7]\n//      rotation_deg: 90.0 # degrees\ntemplate <typename T>\nstruct ParameterWrapper<::nvidia::isaac::Pose2<T>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context, const ::nvidia::isaac::Pose2<T>& value) {\n    YAML::Node node(YAML::NodeType::Map);\n\n    // translation\n    const auto maybe_translation =\n      ParameterWrapper<Eigen::Matrix<T, 2, 1>>::Wrap(context, value.translation);\n    if (!maybe_translation) {\n      return ForwardError(maybe_translation);\n    }\n    node[\"translation\"] = maybe_translation.value();\n\n    // rotation\n    const auto maybe_rotation =\n        ParameterWrapper<T>::Wrap(context, value.rotation.angle());\n    if (!maybe_rotation) {\n      return ForwardError(maybe_rotation);\n    }\n    node[\"rotation\"] = maybe_rotation.value();\n    return node;\n  }\n};\n\n// Support for std::vector of Pose2\ntemplate <typename T>\nstruct ParameterWrapper<std::vector<::nvidia::isaac::Pose2<T>>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context,\n                                   const std::vector<::nvidia::isaac::Pose2<T>>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n    for (auto &v : value) {\n      const auto maybe = ParameterWrapper<::nvidia::isaac::Pose2<T>>::Wrap(context, v);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n// Parameter support for 3D pose.\n// Example formats:\n//      translation: [2.2, 8.7, 0.0]\n//      rotation_rpy: [0.0, 90.0, -180.0] # degrees\n// or\n//      translation: [2.2, 8.7, 0.0]\n//      rotation: [-0.393, -0.469, -0.725, 0.314] # (w, x, y, z) values forming the quaternion\ntemplate <typename T>\nstruct ParameterWrapper<::nvidia::isaac::Pose3<T>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context, const ::nvidia::isaac::Pose3<T>& value) {\n    YAML::Node node(YAML::NodeType::Map);\n    // translation\n    const auto maybe_translation =\n      ParameterWrapper<Eigen::Matrix<T, 3, 1>>::Wrap(context, value.translation);\n    if (!maybe_translation) {\n      return ForwardError(maybe_translation);\n    }\n    node[\"translation\"] = maybe_translation.value();\n\n    // rotation\n    Eigen::Matrix<T, 3, 1> angle = value.rotation.eulerAnglesRPY();\n    angle(0, 0) = ::nvidia::isaac::RadToDeg(angle(0, 0));\n    angle(1, 0) = ::nvidia::isaac::RadToDeg(angle(1, 0));\n    angle(2, 0) = ::nvidia::isaac::RadToDeg(angle(2, 0));\n    const auto maybe_rotation =\n        ParameterWrapper<Eigen::Matrix<T, 3, 1>>::Wrap(context, angle);\n    if (!maybe_rotation) {\n      return ForwardError(maybe_rotation);\n    }\n    node[\"rotation\"] = maybe_rotation.value();\n    return node;\n  }\n};\n\n// Support for std::array of Pose3\ntemplate <typename T>\nstruct ParameterWrapper<std::array<::nvidia::isaac::Pose3<T>, 4>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context,\n                                   const std::array<::nvidia::isaac::Pose3<T>, 4>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n    for (uint32_t i = 0; i < 4; i++) {\n      const auto maybe = ParameterWrapper<::nvidia::isaac::Pose3<T>>::Wrap(context, value[i]);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\ntemplate <typename T>\nstruct ParameterWrapper<std::vector<::nvidia::isaac::Pose3<T>>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context,\n                                   const std::vector<::nvidia::isaac::Pose3<T>>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n    for (auto &v : value) {\n      const auto maybe = ParameterWrapper<::nvidia::isaac::Pose3<T>>::Wrap(context, v);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n// Parameter support for NSphere.\n// Example formats:\n//      center: [X, Y [, ...]]\n//      radius: 3.0\ntemplate <typename T, int N>\nstruct ParameterWrapper<::nvidia::isaac::geometry::NSphere<T, N>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context,\n                                   const ::nvidia::isaac::geometry::NSphere<T, N>& value) {\n    YAML::Node node(YAML::NodeType::Map);\n\n    // center\n    const auto maybe_center =\n      ParameterWrapper<::nvidia::isaac::Vector<T, N>>::Wrap(context, value.center);\n    if (!maybe_center) {\n      return ForwardError(maybe_center);\n    }\n    node[\"center\"] = maybe_center.value();\n\n    // radius\n    const auto maybe_radius =\n      ParameterWrapper<T>::Wrap(context, value.radius);\n    if (!maybe_radius) {\n      return ForwardError(maybe_radius);\n    }\n    node[\"radius\"] = maybe_radius.value();\n\n    return node;\n  }\n};\n\n// Support for std::array of NSphere\ntemplate <typename T, int N>\nstruct ParameterWrapper<std::vector<::nvidia::isaac::geometry::NSphere<T, N>>> {\n  static Expected<YAML::Node> Wrap(\n      gxf_context_t context,\n      const std::vector<::nvidia::isaac::geometry::NSphere<T, N>>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n    for (auto &v : value) {\n      const auto maybe =\n        ParameterWrapper<::nvidia::isaac::geometry::NSphere<T, N>>::Wrap(context, v);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n// Parameter support for NCuboid (where N is the dimension of the cuboid). The input is a std::array\n// of length N, for which each component contains an the minimum and maximum bound for Nth dimension\n// Example: [[-1.0, 1.0], [-2.0, 2.0]] represents a rectangle with the x dimension spanning -1.0 to\n// 1.0 and the y dimension spanning -2.0 to 2.0.\ntemplate <typename T, int N>\nstruct ParameterWrapper<::nvidia::isaac::geometry::NCuboid<T, N>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context,\n                                   const ::nvidia::isaac::geometry::NCuboid<T, N>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n\n    // min\n    const auto maybe_min =\n      ParameterWrapper<::nvidia::isaac::Vector<T, N>>::Wrap(context, value.min());\n    if (!maybe_min) {\n      return ForwardError(maybe_min);\n    }\n    node.push_back(maybe_min.value());\n\n    // max\n    const auto maybe_max =\n      ParameterWrapper<::nvidia::isaac::Vector<T, N>>::Wrap(context, value.max());\n    if (!maybe_max) {\n      return ForwardError(maybe_max);\n    }\n    node.push_back(maybe_max.value());\n    return node;\n  }\n};\n\n// Support for std::vector of NCuboid\ntemplate <typename T, int N>\nstruct ParameterWrapper<std::vector<::nvidia::isaac::geometry::NCuboid<T, N>>> {\n  static Expected<YAML::Node> Wrap(\n      gxf_context_t context,\n      const std::vector<::nvidia::isaac::geometry::NCuboid<T, N>>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n    for (auto &v : value) {\n      const auto maybe =\n        ParameterWrapper<::nvidia::isaac::geometry::NCuboid<T, N>>::Wrap(context, v);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n// Parameter support for Polygon2. Example format:\n// [[60, -104], [32, -96], [-6, -88], [-52, -81], [-95, -82]]\n// This would form a polygon with 5 points in 2D. Each point consist of [x, y] values.\ntemplate <typename T>\nstruct ParameterWrapper<::nvidia::isaac::geometry::Polygon2<T>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context,\n                                   const ::nvidia::isaac::geometry::Polygon2<T>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n    for (auto &point : value.points) {\n      const auto maybe = ParameterWrapper<::nvidia::isaac::Vector<T, 2>>::Wrap(context, point);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n// Support for std::vector of Polygon2\ntemplate <typename T>\nstruct ParameterWrapper<std::vector<::nvidia::isaac::geometry::Polygon2<T>>> {\n  static Expected<YAML::Node> Wrap(\n      gxf_context_t context,\n      const std::vector<::nvidia::isaac::geometry::Polygon2<T>>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n    for (auto &v : value) {\n      const auto maybe = ParameterWrapper<::nvidia::isaac::geometry::Polygon2<T>>::Wrap(context, v);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n// Parameter support for LineSegment. Example format:\n//   [ [-100.0, 0.0], [20.0, 5.0] ]\n// This would form a line segment between (-100.0, 0.0) and (20.0, 5.0).\n// Each point consist of (x, y) values.\ntemplate <typename T, int N>\nstruct ParameterWrapper<::nvidia::isaac::geometry::LineSegment<T, N>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context,\n                                   const ::nvidia::isaac::geometry::LineSegment<T, N>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n\n    // vector_a\n    const auto maybe_a = ParameterWrapper<::nvidia::isaac::Vector<T, N>>::Wrap(context, value.a());\n    if (!maybe_a) {\n      return ForwardError(maybe_a);\n    }\n    node.push_back(maybe_a.value());\n\n    // vector_b\n    const auto maybe_b = ParameterWrapper<::nvidia::isaac::Vector<T, N>>::Wrap(context, value.b());\n    if (!maybe_b) {\n      return ForwardError(maybe_b);\n    }\n    node.push_back(maybe_b.value());\n    return node;\n  }\n};\n\n// Support for std::vector of LineSegment\ntemplate <typename T, int N>\nstruct ParameterWrapper<std::vector<::nvidia::isaac::geometry::LineSegment<T, N>>> {\n  static Expected<YAML::Node> Wrap(\n      gxf_context_t context,\n      const std::vector<::nvidia::isaac::geometry::LineSegment<T, N>>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n    for (auto &v : value) {\n      const auto maybe =\n        ParameterWrapper<::nvidia::isaac::geometry::LineSegment<T, N>>::Wrap(context, v);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n// Parameter support for std::vector.\n// Example format: [1.0, 2.2, -3.7]\ntemplate<typename T>\nstruct ParameterWrapper<std::vector<T>> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context, const std::vector<T>& value) {\n    YAML::Node node(YAML::NodeType::Sequence);\n    for (auto &h : value) {\n      auto maybe = ParameterWrapper<T>::Wrap(context, h);\n      if (!maybe) {\n        return ForwardError(maybe);\n      }\n      node.push_back(maybe.value());\n    }\n    return node;\n  }\n};\n\n// Parameter support for Uuid.\ntemplate<>\nstruct ParameterWrapper<::nvidia::isaac::Uuid> {\n  static Expected<YAML::Node> Wrap(gxf_context_t context, const ::nvidia::isaac::Uuid& uuid) {\n    return ParameterWrapper<std::string>::Wrap(context, uuid.str());\n  }\n};\n\n}  // namespace gxf\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gxf_helpers/gxf/extensions/gxf_helpers/string_provider.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n\n#include \"gxf/core/component.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// A component that provides access to a string. This is useful for situations where one would like\n// to share the same string between multiple components, for example to implement a namespace/prefix\n// for a frame name of a pose tree frame.\nclass StringProvider : public gxf::Component {\n public:\n  gxf_result_t registerInterface(gxf::Registrar* registrar) override;\n\n  // Gets the value of the string.\n  std::string value() const;\n\n private:\n  gxf::Parameter<gxf::Handle<StringProvider>> prefix_;\n  gxf::Parameter<gxf::Handle<StringProvider>> suffix_;\n  gxf::Parameter<std::string> value_;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_gxf_helpers/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_gxf_helpers</name>\n  <version>4.4.0</version>\n  <description>GXF extension containing GXF helpers.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n  <depend>gxf_isaac_gems</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_hesai/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_hesai LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_hesai/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_hesai</name>\n  <version>4.4.0</version>\n  <description>GXF extension to interface with Hesai lidars.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_localization/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_localization LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_localization/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_localization</name>\n  <version>4.4.0</version>\n  <description>GXF extension containing environment management components, such as pose tree, map providers, and the composite schema server.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_message_compositor/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_message_compositor LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_message_compositor/gxf/message_compositor/message_relay.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef NVIDIA_ISAAC_ROS_EXTENSIONS_MESSAGE_RELAY_HPP_\n#define NVIDIA_ISAAC_ROS_EXTENSIONS_MESSAGE_RELAY_HPP_\n\n#include <condition_variable>\n#include <mutex>\n#include <vector>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n\nnamespace nvidia {\nnamespace isaac_ros {\n\n// Receives messages, stores them and provides thread-safe access to them.\nclass MessageRelay : public gxf::Codelet {\n public:\n  gxf_result_t registerInterface(gxf::Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t start() override;\n  gxf_result_t tick() override;\n  gxf_result_t stop() override;\n  gxf_result_t deinitialize() override;\n\n  // Waits until at least the given number of entities have arrived, stores them in the vault,\n  // and returns their UIDs.\n  std::vector<gxf_uid_t> storeBlocking(size_t count);\n\n  // Waits until at least the given number of entities have arrived, stores them in the vault,\n  // and returns their UIDs or times out within a targeted duration (nanoseconds).\n  std::vector<gxf_uid_t> storeBlockingFor(size_t count, int64_t duration_ns);\n\n  // Tries to grab at most specified number of entities and return without waiting.\n  std::vector<gxf_uid_t> store(size_t max_count);\n\n  // Removes the given entities from the vault\n  void free(const std::vector<gxf_uid_t>& entities);\n\n private:\n  // Stores entities assuming lock\n  std::vector<gxf_uid_t> storeImpl(size_t max_count);\n\n  gxf::Parameter<gxf::Handle<gxf::Receiver>> source_;\n  gxf::Parameter<uint64_t> max_waiting_count_;\n  gxf::Parameter<bool> drop_waiting_;\n  gxf::Parameter<int64_t> callback_address_;\n\n  std::vector<gxf::Entity> entities_waiting_;\n  std::vector<gxf::Entity> entities_in_vault_;\n\n  std::mutex mutex_;\n  std::condition_variable condition_variable_;\n  bool alive_;\n\n  std::function<void(void)> * callback_{nullptr};\n};\n\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // NVIDIA_ISAAC_ROS_EXTENSIONS_MESSAGE_RELAY_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_message_compositor/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_message_compositor</name>\n  <version>4.4.0</version>\n  <description>Extension containing a message compositor for fusing messages.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_messages LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    magic_enum::magic_enum\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/accelerometer_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma once\n\n#include \"gems/common/pose_frame_uid.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Contains the measurements of the accelerometer, i.e. linear acceleration.\nstruct Accelerometer {\n  // Linear accelerations of the accelerometer in its own frame.\n  double linear_acceleration_x;\n  double linear_acceleration_y;\n  double linear_acceleration_z;\n};\n\n// Stores the measurements of the accelerometer provides a handle to its data.\nstruct AccelerometerMessageParts {\n  // The entity containing the whole message.\n  gxf::Entity message;\n  // Measurements of the accelerometer.\n  gxf::Handle<Accelerometer> accelerometer;\n  // Timestamp of publishing and acquisition.\n  gxf::Handle<gxf::Timestamp> timestamp;\n  // Reference pose frame of the measurements (linear acceleration).\n  gxf::Handle<PoseFrameUid> pose_frame_uid;\n};\n\ngxf::Expected<AccelerometerMessageParts> CreateAccelerometerMessage(gxf_context_t context);\ngxf::Expected<AccelerometerMessageParts> GetAccelerometerMessage(gxf::Entity message);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/battery_state_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Contains battery state information, conforming to\n// https://github.com/VDA5050/VDA5050/blob/main/VDA5050_EN.md\n\nstruct BatteryState {\n  // VDA5050 variables\n  double charge;     // percent of charge remaining 0.0 - 100.0\n  double voltage;    // current battery voltage in volts\n  int8_t health;     // 0-100, battery health, 0 is bad, 100 is good\n  bool is_charging;  // Is the battery currently charging?\n  uint32_t reach;    // How long in meters the battery drive\n\n  // Extra non VDA5050 info\n  int32_t bat_mcurrent;            // battery current (ma)\n  int16_t bat_temp;                // battery temp (C)\n  bool charge_mode_status;         // Get the charge mode. 0:no charge;1:line charge;2:dock charge;\n  bool charge_mos_ctrl_status;     // status of switch for charging MOS, charging:1; no charge:0\n  uint16_t charge_connect_status;  // Get the charge connect. 0:no connect;1:charge connect\n};\n\nstruct BatteryStateMessageParts {\n  // Entity for the measurement\n  gxf::Entity message;\n  // Battery state data\n  gxf::Handle<BatteryState> battery_state;\n  // Timestamp of publishing and acquisition\n  gxf::Handle<gxf::Timestamp> timestamp;\n};\n\ngxf::Expected<BatteryStateMessageParts> CreateBatteryStateMessage(gxf_context_t context);\ngxf::Expected<BatteryStateMessageParts> GetBatteryStateMessage(gxf::Entity message);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/camera_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gxf/multimedia/camera.hpp\"\n#include \"gxf/multimedia/video.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Message structure for camera messages\nstruct CameraMessageParts {\n  // Message entity\n  gxf::Entity entity;\n  // Camera frame\n  gxf::Handle<gxf::VideoBuffer> frame;\n  // Camera intrinsics\n  gxf::Handle<gxf::CameraModel> intrinsics;\n  // Camera extrinsics\n  gxf::Handle<gxf::Pose3D> extrinsics;\n  // Frame sequence number\n  gxf::Handle<int64_t> sequence_number;\n  // Frame acquisition timestamp\n  gxf::Handle<gxf::Timestamp> timestamp;\n};\n\n// Initializes a CameraMessage with all components added to an entity\n// Frame memory is not allocated. This is useful when only the metadata of a CemeraMessage is needed\n// or frame buffer needs to be allocated later.\ngxf::Expected<CameraMessageParts> InitializeCameraMessage(gxf_context_t context);\n\n// Creates a camera message in the image format specified by the template parameter `Format`.\n// Frame memory is allocated based on `storage_type` and will have dimensions specified by\n// `width` and `height`. The memory surface layout of the frame is specified via `layout`.\n// Optionally, the `padded` boolean parameter can be passed to determine whether the camera message\n// should be padded or not.\ntemplate <gxf::VideoFormat Format>\ngxf::Expected<CameraMessageParts> CreateCameraMessage(gxf_context_t context,\n                                                      uint32_t width,\n                                                      uint32_t height,\n                                                      gxf::SurfaceLayout layout,\n                                                      gxf::MemoryStorageType storage_type,\n                                                      gxf::Handle<gxf::Allocator> allocator,\n                                                      bool padded = true);\n\n// Creates a camera message in the image format specified by the arguments `buffer_info` and `size`.\n// Frame memory is allocated based on `storage_type`.\ngxf::Expected<CameraMessageParts> CreateCameraMessage(gxf_context_t context,\n                                                      gxf::VideoBufferInfo buffer_info,\n                                                      uint64_t size,\n                                                      gxf::MemoryStorageType storage_type,\n                                                      gxf::Handle<gxf::Allocator> allocator);\n\n// Creates a camera message with image dimension, format and storage_type from a tensor.\n// Tensor payload is moved into the camera message created.\n// Tensor handle is invalid after the operation.\ntemplate <gxf::VideoFormat Format>\ngxf::Expected<CameraMessageParts> CreateCameraMessage(gxf_context_t context,\n                                                      gxf::Handle<gxf::Tensor> tensor,\n                                                      gxf::SurfaceLayout layout);\n\n// Parses a camera message from the given entity\ngxf::Expected<CameraMessageParts> GetCameraMessage(const gxf::Entity message);\n\n// Check if an entity contains a camera message\ngxf::Expected<bool> IsCameraMessage(const gxf::Entity entity);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/composite_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <utility>\n\n#include \"extensions/atlas/composite_schema_server.hpp\"\n#include \"gems/common/composite_schema_uid.hpp\"\n#include \"gems/common/pose_frame_uid.hpp\"\n#include \"gems/composite/composite_from_tensor.hpp\"\n#include \"gems/core/tensor/tensor.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Stores the message and provides convenience views to it\nstruct CompositeMessageParts {\n  // The message entity\n  gxf::Entity message;\n  // View to the data\n  ::nvidia::isaac::CpuTensorView2d view;\n  // The uid of the pose frame\n  gxf::Handle<PoseFrameUid> pose_frame_uid;\n  // The uid of the composite schema\n  gxf::Handle<CompositeSchemaUid> composite_schema_uid;\n  // Timestamp\n  gxf::Handle<gxf::Timestamp> timestamp;\n};\n\n// Makes a copy of a composite message. The copy is added to the entity with entity-id\n// message_eid.\ngxf::Expected<CompositeMessageParts> MakeCopyCompositeMessage(\n    gxf_context_t context, gxf_uid_t allocator_cid, gxf_uid_t message_eid,\n    const CompositeMessageParts& input_parts);\n\n// Creates a new entity and adds the components contained in a CompositeMessageParts to it and sets\n// up convenience views/handles to it. Allocates memory for the contained tensor to be of shape\n// num_states x state_size.\ngxf::Expected<CompositeMessageParts> CreateCompositeMessage(\n    gxf_context_t context, gxf::Handle<gxf::Allocator> allocator, int num_states, int state_size);\n\n// Adds the components contained in a CompositeMessageParts to an existing entity and sets up\n// convenience views/handles to it. Allocates memory for the contained tensor to be of shape\n// num_states x state_size.\ngxf::Expected<CompositeMessageParts> AddCompositeMessage(\n    gxf::Entity entity, gxf::Handle<gxf::Allocator> allocator, int num_states, int state_size);\n\n// Extracts a CompositeMessageParts from an entity and sets up convenience views/handles.\ngxf::Expected<CompositeMessageParts> GetCompositeMessage(gxf::Entity message);\n\n// Deprecated: Use AddCompositeMessage() instead.\n// Same as AddCompositeMessage() but using different signature. To be consistent with other message\n// parts structs use AddCompositeMessage() if possible.\ngxf::Expected<CompositeMessageParts> PrepareCompositeMessage(\n        gxf_context_t context, gxf_uid_t allocator_cid, gxf_uid_t message_eid, int num_states,\n        int state_size);\n\n// Deprecated: Use GetCompositeMessage() instead.\n// Same as GetCompositeMessage(). To be consistent with other message parts structs use\n// GetCompositeMessage() if possible.\ngxf::Expected<CompositeMessageParts> ParseCompositeMessage(gxf::Entity message);\n\n// Extract a CompositeArray or CompositeEigen from a composite message using the schema server.\n// Parameter `slice` is used to select the slice of the tensor which is used to populate the\n// composite.\ntemplate <typename Composite>\ngxf::Expected<Composite> ExtractComposite(\n    const CompositeMessageParts& message_parts, const CompositeSchemaServer& schema_server,\n    const composite::Schema& desired_schema, int slice = 0) {\n  // Retrieve the schema of the composite message from the schema server.\n  const auto maybe_schema = schema_server.get(message_parts.composite_schema_uid->uid);\n  if (!maybe_schema) {\n    GXF_LOG_ERROR(\"Failed to get schema from schema server.\");\n    return gxf::Unexpected(maybe_schema.error());\n  }\n  const composite::Schema& current_schema = maybe_schema.value();\n\n  return CompositeFromTensor<Composite>(message_parts.view.const_slice(slice),\n      current_schema, desired_schema);\n}\n\n// Convenience wrapper around function above. Allows to directly pass a gxf::Entity instead of a\n// CompositeMessageParts.\ntemplate <typename Composite>\ngxf::Expected<Composite> ExtractComposite(\n    gxf::Entity entity, const CompositeSchemaServer& schema_server,\n    const composite::Schema& desired_schema, int slice = 0) {\n  // Parse the entity to a composite message parts.\n  const auto maybe_message_parts = GetCompositeMessage(entity);\n  if (!maybe_message_parts) {\n    GXF_LOG_ERROR(\"Failed to parse entity as CompositeMessageParts.\");\n    return gxf::Unexpected(maybe_message_parts.error());\n  }\n  const CompositeMessageParts& message_parts = maybe_message_parts.value();\n  return ExtractComposite<Composite>(message_parts, schema_server, desired_schema, slice);\n}\n\n// Convenience wrapper around function above. Allows to directly pass the handle to a\n// CompositeSchemaServer and checks that that the handle is not null.\ntemplate <typename Composite>\ngxf::Expected<Composite> ExtractComposite(\n    gxf::Entity entity, gxf::Handle<CompositeSchemaServer> schema_server_handle,\n    const composite::Schema& desired_schema, int slice = 0) {\n  if (!schema_server_handle) {\n    GXF_LOG_ERROR(\"No schema server provided.\");\n    return gxf::Unexpected{GXF_FAILURE};\n  }\n  const CompositeSchemaServer& schema_server = *schema_server_handle;\n\n  return ExtractComposite<Composite>(\n      std::move(entity), schema_server, desired_schema, slice);\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/correlated_timestamp_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Correlated timestamp struct\n// Nova has three seperate clocks that we must keep track of, the PTP hardware clock (PHC), the\n// Timer's System Counter (TSC), and the system clock mainatined by the kernel (sys).\n// This struct ontains two pairs of correlated timestamps, phc_val_ anc tsc_val are taken\n// simultanously, and phc2_val and sys_val_ are taken simultanously.\nstruct CorrelatedTimestamps {\n  // phc and tsc are captured at the same time\n  int64_t phc_val;  // PTP Hardware Clock (PHC)\n  int64_t tsc_val;  // Timer's System Counter (TSC)\n\n  // phc2 and tsc are captures at the same time.\n  int64_t phc2_val;  // PTP Hardware Clock (PHC), same clock as phc_val_ above, but taken at a\n                     // different time\n  int64_t sys_val;   // System Clock (Sys)\n\n\n  // Deprecated, previously used for sanity checking output from NVPPS, but we no longer use NVPPS\n  int64_t phc_latency;\n};\n\n// Stores the Correlated Timestamps message and provides views to it.\nstruct CorrelatedTimestampsMessageParts {\n  // The message entity\n  gxf::Entity entity;\n  // View to the CorrelatedTimestamps struct\n  gxf::Handle<CorrelatedTimestamps> correlated_timestamps;\n  // GXF publication timestamp\n  gxf::Handle<gxf::Timestamp> timestamp;\n};\n\n// Create a CorrelatedTimestampStruct and the owning entity.\ngxf::Expected<CorrelatedTimestampsMessageParts> CreateCorrelatedTimestampMessage(\n    gxf_context_t context);\n\n// Create a Correlated timestamp struct from an existing entity\ngxf::Expected<CorrelatedTimestampsMessageParts> GetCorrelatedTimestampMessage(gxf::Entity message);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/encoder_tick_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\nstruct EncoderTicksMessageParts {\n  // Entity for the measurement\n  gxf::Entity message;\n\n  // Encoder data\n  gxf::Handle<int32_t> left_ticks;   // ticks from the left wheel\n  gxf::Handle<int32_t> right_ticks;  // ticks from the right wheel\n  // Timestamp from the encoder, for example on the RMPLite this is a stamp on the CAN message\n  // This can in general be in a different timebase than the system time\n  gxf::Handle<int64_t> encoder_timestamp;\n\n  // Timestamp of publishing and acquisition in system time\n  gxf::Handle<gxf::Timestamp> timestamp;\n};\n\ngxf::Expected<EncoderTicksMessageParts> CreateEncoderTickMessage(gxf_context_t context);\ngxf::Expected<EncoderTicksMessageParts> GetEncoderTickMessage(gxf::Entity message);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/flatscan_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/common/pose_frame_uid.hpp\"\n#include \"gems/core/tensor/tensor.hpp\"\n#include \"gems/flatscan/flatscan_info.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Stores the flatscan message and provides convenience views to it.\nstruct FlatscanMessageParts {\n  // The message entity\n  gxf::Entity entity;\n  // View to the beams data\n  // This is a 2d tensor, where each row is a beam which defined by the schema in\n  // sdk/gems/flatscan/flatscan_types.hpp\n  ::nvidia::isaac::CpuTensorView2d beams;\n  // View to the FlatscanInfo instance holding meta information about this message\n  gxf::Handle<FlatscanInfo> info;\n  // The uid of this scan's origin pose frame\n  gxf::Handle<PoseFrameUid> pose_frame_uid;\n  // Timestamp\n  gxf::Handle<gxf::Timestamp> timestamp;\n};\n\n// Allocates a message entity representing a flatscan message containing `beam_count` beams. The\n// returned message is a struct of type `FlatscanMessageParts` consisting of an entity holding all\n// data, two TensorView objects allowing easy access to individual message parts, and a reference to\n// the FlatscanInfo object containing the message meta data.\n// The entity has the following fields:\n//  - \"beams\": Of type `gxf::Tensor` the definition please refer:\n//  sdk/gems/flatscan/flatscan_types.hpp\n//  - \"info\": Of type `nvidia::isaac::FlatscanInfo`, contains the meta information\n//     describing the message's\n//            content.\n//  - \"pose_frame_uid\": Of type `nvidia::isaac::PoseFrameUid`, contains the uid of\n//     the pose frame the\n//                      flatscan originates from.\n// If `activate` is `true`, the message will be activated after creation.\n// The TensorView objects returned represent the \"angles\" and \"ranges\" tensors, respectively.\ngxf::Expected<FlatscanMessageParts> CreateFlatscanMessage(gxf_context_t context,\n                                                          gxf::Handle<gxf::Allocator> allocator,\n                                                          int beam_count);\n\n// Returns a struct of type `FlatscanMessageParts` consisting of an entity holding all flatscan\n// data: two TensorView objects allowing easy access to individual message parts, and a reference to\n// the FlatscanInfo object containing the message meta data.\ngxf::Expected<FlatscanMessageParts> GetFlatscanMessage(gxf::Entity message);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/gyroscope_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/common/pose_frame_uid.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Contains the measurements of the gyroscope, i.e. angular velocity.\nstruct Gyroscope {\n  // Angular velocities of the gyroscope in its own frame.\n  double angular_velocity_x;\n  double angular_velocity_y;\n  double angular_velocity_z;\n};\n\n// Stores the measurements of the gyroscope provides a handle to its data.\nstruct GyroscopeMessageParts {\n  // The entity containing the whole message.\n  gxf::Entity message;\n  // Measurements of the gyroscope.\n  gxf::Handle<Gyroscope> gyroscope;\n  // Timestamp of publishing and acquisition.\n  gxf::Handle<gxf::Timestamp> timestamp;\n  // Reference pose frame of the measurements (angular velocity).\n  gxf::Handle<PoseFrameUid> pose_frame_uid;\n};\n\ngxf::Expected<GyroscopeMessageParts> CreateGyroscopeMessage(gxf_context_t context);\ngxf::Expected<GyroscopeMessageParts> GetGyroscopeMessage(gxf::Entity message);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/helpers.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n#include <utility>\n\n#include \"gems/core/tensor/tensor.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/tensor.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Adds a tensor with a given `name` to the given `entity`. The tensor is created with the specified\n// characteristics (`storage_type` and `dimensions`), using the passed `allocator` instance. This\n// method returns an object of type `::nvidia::isaac::CpuTensorView` (of value type `K`\n// and rank `N`) that allows access to the underlying `gxf::Tensor`'s data.\ntemplate <typename K, int N>\ngxf::Expected<::nvidia::isaac::CpuTensorView<K, N>> AddTensorToEntity(\n    gxf::Entity& entity, gxf::Handle<gxf::Allocator> allocator, const char* name,\n    gxf::MemoryStorageType storage_type, const ::nvidia::isaac::Vector<int, N>& dimensions) {\n  if (!allocator) {\n    return gxf::Unexpected{GXF_ARGUMENT_NULL};\n  }\n  // Add Tensor to entity\n  auto maybe_tensor = entity.add<gxf::Tensor>(name);\n  if (!maybe_tensor) {\n    return gxf::ForwardError(maybe_tensor);\n  }\n  auto& tensor = maybe_tensor.value();\n  // Prepare shape object\n  std::array<int32_t, gxf::Shape::kMaxRank> shape_entries;\n  for (int dimension_index = 0; dimension_index < N; ++dimension_index) {\n    shape_entries[dimension_index] = dimensions[dimension_index];\n  }\n  const gxf::Shape shape(shape_entries, N);\n  // Reshape tensor\n  auto reshape_result = tensor->reshape<K>(shape, storage_type, allocator);\n  if (!reshape_result) {\n    return gxf::ForwardError(reshape_result);\n  }\n  auto maybe_ptr = tensor->data<K>();\n  if (!maybe_ptr) {\n    return gxf::ForwardError(maybe_ptr);\n  }\n  return ::nvidia::isaac::CreateCpuTensorViewFromData<K, N>(\n      maybe_ptr.value(), shape.size(), dimensions);\n}\n\n// Gets a view to the tensor with a given `name` in the given `entity`. This method returns an\n// object of type `::nvidia::isaac::CpuTensorView` (of value type `K` and rank `N`) that\n// allows access to the underlying `gxf::Tensor`'s data.\ntemplate <typename K, int N>\ngxf::Expected<::nvidia::isaac::CpuTensorView<K, N>> GetTensorViewFromEntity(gxf::Entity& entity,\n                                                                 const char* name) {\n  const auto& maybe_tensor = entity.get<gxf::Tensor>(name);\n  if (!maybe_tensor) {\n    return gxf::ForwardError(maybe_tensor);\n  }\n  auto maybe_data = maybe_tensor.value()->data<K>();\n  if (!maybe_data) {\n    return gxf::ForwardError(maybe_data);\n  }\n  K* ptr_data = maybe_data.value();\n  // Memory storage should be CPU or system\n  if (maybe_tensor.value()->storage_type() == gxf::MemoryStorageType::kDevice) {\n    return gxf::Unexpected{GXF_MEMORY_INVALID_STORAGE_MODE};\n  }\n  const auto& shape = maybe_tensor.value()->shape();\n  if (shape.rank() != N) {\n    return gxf::Unexpected{GXF_FAILURE};\n  }\n  ::nvidia::isaac::Vector<int, N> dimensions;\n  for (int i = 0; i < N; i++) {\n    dimensions[i] = shape.dimension(i);\n  }\n  return ::nvidia::isaac::CreateCpuTensorViewFromData<K, N>(ptr_data, shape.size(), dimensions);\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/imu_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gems/common/pose_frame_uid.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Contains the measurements of the IMU, i.e. linear acceleration and angular velocity.\nstruct Imu {\n  // Linear accelerations of the IMU in its own frame.\n  double linear_acceleration_x;\n  double linear_acceleration_y;\n  double linear_acceleration_z;\n\n  // Angular velocities of the IMU in its own frame.\n  double angular_velocity_x;\n  double angular_velocity_y;\n  double angular_velocity_z;\n};\n\n// Stores the measurements of the IMU provides a handle to its data.\nstruct ImuMessageParts {\n  // The entity containing the whole message.\n  gxf::Entity message;\n  // Measurements of the IMU.\n  gxf::Handle<Imu> imu;\n  // Timestamp of publishing and acquisition.\n  gxf::Handle<gxf::Timestamp> timestamp;\n  // Reference pose frame of the measurements (linear acceleration and angular velocity).\n  gxf::Handle<PoseFrameUid> pose_frame_uid;\n};\n\ngxf::Expected<ImuMessageParts> CreateImuMessage(gxf_context_t context);\ngxf::Expected<ImuMessageParts> GetImuMessage(gxf::Entity message);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/json_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <string>\n\n#include \"gems/serialization/json.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Stores the Sop message and provides convenience views to it.\nstruct JsonMessageParts {\n  // The message entity\n  gxf::Entity entity;\n  // Handle to the Sop instance for sight visualization\n  gxf::Handle<::nvidia::isaac::Json> json;\n  // Timestamp\n  gxf::Handle<gxf::Timestamp> timestamp;\n};\n\n// Creates message entity and attaches a Sop instance to it to be populated later for sight\n// visualization.\ngxf::Expected<JsonMessageParts> CreateJsonMessage(const char* tag_name, gxf_context_t context);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/gxf/extensions/messages/pose3d_cov_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_ISAAC_EXTENSIONS_MESSAGES_POSE3D_COV_MESSAGE_HPP_\n#define NVIDIA_ISAAC_EXTENSIONS_MESSAGES_POSE3D_COV_MESSAGE_HPP_\n\n#include \"gems/common/pose_frame_uid.hpp\"\n#include \"gems/core/math/pose3.hpp\"\n#include \"gems/core/tensor/tensor.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Stores the 3D Pose, its base frame UID, its covariance matrix and timestamp\nstruct Pose3dCovMessageParts {\n  // The message entity\n  gxf::Entity entity;\n  // The pose, ie, the transform that moves a point from child to parent coordinate\n  // p_parent = transform_parent_to_child * p_child\n  gxf::Handle<::nvidia::isaac::Pose3d> pose;\n  // The pose covariance represented as 6-by-6 tensor.\n  // Row-major representation of the 6x6 covariance matrix\n  // The orientation parameters use a fixed-axis representation.\n  // In order, the parameters are:\n  // (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\n  // Precision is double\n  // The below object is a view to the covariance tensor\n  ::nvidia::isaac::CpuTensorView2d covariance;\n  // The parent frame UID.\n  // This the reference frame UID for the pose in this message.\n  gxf::Handle<PoseFrameUid> parent_pose_frame_uid;\n  // The child/this frame UID.\n  gxf::Handle<PoseFrameUid> child_pose_frame_uid;\n  // Timestamp for this message.\n  gxf::Handle<gxf::Timestamp> timestamp;\n};\n\n// Allocates a message entity representing Pose3dCovMessageParts.\ngxf::Expected<Pose3dCovMessageParts> CreatePose3dCovMessage(\n    gxf_context_t context, gxf::Handle<gxf::Allocator> allocator);\n\n// Parses a gxf entity into a Pose3dCovMessageParts.\ngxf::Expected<Pose3dCovMessageParts> GetPose3dCovMessage(gxf::Entity message);\n\n}  // namespace isaac\n}  // namespace nvidia\n\n#endif  // NVIDIA_ISAAC_EXTENSIONS_MESSAGES_POSE3D_COV_MESSAGE_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_messages</name>\n  <version>4.4.0</version>\n  <description>Extension containing Isaac SDK related messages and data types.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n  <depend>magic_enum</depend>\n\n  <build_depend>gxf_isaac_gems</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages_throttler/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_messages_throttler LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_LINK_LIBRARIES \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n    install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n            DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n    set_target_properties(${PROJECT_NAME} PROPERTIES\n            INTERFACE_INCLUDE_DIRECTORIES \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_messages_throttler/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_messages_throttler</name>\n  <version>4.4.0</version>\n  <description>NvIsaacMessagesThrottler</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Benjamin Butin</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_optimizer/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_optimizer LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_optimizer/gxf/extensions/gxf_optimizer/common/type.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <map>\n#include <memory>\n#include <set>\n#include <string>\n#include <vector>\n\n#include \"gxf/core/expected.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\nnamespace optimizer {\n\ntemplate<typename T>\nusing UniquePointerMap = std::map<gxf_uid_t, std::unique_ptr<T>>;\n\nusing UIDList = std::vector<gxf_uid_t>;\nusing UIDSet = std::set<gxf_uid_t>;\nusing UIDStringMap = std::map<gxf_uid_t, std::string>;\nusing StringMap = std::map<std::string, std::string>;\nusing StringList = std::vector<std::string>;\nusing StringSet = std::set<std::string>;\n\nconstexpr char kAnyDataType[] = \"any\";\nconstexpr char kToBeDeterminedDataType[] = \"tbd\";\nconstexpr char kUndefinedDataType[] = \"undefined\";\n\n// Factors for the cost of graph (COG)\nenum class gxf_cog_factor_t : std::int32_t {\n  THROUGHPUT = 0,\n  LATENCY,\n  POWER,\n  ACCURACY,\n};\n\n// Get the gxf_cog_factor_t enum value from a COG factor's string.\nExpected<gxf_cog_factor_t> ParseCOGFactorString(std::string factor_str);\n\n// Get the string of a gxf_cog_factor_t factor.\nExpected<const char*> GetCOGFactorString(const gxf_cog_factor_t factor);\n\n}  // namespace optimizer\n}  // namespace gxf\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_optimizer/gxf/extensions/gxf_optimizer/core/optimizer.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"extensions/gxf_optimizer/common/type.hpp\"\n#include \"extensions/gxf_optimizer/exporter/graph_types.hpp\"\n#include \"gxf/core/expected.hpp\"\n\nnamespace nvidia {\nnamespace gxf {\nnamespace optimizer {\n\nclass ConfigurationGraph;\n\nclass Optimizer {\n  class OptimizerImpl;\n  std::unique_ptr<OptimizerImpl> pimpl;\n\n public:\n  // Constructor & destructor\n  Optimizer();\n  ~Optimizer();\n\n  // Load a graph from the given YAML file.\n  Expected<void> loadGraph(const std::string& graph_yaml_file_path);\n\n  // Load optimization rules from the given YAML file.\n  Expected<void> loadRules(const std::string& rules_yaml_file_path);\n\n  // Load extension specs from the YAML file.\n  Expected<void> loadExtensionSpecs(const std::string& specs_yaml_file_path);\n\n  // Load preset extension specs.\n  Expected<void> loadPresetExtensionSpecs(const std::string& preset_key);\n\n  // Export the originally loaded graph as YAML string.\n  // It returns a YAML string that corresponds to the application graph,\n  // otherwise one of the GXF error codes.\n  Expected<std::string> exportLoadedGraph() const;\n\n  // Export the graph candidate at the given index as a YAML string.\n  // It returns a YAML string that corresponds to the graph of the specified candidate,\n  // otherwise one of the GXF error codes.\n  Expected<std::string> exportGraph(const size_t index) const;\n\n  // Get the graph based on the given ingress-egres group data type assignments.\n  // If a prefix string is given, then unnamed entities will be given random names and all\n  // entity names will be prepended with the given prefix.\n  // It returns a string that corresponds to the graph's YAML of the specified candidate,\n  // otherwise one of the GXF error codes.\n  Expected<std::string> exportGraph(\n      const GraphIOGroupDataTypeConfigurationsList& configs,\n      const std::string& prefix = std::string());\n\n  // Get the graph based on the given ingress-egres group data type assignments.\n  // If a prefix string is given, then unnamed entities will be given random names and all\n  // entity names will be prepended with the given prefix.\n  // The resulting graph(s) (including all subgraphs) are saved to the specified directory.\n  // The top level graph YAML file is named <export_base_filename>.yaml. Its first-level\n  // subgraphs are named <export_base_filename>_subgraph<index>.yaml and second-level\n  // subgraphs being <export_base_filename>_subgraph<index>_subgraph<2nd_level_index>.yaml\n  // and so on and so forth if any.\n  Expected<void> exportGraphToFiles(\n      const GraphIOGroupDataTypeConfigurationsList& configs,\n      const std::string& export_directory,\n      const std::string& export_base_filename,\n      const std::string& prefix = std::string());\n\n  // Export ingress-egress groups and their supported data types\n  Expected<GraphIOGroupSupportedDataTypesInfoList> exportGraphIOGroupSupportedDataTypesInfoList();\n\n  // Run graph optimization.\n  // It returns SUCCESS if the operation was finished without an error,\n  // otherwise one of the GXF error codes.\n  Expected<void> optimize(const gxf_cog_factor_t top_factor);\n\n  // Get a copy of the loaded configuration graph.\n  // It returns a copy of the loaded configruation graph,\n  // otherwise one of the GXF error codes.\n  Expected<ConfigurationGraph> getLoadedGraph() const;\n\n  // Get a graph candidate at the given index.\n  // It returns a copy of the graph candidate at the given index or\n  // GXF_ARGUMENT_OUT_OF_RANGE if the index is out of range.\n  Expected<ConfigurationGraph> getGraphCandidate(const size_t index) const;\n\n  // Get the number of available graph candidates.\n  size_t getGraphCandidateCount() const;\n};\n\n}  // namespace optimizer\n}  // namespace gxf\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_optimizer/gxf/extensions/gxf_optimizer/exporter/graph_types.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <algorithm>\n#include <map>\n#include <string>\n#include <vector>\n\nnamespace nvidia {\nnamespace gxf {\nnamespace optimizer {\n\nusing ComponentKey = std::string;\n\nstruct ComponentInfo {\n  std::string component_type_name;\n  std::string component_name;\n  std::string entity_name;\n\n  bool operator==(const ComponentInfo& other) const {\n    return component_type_name == other.component_type_name &&\n           component_name == other.component_name &&\n           entity_name == other.entity_name;\n  }\n};\n\nstruct GraphIOGroupSupportedDataTypesInfo {\n  std::vector<ComponentInfo> ingress_infos;\n  std::vector<ComponentInfo> egress_infos;\n  std::vector<std::map<ComponentKey, std::string>> supported_data_types;\n};\nusing GraphIOGroupSupportedDataTypesInfoList = std::vector<GraphIOGroupSupportedDataTypesInfo>;\n\nstruct GraphIOGroupDataTypeConfigurations {\n  std::vector<ComponentInfo> ingress_infos;\n  std::vector<ComponentInfo> egress_infos;\n  std::map<ComponentKey, std::string> data_type_configurations;\n};\nusing GraphIOGroupDataTypeConfigurationsList = std::vector<GraphIOGroupDataTypeConfigurations>;\n\ninline ComponentKey GenerateComponentKey(const ComponentInfo& comp_info) {\n  return comp_info.entity_name + \"/\" + comp_info.component_name;\n}\n\ninline std::vector<std::string> GetSupportedDataTypes(\n    const GraphIOGroupSupportedDataTypesInfo& supported_data_types_info,\n    const ComponentInfo& comp_info) {\n  std::vector<std::string> supported_data_types;\n  for (auto& type_map : supported_data_types_info.supported_data_types) {\n    // Add the data type but avoid the duplicates\n    const std::string& data_type = type_map.at(GenerateComponentKey(comp_info));\n    if (std::find(supported_data_types.begin(), supported_data_types.end(), data_type) ==\n        supported_data_types.end()) {\n      supported_data_types.push_back(data_type);\n    }\n  }\n  return supported_data_types;\n}\n\ninline std::string ToGraphIOGroupSupportedDataTypesInfoListStr(\n    const GraphIOGroupSupportedDataTypesInfoList& gxf_io_group_info_list_) {\n  std::string out_str{};\n  for (size_t group_index = 0; group_index < gxf_io_group_info_list_.size(); group_index++) {\n    out_str += \"#\" + std::to_string(group_index + 1) + \" I/O group:\\r\\n\";;\n    const auto& gxf_io_group = gxf_io_group_info_list_[group_index];\n    for (size_t combo_index = 0;\n         combo_index < gxf_io_group.supported_data_types.size();\n         combo_index++) {\n      const auto& supported_data_type_map = gxf_io_group.supported_data_types[combo_index];\n      out_str += \"\\t#\" + std::to_string(combo_index + 1) +\" format combination:\\r\\n\";\n      // ingress ports\n      for (const auto& ingress_comp_info : gxf_io_group.ingress_infos) {\n        const std::string component_key =\n          gxf::optimizer::GenerateComponentKey(ingress_comp_info);\n        const std::string supported_format = supported_data_type_map.at(component_key);\n        out_str += \"\\t\\t[in]\\t\" + component_key + \": \" + supported_format + \"\\r\\n\";\n      }\n      // egress ports\n      for (const auto& egress_comp_info : gxf_io_group.egress_infos) {\n        const std::string component_key =\n          gxf::optimizer::GenerateComponentKey(egress_comp_info);\n        const std::string supported_format = supported_data_type_map.at(component_key);\n        out_str += \"\\t\\t[out]\\t\" + component_key + \": \" + supported_format + \"\\r\\n\";\n      }\n    }\n  }\n  return out_str;\n}\n\n}  // namespace optimizer\n}  // namespace gxf\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_optimizer/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_optimizer</name>\n  <version>4.4.0</version>\n  <description>GXF extension containing environment management components, such as pose tree, map providers, and the composite schema server.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_point_cloud/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_point_cloud LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_point_cloud/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_point_cloud</name>\n  <version>4.4.0</version>\n  <description>GXF extension containing environment management components, such as pose tree, map providers, and the composite schema server.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_range_scan_processing/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_range_scan_processing LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_range_scan_processing/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_range_scan_processing</name>\n  <version>4.4.0</version>\n  <description>GXF extension containing components for processing range scan data.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_ros_cuda/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_ros_cuda LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n    gxf_isaac_cuda::gxf_isaac_cuda\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_ros_cuda/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_ros_cuda</name>\n  <version>4.4.0</version>\n  <description>GXF extension for Cuda related components in Isaac.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n  <depend>gxf_isaac_cuda</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_ros_messages/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_ros_messages LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_ros_messages/gxf/messages/flat_scan_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2020-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_ISAAC_EXTENSIONS_MESSAGES_FLATSCAN_TENSOR_MESSAGE_HPP_\n#define NVIDIA_ISAAC_EXTENSIONS_MESSAGES_FLATSCAN_TENSOR_MESSAGE_HPP_\n\n#include \"gems/core/tensor/tensor.hpp\"\n#include \"gems/common/pose_frame_uid.hpp\"\n#include \"gems/flatscan/flatscan_info.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac_ros {\nnamespace messages {\n\n// Stores the flatscan message and provides convenience views to it.\nstruct FlatscanMessageParts {\n  // The message entity\n  gxf::Entity entity;\n  // View to the beams data\n  // This is a 2d tensor, where each row is a beam which defined by the schema in\n  // sdk/gems/flatscan/flatscan_types.hpp\n  gxf::Handle<gxf::Tensor> beams;\n  // View to the nvidia::isaac::nvidia::isaac::FlatscanInfo instance holding meta information about this message\n  gxf::Handle<nvidia::isaac::FlatscanInfo> info;\n  // The uid of this scan's origin pose frame\n  gxf::Handle<nvidia::isaac::PoseFrameUid> pose_frame_uid;\n  // Timestamp\n  gxf::Handle<gxf::Timestamp> timestamp;\n};\n\n// Allocates a message entity representing a flatscan message containing `beam_count` beams. The\n// returned message is a struct of type `FlatscanMessageParts` consisting of an entity holding all\n// data, two TensorView objects allowing easy access to individual message parts, and a reference to\n// the FlatscanInfo object containing the message meta data.\n// The entity has the following fields:\n//  - \"beams\": Of type `gxf::Tensor` the definition please refer:\n//  sdk/gems/flatscan/flatscan_types.hpp\n//  - \"info\": Of type `isaac::FlatscanInfo`, contains the meta information describing the message's\n//            content.\n//  - \"pose_frame_uid\": Of type `isaac::PoseFrameUid`, contains the uid of the pose frame the\n//                      flatscan originates from.\n// If `activate` is `true`, the message will be activated after creation.\n// The TensorView objects returned represent the \"angles\" and \"ranges\" tensors, respectively.\ngxf::Expected<FlatscanMessageParts> CreateFlatscanMessage(gxf_context_t context,\n                                                          gxf::Handle<gxf::Allocator> allocator,\n                                                          int beam_count, bool activate = true);\n\n// Returns a struct of type `FlatscanMessageParts` consisting of an entity holding all flatscan\n// data: two TensorView objects allowing easy access to individual message parts, and a reference to\n// the FlatscanInfo object containing the message meta data.\ngxf::Expected<FlatscanMessageParts> GetFlatscanMessage(gxf::Entity message);\n\n}  // namespace messages\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // NVIDIA_ISAAC_EXTENSIONS_MESSAGES_FLATSCAN_TENSOR_MESSAGE_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_ros_messages/gxf/messages/point_cloud_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef NVIDIA_ISAAC_ROS_EXTENSIONS_MESSAGES_POINT_CLOUD_HPP_\n#define NVIDIA_ISAAC_ROS_EXTENSIONS_MESSAGES_POINT_CLOUD_HPP_\n\n#include \"gems/common/pose_frame_uid.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia {\nnamespace isaac_ros {\nnamespace messages {\n\n// Data structure holding meta information about flatscan messages. Every flatscan message must\n// contain an instance of this message.\nstruct PointCloudInfo {\n  // If the beam free range end is greater than or equal to this value the beam did not hit an\n  // obstacle at the end of the free range.\n  double max_range;\n  // Beams with a range lesser than or equal to this range in meters are considered to have returned\n  // an invalid measurement.\n  double min_range;\n  // Flag showing whether this pointcloud message contains color data\n  bool use_color;\n  // Is this data bigendian?\n  bool is_bigendian;\n};\n\n// Stores the point cloud message and provides views to it.\nstruct PointCloudMessageParts {\n  // The message entity\n  gxf::Entity message;\n  // Tensor containing the point positions(and color if use_color is true)\n  // Each color channel data is stored as a float in the colors array.\n  // Each point data bytes are ordered as:\n  // x_pos_float, y_pos_float, z_pos_float, color_float(only if if use_color is true)\n  // If use_color is true and is_bigendian is true\n  // color_float -> uint8 Data format: B, G, R, dont_care_byte\n  // Else if use_color is true and is_bigendian is false\n  // color_float -> uint8 Data format: dont_care_byte, R, G, B\n  gxf::Handle<gxf::Tensor> points;\n  // Pointcloud info data\n  gxf::Handle<PointCloudInfo> info;\n  // The uid of this scan's origin pose frame\n  gxf::Handle<nvidia::isaac::PoseFrameUid> pose_frame_uid;\n  // Timestamp\n  gxf::Handle<gxf::Timestamp> timestamp;\n};\n\n// Allocates a message entity representing a point cloud message, i.e. the cartesian coordinates of\n// points and color is enabled. The returned message is a struct of\n// type 'PointCloudMessageParts', consisting of an entity holding all data and\n// allowing easy access to individual message parts.\ngxf::Expected<PointCloudMessageParts> CreatePointCloudMessage(\n    gxf_context_t context, gxf::Handle<gxf::Allocator> allocator, int point_count, bool use_color);\n\n// Returns a struct of type `PointCloudMessageParts`\n// consisting of an entity holding all point cloud data.\ngxf::Expected<PointCloudMessageParts> GetPointCloudMessage(gxf::Entity message);\n\n}  // namespace messages\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // NVIDIA_ISAAC_ROS_EXTENSIONS_MESSAGES_POINT_CLOUD_HPP_\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_ros_messages/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_ros_messages</name>\n  <version>4.4.0</version>\n  <description>Extension containing Isaac ROS related messages and data types.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_segway/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_segway LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelse()\n  message(WARNING\n          \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}. \"\n          \"${PROJECT_NAME} is only supported on aarch64 platforms.\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\nif(EXISTS \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\")\n  install(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\n  set_property(TARGET ${PROJECT_NAME} PROPERTY\n    INTERFACE_LINK_LIBRARIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n  )\nendif()\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_segway/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_segway</name>\n  <version>4.4.0</version>\n  <description>GXF extension to interface with Segway chassis.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_sight/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_sight LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_sight/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_sight</name>\n  <version>4.4.0</version>\n  <description>Extension containing components for visualization with sight.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_timestamp_correlator/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(gxf_isaac_timestamp_correlator LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n  set(GXF_EXT_LIB_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/lib/gxf_x86_64_cuda_13_0\")\nelse()\n  message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install shared library\ninstall(FILES \"${GXF_EXT_LIB_PATH}/lib${PROJECT_NAME}.so\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib\")\nset_property(TARGET ${PROJECT_NAME} PROPERTY\n  INTERFACE_LINK_LIBRARIES\n    \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/lib/lib${PROJECT_NAME}.so\"\n)\n\n# Install headers\nif(EXISTS \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\n  install(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/gxf/\"\n          DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\n  set_target_properties(${PROJECT_NAME} PROPERTIES\n    INTERFACE_INCLUDE_DIRECTORIES\n      \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\nendif()\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_timestamp_correlator/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_timestamp_correlator</name>\n  <version>4.4.0</version>\n  <description>Codelets to translate between TS, PHC, and system clocks.</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_utils/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.5)\nproject(gxf_isaac_utils LANGUAGES C CXX CUDA)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-gpu-targets)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Dependencies\nfind_package(CUDAToolkit REQUIRED)\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(yaml-cpp)\n\n# Import the original Isaac's utils extension as library\nadd_library(libgxf_utils SHARED IMPORTED)\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n  if(CMAKE_DEVICE STREQUAL \"sbsa\")\n    set(GXF_EXT_LIB_PATH \"lib/gxf_aarch64_cuda_13_0\")\n  elseif(CMAKE_DEVICE STREQUAL \"arm64\")\n    set(GXF_EXT_LIB_PATH \"lib/gxf_jetpack70\")\n  else()\n    message(FATAL_ERROR \"Device is not supported.\")\n  endif()\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n    set(GXF_EXT_LIB_PATH \"lib/gxf_x86_64_cuda_13_0\")\nelse()\n    message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\nset_property(TARGET libgxf_utils PROPERTY IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/${GXF_EXT_LIB_PATH}/libgxf_utils.so)\n\n# gxf_isaac_uitls extension\nament_auto_add_library(gxf_isaac_utils SHARED\n  gxf/extensions/utils/utils.cpp\n  gxf/extensions/utils/disparity_to_depth.cpp\n  gxf/extensions/utils/disparity_to_depth.cu.cpp\n  gxf/extensions/utils/image_loader.cpp\n  gxf/extensions/utils/udp_receiver.cpp\n)\n# Mark as CUDA files with non-standard extensions\nset_source_files_properties(\n  gxf/extensions/utils/disparity_to_depth.cu.cpp\n  gxf/extensions/utils/disparity_to_depth.cu.hpp\n  PROPERTIES LANGUAGE CUDA\n)\ntarget_include_directories(gxf_isaac_utils PRIVATE \"${CMAKE_CURRENT_SOURCE_DIR}/gxf\")\ntarget_link_libraries(gxf_isaac_utils\n    CUDA::cudart\n    Eigen3::Eigen\n    yaml-cpp\n    libgxf_utils\n)\nset_target_properties(gxf_isaac_utils PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE\n  INSTALL_RPATH \"$ORIGIN:$ORIGIN/../share/${PROJECT_NAME}/gxf/lib\") # find libgxf_utils.so in share/\n\n# Disable -Wpedantic flag\nget_target_property(target_options gxf_isaac_utils COMPILE_OPTIONS)\nlist(REMOVE_ITEM target_options \"-Wpedantic\")\nset_property(TARGET gxf_isaac_utils PROPERTY COMPILE_OPTIONS ${target_options})\n\n# Install libgxf_isaac_utils.so in share/\ninstall(TARGETS gxf_isaac_utils DESTINATION share/${PROJECT_NAME}/gxf/lib)\n# Install libgxf_utils.so dependency in share/\ninstall(FILES ${CMAKE_CURRENT_SOURCE_DIR}/${GXF_EXT_LIB_PATH}/libgxf_utils.so\n        DESTINATION ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/lib)\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE)\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_utils/gxf/extensions/utils/disparity_to_depth.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"extensions/utils/disparity_to_depth.hpp\"\n\n#include <numeric>\n\n#include \"extensions/messages/camera_message.hpp\"\n#include \"extensions/utils/disparity_to_depth.cu.hpp\"\n#include \"gems/gxf_helpers/expected_macro_gxf.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\ngxf_result_t DisparityToDepth::registerInterface(gxf::Registrar* registrar) {\n  gxf::Expected<void> result;\n\n  result &= registrar->parameter(\n      disparity_input_, \"disparity_input\", \"Input disparity\",\n      \"Incoming CameraMessage frames with disparity\");\n  result &= registrar->parameter(\n      depth_output_, \"depth_output\", \"Output depth\",\n      \"Computed depth maps as CameraMessage\");\n  result &= registrar->parameter(\n      allocator_, \"allocator\", \"Allocator\",\n      \"Allocator to allocate output messages\");\n\n  return gxf::ToResultCode(result);\n}\n\ngxf_result_t DisparityToDepth::start() {\n  return GXF_SUCCESS;\n}\n\ngxf_result_t DisparityToDepth::stop() {\n  return GXF_SUCCESS;\n}\n\ngxf_result_t DisparityToDepth::tick() {\n  gxf::Entity message = UNWRAP_OR_RETURN(disparity_input_->receive());\n\n  CameraMessageParts disparity_message =\n    UNWRAP_OR_RETURN(GetCameraMessage(message));\n\n  gxf::VideoBufferInfo disparity_info = disparity_message.frame->video_frame_info();\n\n  // validate input message\n  if (disparity_message.frame->storage_type() != gxf::MemoryStorageType::kDevice) {\n    GXF_LOG_ERROR(\"Input disparity image must be stored in \"\n                  \"gxf::MemoryStorageType::kDevice\");\n    return GXF_INVALID_DATA_FORMAT;\n  }\n\n  if (disparity_info.color_format != gxf::VideoFormat::GXF_VIDEO_FORMAT_D32F) {\n    GXF_LOG_ERROR(\"Input disparity must be of type \"\n                  \"gxf::VideoFormat::GXF_VIDEO_FORMAT_D32F\");\n    return GXF_INVALID_DATA_FORMAT;\n  }\n\n  CameraMessageParts depth_message =\n    UNWRAP_OR_RETURN(CreateCameraMessage<gxf::VideoFormat::GXF_VIDEO_FORMAT_D32F>(\n        context(),\n        disparity_info.width,\n        disparity_info.height,\n        disparity_info.surface_layout,\n        gxf::MemoryStorageType::kDevice,\n        allocator_,\n        false));  // TODO(kpatzwaldt): change to true and adjust kernel accordingly\n\n  // compute parameters\n  float focal_length = disparity_message.intrinsics->focal_length.x;\n\n  std::array<float, 3> translation = disparity_message.extrinsics->translation;\n  float baseline = std::sqrt(std::inner_product(translation.begin(),\n                                                translation.end(),\n                                                translation.begin(),\n                                                0.0L));\n\n  // call CUDA kernel and wait for completion\n  disparity_to_depth_cuda(\n      reinterpret_cast<const float *>(disparity_message.frame->pointer()),\n      reinterpret_cast<float *>(depth_message.frame->pointer()),\n      baseline, focal_length, disparity_info.height, disparity_info.width);\n\n  // forward other components as is\n  *depth_message.intrinsics = *disparity_message.intrinsics;\n  *depth_message.extrinsics = *disparity_message.extrinsics;\n  *depth_message.sequence_number = *disparity_message.sequence_number;\n  *depth_message.timestamp = *disparity_message.timestamp;\n\n  RETURN_IF_ERROR(depth_output_->publish(depth_message.entity));\n\n  return GXF_SUCCESS;\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_utils/gxf/extensions/utils/disparity_to_depth.cu.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"disparity_to_depth.cu.hpp\"\n\n#include <cstdint>\n#include <limits>\n\n#include \"cuda.h\"\n#include \"cuda_runtime.h\"\n\nnamespace nvidia {\nnamespace isaac {\n\n__global__ void disparity_to_depth_kernel(\n    const float * input, float * output, float baseline, float focal_length,\n    int image_height, int image_width)\n{\n  const uint32_t x = blockIdx.x * blockDim.x + threadIdx.x;\n  const uint32_t y = blockIdx.y * blockDim.y + threadIdx.y;\n  const uint32_t index = y * image_width + x;\n  if (x < image_width && y < image_height)\n  {\n    if (input[index] > 0)\n    {\n      output[index] = (baseline * focal_length) / input[index];\n    } else\n    {\n      output[index] = 0;\n    }\n  }\n}\n\nuint16_t ceil_div(uint16_t numerator, uint16_t denominator)\n{\n  uint32_t accumulator = numerator + denominator - 1;\n  return accumulator / denominator;\n}\n\nvoid disparity_to_depth_cuda(\n    const float * input, float * output, float baseline, float focal_length,\n    int image_height, int image_width)\n{\n  dim3 block(16, 16);\n  dim3 grid(ceil_div(image_width, 16), ceil_div(image_height, 16), 1);\n  disparity_to_depth_kernel << < grid, block >> > (input, output, baseline, focal_length, image_height, image_width);\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_utils/gxf/extensions/utils/disparity_to_depth.cu.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma once\n\nnamespace nvidia {\nnamespace isaac {\n\nvoid disparity_to_depth_cuda(\n    const float * input, float * output, float baseline, float focal_length,\n    int imageHeight, int imageWidth);\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_utils/gxf/extensions/utils/disparity_to_depth.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/receiver.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Disparity to depth converter\n//\n// This codelet consumes a CameraMessage, and converts the disparity\n// in the VideoBuffer into a depth map\nclass DisparityToDepth : public gxf::Codelet {\n public:\n  gxf_result_t registerInterface(gxf::Registrar* registrar) override;\n  gxf_result_t start() override;\n  gxf_result_t stop() override;\n\n  gxf_result_t tick() override;\n\n private:\n  gxf::Parameter<gxf::Handle<gxf::Receiver>> disparity_input_;\n  gxf::Parameter<gxf::Handle<gxf::Transmitter>> depth_output_;\n  gxf::Parameter<gxf::Handle<gxf::Allocator>> allocator_;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_utils/gxf/extensions/utils/image_loader.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include \"extensions/utils/image_loader.hpp\"\n\n#include \"gems/core/image/image.hpp\"\n#include \"gems/core/tensor/tensor.hpp\"\n#include \"gems/gxf_helpers/image_view.hpp\"\n#include \"gems/image/io.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\ngxf_result_t ImageLoader::registerInterface(gxf::Registrar* registrar) {\n  gxf::Expected<void> result;\n  result &= registrar->parameter(\n      filename_, \"filename\", \"Filename\", \"Path to PNG or JPG/JPEG image file to load\",\n      gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL);\n  result &= registrar->parameter(\n      allocator_, \"allocator\", \"Allocator\",\n      \"Allocator for tensor\");\n  result &= registrar->parameter(\n      tensor_, \"tensor\", \"Tensor\",\n      \"Tensor to contain the loaded image data\");\n  result &= registrar->parameter(\n      color_, \"color\", \"Color\",\n      \"Whether the loaded image is loaded with color or not. False by default.\", false);\n  return gxf::ToResultCode(result);\n}\n\ngxf_result_t ImageLoader::initialize() {\n  if (!filename_.try_get()) {\n    GXF_LOG_WARNING(\"Not passing filename, so not using image loader\");\n    *(tensor_.get()) = gxf::Tensor();\n    return GXF_SUCCESS;\n  }\n\n  const auto& filename = filename_.try_get().value();\n\n  // Get image dimensions\n  ::nvidia::isaac::Vector3i dimensions;\n  if (!::nvidia::isaac::LoadImageShape(filename, dimensions)) {\n    GXF_LOG_ERROR(\"Failed to load image shape.\");\n    return GXF_FAILURE;\n  }\n  gxf::Shape shape;\n  if (color_.get()) {\n    shape = {dimensions[0], dimensions[1], 3};\n  } else {\n    shape = {dimensions[0], dimensions[1]};\n  }\n\n  // Reshape tensor\n  auto tensor_reshape_result = tensor_->reshape<uint8_t>(\n       shape, gxf::MemoryStorageType::kHost, allocator_);\n  if (!tensor_reshape_result) {\n    GXF_LOG_ERROR(\"Tensor reshape failed\");\n    return gxf::ToResultCode(tensor_reshape_result);\n  }\n\n  // Convert to Isaac TensorView\n  if (color_.get()) {\n    auto maybe_view = ToIsaacImageView<uint8_t, 3>(*tensor_.get());\n    if (!maybe_view) {\n      GXF_LOG_ERROR(\"Could not create view\");\n      return gxf::ToResultCode(maybe_view);\n    }\n    // Load image from file\n    if (!::nvidia::isaac::LoadImage(filename, maybe_view.value())) {\n      GXF_LOG_ERROR(\"Failed to load image with color.\");\n      return GXF_FAILURE;\n    }\n  } else {\n    auto maybe_view = ToIsaacImageView<uint8_t, 1>(*tensor_.get());\n    if (!maybe_view) {\n      GXF_LOG_ERROR(\"Could not create view\");\n      return gxf::ToResultCode(maybe_view);\n    }\n    // Load image from file\n    if (!::nvidia::isaac::LoadImage(filename, maybe_view.value())) {\n      GXF_LOG_ERROR(\"Failed to load image without color.\");\n      return GXF_FAILURE;\n    }\n  }\n  return GXF_SUCCESS;\n}\n\ngxf_result_t ImageLoader::deinitialize() {\n  // Clears loaded Tensor to avoid racing destruction with Allocator instance\n  *(tensor_.get()) = gxf::Tensor();\n  return GXF_SUCCESS;\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_utils/gxf/extensions/utils/image_loader.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <array>\n#include <string>\n\n#include \"gxf/core/component.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/tensor.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// This component loads data from an image file into a 2 or 3-dimensional tensor. Supports both\n// grayscale and color PNGs and color JPEGs. On initialize, codelet allocates the\n// appropriate amount of memory for the tensor based on the provided dimensions.\nclass ImageLoader : public gxf::Component {\n public:\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override;\n\n  gxf_result_t registerInterface(gxf::Registrar* registrar) override;\n\n private:\n  gxf::Parameter<std::string> filename_;\n  gxf::Parameter<gxf::Handle<gxf::Allocator>> allocator_;\n  gxf::Parameter<gxf::Handle<gxf::Tensor>> tensor_;\n  gxf::Parameter<bool> color_;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_utils/gxf/extensions/utils/udp_receiver.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include \"extensions/utils/udp_receiver.hpp\"\n\n#include <sys/eventfd.h>\n#include <sys/poll.h>\n\n#include <utility>\n\n#include \"common/span.hpp\"\n#include \"gxf/std/memory_buffer.hpp\"\n#include \"gxf/std/tensor.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\nnamespace {\n\n// Maximum size of a UDP packet\n// Assumes a 2 byte size field (65535 bytes maximum)\n// with 20 bytes reserved for IP header and 8 bytes reserved for UDP header\nconstexpr size_t kUdpMaxPacketSize = 65507;\n\n}\n\ngxf_result_t UdpReceiver::registerInterface(gxf::Registrar* registrar) {\n  if (registrar == nullptr) {\n    return GXF_ARGUMENT_NULL;\n  }\n  gxf::Expected<void> result;\n  result &= registrar->parameter(\n      tensor_, \"tensor\", \"Tensor\",\n      \"Tensor output\");\n  result &= registrar->parameter(\n      allocator_, \"allocator\", \"Allocator\",\n      \"Memory allocator\");\n  result &= registrar->parameter(\n      async_scheduling_term_, \"async_scheduling_term\", \"Asynchronous Scheduling Term\",\n      \"Schedules execution when socket has data available to read\");\n  result &= registrar->parameter(\n      address_, \"address\", \"Address\",\n      \"IP address\");\n  result &= registrar->parameter(\n      port_, \"port\", \"Port\",\n      \"Port number\");\n  result &= registrar->parameter(\n      packet_accumulation_, \"packet_accumulation\", \"Packet Accumulation\",\n      \"Number of packets to accumulate before publishing\",\n      1UL);\n  result &= registrar->parameter(\n      buffer_size_, \"buffer_size\", \"Buffer Size\",\n      \"Read buffer size in bytes\",\n      kUdpMaxPacketSize);\n  result &= registrar->parameter(\n      receive_buffer_size_, \"receive_buffer_size\", \"Receive Buffer Size\",\n      \"UDP receive buffer size in bytes (overrides value in /proc/sys/net/core/rmem_default)\",\n      gxf::Registrar::NoDefaultParameter(), GXF_PARAMETER_FLAGS_OPTIONAL);\n  return gxf::ToResultCode(result);\n}\n\ngxf_result_t UdpReceiver::initialize() {\n  if (packet_accumulation_ == 0) {\n    GXF_LOG_ERROR(\"packet_accumulation must be greater than 0\");\n    return GXF_PARAMETER_OUT_OF_RANGE;\n  }\n  if (buffer_size_ == 0 || buffer_size_ > kUdpMaxPacketSize) {\n    GXF_LOG_ERROR(\"buffer_size must be greater than 0 and less than or equal to %zu\",\n                  kUdpMaxPacketSize);\n    return GXF_PARAMETER_OUT_OF_RANGE;\n  }\n  return GXF_SUCCESS;\n}\n\ngxf_result_t UdpReceiver::start() {\n  // Create UDP socket\n  socket_.reset(::nvidia::isaac::Socket::CreateRxUDPSocket(address_, port_));\n  int result = socket_->startSocket();\n  if (result < 0) {\n    GXF_LOG_ERROR(\"Failed to start socket\");\n    return GXF_FAILURE;\n  }\n\n  auto receive_buffer_size = receive_buffer_size_.try_get();\n  if (receive_buffer_size) {\n    int opt = static_cast<int>(receive_buffer_size.value());\n    uint32_t length = sizeof(opt);\n    result = setsockopt(socket_->getFileDescriptor(), SOL_SOCKET, SO_RCVBUF, &opt, length);\n    if (result != 0) {\n      GXF_LOG_ERROR(\"%s\", strerror(errno));\n      return GXF_FAILURE;\n    }\n    result = getsockopt(socket_->getFileDescriptor(), SOL_SOCKET, SO_RCVBUF, &opt, &length);\n    if (result != 0) {\n      GXF_LOG_ERROR(\"%s\", strerror(errno));\n      return GXF_FAILURE;\n    }\n    // Configured size is doubled by kernel for bookkeeping so divide actual size by 2\n    opt /= 2;\n    if (static_cast<size_t>(opt) != receive_buffer_size.value()) {\n      // If the configured buffer size does not match the requested buffer size,\n      // then the requested amount is likely greater than the value specified in\n      // /proc/sys/net/core/rmem_max\n      GXF_LOG_WARNING(\"UDP receive buffer is %dB but user requested %luB\",\n                      opt, receive_buffer_size.value());\n    }\n  }\n\n  // Create event file descriptor\n  event_fd_ = eventfd(0, 0);\n  if (event_fd_ < 0) {\n    GXF_LOG_ERROR(\"%s\", strerror(errno));\n    return GXF_FAILURE;\n  }\n\n  // Start async thread\n  thread_ = std::thread([this] { asyncSocketMonitor(); });\n\n  return GXF_SUCCESS;\n}\n\ngxf_result_t UdpReceiver::tick() {\n  // Create message entity\n  auto entity = gxf::Entity::New(context());\n  if (!entity) {\n    return gxf::ToResultCode(entity);\n  }\n\n  // Accumulate packets and add them to message entity as tensors\n  for (size_t packet = 0; packet < packet_accumulation_; packet++) {\n    gxf::MemoryBuffer buffer;\n    gxf::Handle<gxf::Tensor> tensor;\n    auto result = entity->add<gxf::Tensor>()\n        .assign_to(tensor)\n        .and_then([&]() {\n          return buffer.resize(allocator_, buffer_size_, gxf::MemoryStorageType::kSystem);\n        })\n        .and_then([&]() -> gxf::Expected<int> {\n          Span<char> span(reinterpret_cast<char*>(buffer.pointer()), buffer.size());\n          // This call will block if there is no data available on the socket\n          const int received = socket_->readPacket(span.data(), span.size());\n          if (received < 0) {\n            GXF_LOG_ERROR(\"Failed to read from socket\");\n            return gxf::Unexpected{GXF_FAILURE};\n          }\n          return received;\n        })\n        .map([&](int size) {\n          return tensor->wrapMemoryBuffer({size},\n                                          gxf::PrimitiveType::kUnsigned8,\n                                          PrimitiveTypeSize(gxf::PrimitiveType::kUnsigned8),\n                                          gxf::Unexpected{GXF_UNINITIALIZED_VALUE},\n                                          std::move(buffer));\n        });\n    if (!result) {\n      return gxf::ToResultCode(result);\n    }\n  }\n\n  // Put main thread in a waiting state until socket has data to read\n  async_scheduling_term_->setEventState(gxf::AsynchronousEventState::EVENT_WAITING);\n  const uint64_t state = static_cast<uint64_t>(async_scheduling_term_->getEventState());\n  const ssize_t bytes = write(event_fd_, &state, sizeof(state));\n  if (bytes < 0) {\n    GXF_LOG_ERROR(\"Failed to write file descriptor: %s\", strerror(errno));\n    return GXF_FAILURE;\n  }\n\n  // Publish message entity with timestamp\n  return gxf::ToResultCode(tensor_->publish(entity.value(), getExecutionTimestamp()));\n}\n\ngxf_result_t UdpReceiver::stop() {\n  // Stop async thread and wait for exit\n  async_scheduling_term_->setEventState(gxf::AsynchronousEventState::EVENT_NEVER);\n  const uint64_t state = static_cast<uint64_t>(async_scheduling_term_->getEventState());\n  const ssize_t bytes = write(event_fd_, &state, sizeof(state));\n  if (bytes < 0) {\n    GXF_LOG_ERROR(\"Failed to write file descriptor: %s\", strerror(errno));\n    return GXF_FAILURE;\n  }\n  thread_.join();\n\n  // Close event file descriptor\n  const int result = close(event_fd_);\n  if (result != 0) {\n    GXF_LOG_ERROR(\"%s\", strerror(errno));\n    return GXF_FAILURE;\n  }\n\n  // Close UDP socket\n  socket_->closeSocket();\n\n  return GXF_SUCCESS;\n}\n\nvoid UdpReceiver::asyncSocketMonitor() {\n  // Poll structure for state\n  pollfd state_fds;\n  state_fds.fd = event_fd_;\n  state_fds.events = POLLIN;\n\n  // Poll structure for socket\n  pollfd socket_fds;\n  socket_fds.fd = socket_->getFileDescriptor();\n  socket_fds.events = POLLIN;\n\n  while (true) {\n    // Block until main thread signals a state change\n    int result = poll(&state_fds, 1, -1);\n    if (result < 0) {\n      LOG_ERROR(\"%s\", strerror(errno));\n    }\n\n    if (state_fds.revents & POLLIN) {\n      // Read current state from main thread\n      uint64_t state;\n      const ssize_t bytes = read(state_fds.fd, &state, sizeof(state));\n      if (bytes < 0) {\n        GXF_LOG_ERROR(\"%s\", strerror(errno));\n      }\n\n      // Stop async thread if codelet is stopped\n      if (state == static_cast<uint64_t>(gxf::AsynchronousEventState::EVENT_NEVER)) {\n        break;\n      }\n\n      // Main thread should be done reading from socket before polling\n      if (state != static_cast<uint64_t>(gxf::AsynchronousEventState::EVENT_WAITING)) {\n        continue;\n      }\n    }\n\n    // Block until data is available to read on the socket\n    result = poll(&socket_fds, 1, -1);\n    if (result < 0) {\n      LOG_ERROR(\"%s\", strerror(errno));\n    }\n\n    // Change scheduling state if there is data available to read\n    if (socket_fds.revents & POLLIN) {\n      async_scheduling_term_->setEventState(gxf::AsynchronousEventState::EVENT_DONE);\n    }\n  }\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_utils/gxf/extensions/utils/udp_receiver.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#pragma once\n\n#include <memory>\n#include <string>\n#include <thread>\n\n#include \"gems/coms/socket.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/codelet.hpp\"\n#include \"gxf/std/scheduling_terms.hpp\"\n#include \"gxf/std/transmitter.hpp\"\n\nnamespace nvidia {\nnamespace isaac {\n\n// Network interface codelet that receives data from a UDP socket and publishes the data as a tensor\nclass UdpReceiver : public gxf::Codelet {\n public:\n  gxf_result_t registerInterface(gxf::Registrar* registrar) override;\n  gxf_result_t initialize() override;\n  gxf_result_t deinitialize() override { return GXF_SUCCESS; }\n\n  gxf_result_t start() override;\n  gxf_result_t tick() override;\n  gxf_result_t stop() override;\n\n private:\n  // Asynchronous thread that monitors when the socket has data available to read\n  void asyncSocketMonitor();\n\n  gxf::Parameter<gxf::Handle<gxf::Transmitter>> tensor_;\n  gxf::Parameter<gxf::Handle<gxf::Allocator>> allocator_;\n  gxf::Parameter<gxf::Handle<gxf::AsynchronousSchedulingTerm>> async_scheduling_term_;\n  gxf::Parameter<std::string> address_;\n  gxf::Parameter<uint16_t> port_;\n  gxf::Parameter<size_t> packet_accumulation_;\n  gxf::Parameter<size_t> buffer_size_;\n  gxf::Parameter<size_t> receive_buffer_size_;\n\n  // UDP socket to receive data from\n  std::unique_ptr<::nvidia::isaac::Socket> socket_;\n  // Async thread\n  std::thread thread_;\n  // Event file descriptor used to stop async thread\n  int event_fd_;\n};\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_utils/gxf/extensions/utils/utils.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include \"extensions/utils/disparity_to_depth.hpp\"\n#include \"extensions/utils/image_loader.hpp\"\n#include \"extensions/utils/udp_receiver.hpp\"\n#include \"gxf/std/extension_factory_helper.hpp\"\n\nGXF_EXT_FACTORY_BEGIN()\n\nGXF_EXT_FACTORY_SET_INFO(0x157ff311835a4333, 0x1f3c65ceeb5847b4, \"NvIsaacUtilsExtension\",\n                         \"Extension containing miscellaneous utility components\", \"Isaac SDK\",\n                         \"2.0.0\", \"LICENSE\");\n\nGXF_EXT_FACTORY_ADD(0x0626d66ce3ae11ed, 0x800e63ef7b59e300,\n                    nvidia::isaac::DisparityToDepth, nvidia::gxf::Codelet,\n                    \"Converts disparity maps to depth maps\");\n\nGXF_EXT_FACTORY_ADD(0x666c1d1dae4e4e81, 0xd1e4c3bef6f34a68,\n                    nvidia::isaac::ImageLoader, nvidia::gxf::Component,\n                    \"Loads image data into a Tensor\");\n\nGXF_EXT_FACTORY_ADD(0xcb7d1d801f5daf96, 0x985e15059bd5a332,\n                    nvidia::isaac::UdpReceiver, nvidia::gxf::Codelet,\n                    \"Receives packets from a UDP socket and publishes them as tensors\");\n\nGXF_EXT_FACTORY_END()\n"
  },
  {
    "path": "isaac_ros_gxf_extensions/gxf_isaac_utils/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nSPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\nhttp://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\nSPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>gxf_isaac_utils</name>\n  <version>4.4.0</version>\n  <description>NvIsaacUtilsExtension</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake_auto</buildtool_depend>\n\n  <depend>isaac_ros_common</depend>\n  <depend>isaac_ros_gxf</depend>\n  <depend>gxf_isaac_messages</depend>\n\n  <build_depend>gxf_isaac_gems</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_managed_nitros/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_managed_nitros LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Isaac ROS Managed Nitros\n# include_directories( include )\n\n# install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}/)\n# ament_export_include_directories(include)\n\n\n# Add an interface target to export dependencies\nadd_library(${PROJECT_NAME} INTERFACE)\n\n# Install headers\ninstall(DIRECTORY \"${CMAKE_CURRENT_SOURCE_DIR}/include/\"\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/gxf/include\")\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  INTERFACE_INCLUDE_DIRECTORIES \"$<INSTALL_PREFIX>/share/${PROJECT_NAME}/gxf/include\")\n# set_property(TARGET ${PROJECT_NAME} PROPERTY\n#   INTERFACE_LINK_LIBRARIES\n#     gxf_isaac_message_compositor::gxf_isaac_message_compositor\n# )\n\ninstall(TARGETS ${PROJECT_NAME}\n        EXPORT export_${PROJECT_NAME}\n        LIBRARY DESTINATION lib\n        ARCHIVE DESTINATION lib\n        RUNTIME DESTINATION bin)\nament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)\n\nif(BUILD_TESTING)\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()"
  },
  {
    "path": "isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_message_filters_subscriber.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_MANAGED_NITROS__MANAGED_NITROS_MESSAGE_FILTERS_SUBSCRIBER_HPP_\n#define ISAAC_ROS_MANAGED_NITROS__MANAGED_NITROS_MESSAGE_FILTERS_SUBSCRIBER_HPP_\n\n#include <memory>\n#include <string>\n\n#include \"message_filters/subscriber.h\"\n\n#include \"isaac_ros_managed_nitros/managed_nitros_subscriber.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_message_filter_traits.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\nnamespace message_filters\n{\n\ntemplate<class NitrosTypeViewT, class NodeType = rclcpp::Node>\nclass Subscriber\n  : public ::message_filters::SubscriberBase<NodeType>,\n  public ::message_filters::SimpleFilter<typename NitrosTypeViewT::BaseType>\n{\npublic:\n  typedef std::shared_ptr<NodeType> NodePtr;\n  typedef typename NitrosTypeViewT::BaseType MessageType;\n  typedef ::message_filters::MessageEvent<MessageType const> EventType;\n\n  /**\n   * \\brief Constructor\n   *\n   * See the rclcpp::Node::subscribe() variants for more information on the parameters\n   *\n   * \\param node The rclcpp::Node::SharedPtr to use to subscribe.\n   * \\param topic The topic to subscribe to.\n   * \\param qos (optional) The rmw qos profile to use to subscribe\n   */\n  Subscriber(\n    NodePtr node, const std::string & topic,\n    const rmw_qos_profile_t qos = rmw_qos_profile_default)\n  {\n    subscribe(node, topic, qos);\n  }\n\n  Subscriber(\n    NodeType * node, const std::string & topic,\n    const rmw_qos_profile_t qos = rmw_qos_profile_default)\n  {\n    subscribe(node, topic, qos);\n  }\n\n  /**\n   * \\brief Constructor\n   *\n   * See the rclcpp::Node::subscribe() variants for more information on the parameters\n   *\n   * \\param node The rclcpp::Node::SharedPtr to use to subscribe.\n   * \\param topic The topic to subscribe to.\n   * \\param qos The rmw qos profile to use to subscribe.\n   * \\param options The subscription options to use to subscribe.\n   */\n  Subscriber(\n    NodePtr node,\n    const std::string & topic,\n    const rmw_qos_profile_t qos,\n    rclcpp::SubscriptionOptions options)\n  {\n    subscribe(node.get(), topic, qos, options);\n  }\n\n  Subscriber(\n    NodeType * node,\n    const std::string & topic,\n    const rmw_qos_profile_t qos,\n    rclcpp::SubscriptionOptions options)\n  {\n    subscribe(node, topic, qos, options);\n  }\n\n  /**\n   * \\brief Empty constructor, use subscribe() to subscribe to a topic\n   */\n  Subscriber() = default;\n\n  ~Subscriber()\n  {\n    unsubscribe();\n  }\n\n  /**\n   * \\brief Subscribe to a topic.\n   *\n   * If this Subscriber is already subscribed to a topic, this function will first unsubscribe.\n   *\n   * \\param node The rclcpp::Node::SharedPtr to use to subscribe.\n   * \\param topic The topic to subscribe to.\n   * \\param qos (optional) The rmw qos profile to use to subscribe\n   */\n  void subscribe(\n    NodePtr node, const std::string & topic,\n    const rmw_qos_profile_t qos = rmw_qos_profile_default) override\n  {\n    // Use 4-parameter shared pointer overload\n    subscribe(node, topic, qos, rclcpp::SubscriptionOptions());\n  }\n\n  /**\n   * \\brief Subscribe to a topic.\n   *\n   * If this Subscriber is already subscribed to a topic, this function will first unsubscribe.\n   *\n   * \\param node The rclcpp::Node to use to subscribe.\n   * \\param topic The topic to subscribe to.\n   * \\param qos (optional) The rmw qos profile to use to subscribe\n   */\n  // TODO(wjwwood): deprecate in favor of API's that use `rclcpp::QoS` instead.\n  void subscribe(\n    NodeType * node, const std::string & topic,\n    const rmw_qos_profile_t qos = rmw_qos_profile_default) override\n  {\n    // Use 4-parameter raw pointer overload\n    subscribe(node, topic, qos, rclcpp::SubscriptionOptions());\n  }\n\n  /**\n   * \\brief Subscribe to a topic.\n   *\n   * If this Subscriber is already subscribed to a topic, this function will first unsubscribe.\n   *\n   * \\param node The rclcpp::Node::SharedPtr to use to subscribe.\n   * \\param topic The topic to subscribe to.\n   * \\param qos The rmw qos profile to use to subscribe.\n   * \\param options The subscription options to use to subscribe.\n   */\n  void subscribe(\n    NodePtr node,\n    const std::string & topic,\n    const rmw_qos_profile_t qos,\n    rclcpp::SubscriptionOptions options) override\n  {\n    // Use 5-parameter shared pointer overload\n    subscribe(node, topic, qos, options, \"\");\n  }\n\n  /**\n   * \\brief Subscribe to a topic.\n   *\n   * If this Subscriber is already subscribed to a topic, this function will first unsubscribe.\n   *\n   * \\param node The rclcpp::Node to use to subscribe.\n   * \\param topic The topic to subscribe to.\n   * \\param qos The rmw qos profile to use to subscribe.\n   * \\param options The subscription options to use to subscribe.\n   */\n  void subscribe(\n    NodeType * node,\n    const std::string & topic,\n    const rmw_qos_profile_t qos,\n    rclcpp::SubscriptionOptions options) override\n  {\n    // Use 5-parameter raw pointer overload\n    subscribe(node, topic, qos, options, \"\");\n  }\n\n  /**\n   * \\brief Subscribe to a topic.\n   *\n   * If this Subscriber is already subscribed to a topic, this function will first unsubscribe.\n   *\n   * \\param node The rclcpp::Node::SharedPtr to use to subscribe.\n   * \\param topic The topic to subscribe to.\n   * \\param qos The rmw qos profile to use to subscribe\n   * \\param options The subscription options to use to subscribe.\n   * \\param compatible_data_format The NITROS compatible data format to use to subscribe.\n   * \\param diagnostics_config The NITROS diagnostics config to use to subscribe.\n   */\n  void subscribe(\n    NodePtr node,\n    const std::string & topic,\n    const rmw_qos_profile_t qos,\n    rclcpp::SubscriptionOptions options,\n    const std::string & compatible_data_format,\n    const NitrosDiagnosticsConfig & diagnostics_config = {})\n  {\n    // Use raw pointer implementation, then reset the node pointer member variables\n    subscribe(node.get(), topic, qos, options, compatible_data_format, diagnostics_config);\n    node_raw_ = nullptr;\n    node_shared_ = node;\n  }\n\n  /**\n   * \\brief Subscribe to a topic.\n   *\n   * If this Subscriber is already subscribed to a topic, this function will first unsubscribe.\n   *\n   * \\param node The rclcpp::Node to use to subscribe.\n   * \\param topic The topic to subscribe to.\n   * \\param qos The rmw qos profile to use to subscribe\n   * \\param options The subscription options to use to subscribe.\n   * \\param compatible_data_format The NITROS compatible data format to use to subscribe.\n   * \\param diagnostics_config The NITROS diagnostics config to use to subscribe.\n   */\n  void subscribe(\n    NodeType * node,\n    const std::string & topic,\n    const rmw_qos_profile_t qos,\n    rclcpp::SubscriptionOptions options,\n    const std::string & compatible_data_format,\n    const NitrosDiagnosticsConfig & diagnostics_config = {})\n  {\n    unsubscribe();\n\n    if (!topic.empty()) {\n      topic_ = topic;\n      rclcpp::QoS rclcpp_qos(rclcpp::QoSInitialization::from_rmw(qos));\n      rclcpp_qos.get_rmw_qos_profile() = qos;\n      qos_ = qos;\n      options_ = options;\n      compatible_data_format_ = compatible_data_format !=\n        \"\" ? compatible_data_format : MessageType::GetDefaultCompatibleFormat();\n      diagnostics_config_ = diagnostics_config;\n\n      sub_ = std::make_shared<ManagedNitrosSubscriber<NitrosTypeViewT>>(\n        node,\n        topic,\n        compatible_data_format_,\n        [this](const NitrosTypeViewT & nitrosViewMsg) {\n          this->cb(EventType(std::make_shared<const MessageType>(nitrosViewMsg.GetMessage())));\n        },\n        diagnostics_config,\n        rclcpp_qos\n      );\n\n      node_raw_ = node;\n    }\n  }\n\n  /**\n   * \\brief Re-subscribe to a topic.  Only works if this subscriber has previously been subscribed to a topic.\n   */\n  void subscribe() override\n  {\n    if (!topic_.empty()) {\n      if (node_raw_ != nullptr) {\n        subscribe(node_raw_, topic_, qos_, options_, compatible_data_format_, diagnostics_config_);\n      } else if (node_shared_ != nullptr) {\n        subscribe(node_shared_, topic_, qos_, options_, compatible_data_format_,\n                  diagnostics_config_);\n      }\n    }\n  }\n\n  /**\n   * \\brief Force immediate unsubscription of this subscriber from its topic\n   */\n  void unsubscribe() override\n  {\n    sub_.reset();\n  }\n\n  std::string getTopic() const\n  {\n    return this->topic_;\n  }\n\n  /**\n   * \\brief Returns the internal shared pointer to the ManagedNitrosSubscriber<NitrosTypeViewT> object\n   */\n  const std::shared_ptr<ManagedNitrosSubscriber<NitrosTypeViewT>> getSubscriber() const\n  {\n    return sub_;\n  }\n\n  /**\n   * \\brief Does nothing.  Provided so that Subscriber may be used in a message_filters::Chain\n   */\n  template<typename F>\n  void connectInput(F & f)\n  {\n    (void)f;\n  }\n\n  /**\n   * \\brief Does nothing.  Provided so that Subscriber may be used in a message_filters::Chain\n   */\n  void add(const EventType & e)\n  {\n    (void)e;\n  }\n\nprivate:\n  void cb(const EventType & e)\n  {\n    this->signalMessage(e);\n  }\n\n  std::shared_ptr<ManagedNitrosSubscriber<NitrosTypeViewT>> sub_;\n\n  NodePtr node_shared_;\n  NodeType * node_raw_ {nullptr};\n\n  std::string topic_;\n  rmw_qos_profile_t qos_;\n  rclcpp::SubscriptionOptions options_;\n  std::string compatible_data_format_;\n  NitrosDiagnosticsConfig diagnostics_config_;\n};\n\n}  // namespace message_filters\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_MANAGED_NITROS__MANAGED_NITROS_MESSAGE_FILTERS_SUBSCRIBER_HPP_\n"
  },
  {
    "path": "isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_publisher.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_MANAGED_NITROS__MANAGED_NITROS_PUBLISHER_HPP_\n#define ISAAC_ROS_MANAGED_NITROS__MANAGED_NITROS_PUBLISHER_HPP_\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"message_compositor/message_relay.hpp\"\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#include \"gxf/std/vault.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"extensions/gxf_optimizer/core/optimizer.hpp\"\n#include \"extensions/gxf_optimizer/exporter/graph_types.hpp\"\n\n#include \"isaac_ros_nitros/nitros_publisher.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_manager.hpp\"\n#include \"isaac_ros_nitros/nitros_publisher_subscriber_group.hpp\"\n#include \"isaac_ros_nitros/nitros_context.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\ntemplate<typename T>\nclass ManagedNitrosPublisher\n{\npublic:\n  ManagedNitrosPublisher(\n    rclcpp::Node * node,\n    const std::string & topic,\n    const std::string & format,\n    const NitrosDiagnosticsConfig & diagnostics_config = {},\n    const rclcpp::QoS qos = rclcpp::QoS(1))\n  : node_{node},\n    context_{GetTypeAdapterNitrosContext()},\n    nitros_type_manager_{std::make_shared<NitrosTypeManager>(node_)}\n  {\n    nitros_type_manager_->registerSupportedType<T>();\n    nitros_type_manager_->loadExtensions(format);\n\n    std::vector<std::string> supported_data_formats{format};\n\n    NitrosPublisherSubscriberConfig component_config{\n      .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED,\n      .qos = qos,\n      .compatible_data_format = format,\n      .topic_name = topic\n    };\n\n    nitros_pub_ = std::make_shared<NitrosPublisher>(\n      *node_, GetTypeAdapterNitrosContext().getContext(), nitros_type_manager_,\n      supported_data_formats, component_config, diagnostics_config);\n\n    nitros_pub_->start();\n\n    RCLCPP_INFO(\n      node_->get_logger().get_child(\"ManagedNitrosPublisher\"),\n      \"Starting Managed Nitros Publisher\");\n  }\n\n  void publish(T msg)\n  {\n    nitros_pub_->publish(msg);\n  }\n\nprivate:\n  rclcpp::Node * node_;\n  NitrosContext context_;\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager_;\n  std::shared_ptr<NitrosPublisher> nitros_pub_;\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_MANAGED_NITROS__MANAGED_NITROS_PUBLISHER_HPP_\n"
  },
  {
    "path": "isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_subscriber.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_MANAGED_NITROS__MANAGED_NITROS_SUBSCRIBER_HPP_\n#define ISAAC_ROS_MANAGED_NITROS__MANAGED_NITROS_SUBSCRIBER_HPP_\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"extensions/gxf_optimizer/core/optimizer.hpp\"\n#include \"isaac_ros_nitros/nitros_subscriber.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_manager.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\ntemplate<typename NitrosMsgView>\nclass ManagedNitrosSubscriber\n{\npublic:\n  explicit ManagedNitrosSubscriber(\n    rclcpp::Node * node,\n    const std::string & topic_name,\n    const std::string & format,\n    std::function<void(const NitrosMsgView & msg_view)> callback = nullptr,\n    const NitrosDiagnosticsConfig & diagnostics_config = {},\n    const rclcpp::QoS qos = rclcpp::QoS(1))\n  : node_{node}, topic_{topic_name},\n    nitros_type_manager_{std::make_shared<NitrosTypeManager>(node_)}\n  {\n    nitros_type_manager_->registerSupportedType<typename NitrosMsgView::BaseType>();\n    nitros_type_manager_->loadExtensions(format);\n\n    std::vector<std::string> supported_data_formats{format};\n\n    NitrosPublisherSubscriberConfig component_config{\n      .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED,\n      .qos = qos,\n      .compatible_data_format = format,\n      .topic_name = topic_name,\n      .callback = [callback](const gxf_context_t, NitrosTypeBase & msg) -> void {\n          const NitrosMsgView view(*(static_cast<typename NitrosMsgView::BaseType *>(&msg)));\n          callback(view);\n        }\n    };\n\n    nitros_sub_ = std::make_shared<NitrosSubscriber>(\n      *node_, GetTypeAdapterNitrosContext().getContext(), nitros_type_manager_,\n      supported_data_formats, component_config, diagnostics_config);\n\n    nitros_sub_->start();\n\n    RCLCPP_INFO(\n      node_->get_logger().get_child(\"ManagedNitrosSubscriber\"),\n      \"Starting Managed Nitros Subscriber\");\n  }\n\nprivate:\n  rclcpp::Node * node_;\n  std::string topic_;\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager_;\n  std::shared_ptr<NitrosSubscriber> nitros_sub_;\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_MANAGED_NITROS__MANAGED_NITROS_SUBSCRIBER_HPP_\n"
  },
  {
    "path": "isaac_ros_managed_nitros/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_managed_nitros</name>\n  <version>4.4.0</version>\n  <description>Utilities for leveraging NITROS in custom ROS 2 nodes</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Jaiveer Singh</author>\n  <author>Swapnesh Wani</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>message_filters</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>isaac_ros_nitros_tensor_list_type</depend>\n  <depend>isaac_ros_gxf</depend>\n  <depend>gxf_isaac_message_compositor</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_dnn_image_encoder/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(custom_nitros_dnn_image_encoder LANGUAGES C CXX CUDA)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wno-narrowing)\nendif()\n\n\nfind_package(ament_cmake_auto REQUIRED)\n\nament_auto_find_build_dependencies()\n\n# image Encoder node\nament_auto_add_library(image_encoder_node SHARED src/image_encoder_node.cpp)\ntarget_link_libraries(image_encoder_node cvcuda nvcv_types)\nrclcpp_components_register_nodes(image_encoder_node \"custom_nitros_dnn_image_encoder::ImageEncoderNode\")\nset(node_plugins \"${node_plugins}custom_nitros_dnn_image_encoder::ImageEncoderNode;$<TARGET_FILE:image_encoder_node>\\n\")\nset_target_properties(image_encoder_node PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  find_package(ament_lint_auto REQUIRED)\n  ament_lint_auto_find_test_dependencies()\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/custom_nitros_dnn_image_encoder_pol.py TIMEOUT \"600\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE launch)"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_dnn_image_encoder/include/custom_nitros_dnn_image_encoder/image_encoder_node.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef CUSTOM_NITROS_DNN_IMAGE_ENCODER__IMAGE_ENCODER_NODE_HPP_\n#define CUSTOM_NITROS_DNN_IMAGE_ENCODER__IMAGE_ENCODER_NODE_HPP_\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"rclcpp/rclcpp.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_publisher.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list.hpp\"\n#include \"sensor_msgs/msg/image.hpp\"\n\n#include <nvcv/Tensor.hpp>\n\n#include <cvcuda/OpResize.hpp>\n#include <cvcuda/OpCvtColor.hpp>\n#include <cvcuda/OpConvertTo.hpp>\n#include <cvcuda/OpNormalize.hpp>\n#include <cvcuda/OpReformat.hpp>\n\n#include \"cuda_runtime.h\"  // NOLINT - include .h without directory\n\nnamespace custom_nitros_dnn_image_encoder\n{\n\nstruct NVCVImageFormat\n{\n  nvcv::ImageFormat interleaved_format;\n  nvcv::ImageFormat planar_float_format;\n  nvcv::ImageFormat interleaved_float_format;\n};\n\nclass ImageEncoderNode : public rclcpp::Node\n{\npublic:\n  explicit ImageEncoderNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions());\n\n  ~ImageEncoderNode();\n\nprivate:\n  void InputCallback(const sensor_msgs::msg::Image::SharedPtr msg);\n\n  // Subscription to input image messages\n  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;\n\n  // Publisher for output NitrosTensorList messages\n  std::shared_ptr<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n      nvidia::isaac_ros::nitros::NitrosTensorList>> nitros_pub_;\n\n  // Name of tensor in NitrosTensorList\n  std::string tensor_name_{};\n  std::string input_image_encoding_{};\n  std::string output_image_encoding_{};\n  const uint16_t output_image_width_{};\n  const uint16_t output_image_height_{};\n  const uint16_t input_image_width_{};\n  const uint16_t input_image_height_{};\n  std::vector<double> image_mean_{};\n  std::vector<double> image_stddev_{};\n\n  const uint16_t batch_size_ = 1;\n  uint16_t input_image_channels_ = 0;\n  uint16_t output_image_channels_ = 0;\n\n  nvcv::TensorDataStridedCuda::Buffer input_image_buffer_;\n  nvcv::Tensor input_image_tensor_;\n\n  nvcv::TensorDataStridedCuda::Buffer output_image_buffer_;\n  nvcv::Tensor output_image_tensor_;\n\n  cvcuda::Resize resize_op_;\n  cvcuda::CvtColor cvtColor_op_;\n  cvcuda::Reformat reformat_op_;\n  cvcuda::Normalize norm_op_;\n  cvcuda::ConvertTo convert_op_;\n\n  nvcv::Tensor std_dev_tensor_;\n  nvcv::Tensor mean_tensor_;\n  nvcv::Tensor resized_tensor_;\n  nvcv::Tensor color_converted_tensor_;\n  nvcv::Tensor float_tensor_;\n  nvcv::Tensor norm_tensor_;\n\n  NVCVColorConversionCode color_conversion_code_;\n\n  // CUDA stream to process dynamics detection on\n  cudaStream_t cuda_stream_;\n};\n\n}  // namespace custom_nitros_dnn_image_encoder\n\n#endif  // CUSTOM_NITROS_DNN_IMAGE_ENCODER__IMAGE_ENCODER_NODE_HPP_\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_dnn_image_encoder/launch/custom_image_isaac_ros_dope_tensor_rt.launch.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\nimport launch\nfrom launch.actions import DeclareLaunchArgument\nfrom launch.substitutions import LaunchConfiguration\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\n\n\ndef generate_launch_description():\n    \"\"\"Generate launch description for DOPE encoder->TensorRT->DOPE decoder.\"\"\"\n    launch_args = [\n        DeclareLaunchArgument(\n            'output_image_width',\n            default_value='640',\n            description='The expected output image width from custom encoder'),\n        DeclareLaunchArgument(\n            'output_image_height',\n            default_value='480',\n            description='The expected output image height from custom encoder'),\n        DeclareLaunchArgument(\n            'input_image_width',\n            default_value='1920',\n            description='The expected output image width from custom encoder'),\n        DeclareLaunchArgument(\n            'input_image_height',\n            default_value='1080',\n            description='The expected output image height from custom encoder'),\n        DeclareLaunchArgument(\n            'image_mean',\n            default_value='[0.5, 0.5, 0.5]',\n            description='The mean for image normalization'),\n        DeclareLaunchArgument(\n            'image_stddev',\n            default_value='[0.5, 0.5, 0.5]',\n            description='The standard deviation for image normalization'),\n        DeclareLaunchArgument(\n            'model_file_path',\n            description='The absolute file path to the ONNX file'),\n        DeclareLaunchArgument(\n            'engine_file_path',\n            default_value='/tmp/trt_engine.plan',\n            description='The absolute file path to the TensorRT engine file'),\n        DeclareLaunchArgument(\n            'input_tensor_names',\n            default_value='[\"input_tensor\"]',\n            description='A list of tensor names to bound to the specified input binding names'),\n        DeclareLaunchArgument(\n            'input_binding_names',\n            default_value='[\"input\"]',\n            description='A list of input tensor binding names (specified by model)'),\n        DeclareLaunchArgument(\n            'input_tensor_formats',\n            default_value='[\"nitros_tensor_list_nchw_rgb_f32\"]',\n            description='The nitros format of the input tensors'),\n        DeclareLaunchArgument(\n            'output_tensor_names',\n            default_value='[\"output\"]',\n            description='A list of tensor names to bound to the specified output binding names'),\n        DeclareLaunchArgument(\n            'output_binding_names',\n            default_value='[\"output\"]',\n            description='A  list of output tensor binding names (specified by model)'),\n        DeclareLaunchArgument(\n            'output_tensor_formats',\n            default_value='[\"nitros_tensor_list_nhwc_rgb_f32\"]',\n            description='The nitros format of the output tensors'),\n        DeclareLaunchArgument(\n            'tensorrt_verbose',\n            default_value='False',\n            description='Whether TensorRT should verbosely log or not'),\n        DeclareLaunchArgument(\n            'object_name',\n            default_value='Ketchup',\n            description='The object class that the DOPE network is detecting'),\n        DeclareLaunchArgument(\n            'force_engine_update',\n            default_value='False',\n            description='Whether TensorRT should update the TensorRT engine file or not'),\n    ]\n\n    # Custom Image Encoder parameters\n    input_image_width = LaunchConfiguration('input_image_width')\n    input_image_height = LaunchConfiguration('input_image_height')\n    network_image_width = LaunchConfiguration('output_image_width')\n    network_image_height = LaunchConfiguration('output_image_height')\n    encoder_image_mean = LaunchConfiguration('image_mean')\n    encoder_image_stddev = LaunchConfiguration('image_stddev')\n\n    # Tensor RT parameters\n    model_file_path = LaunchConfiguration('model_file_path')\n    engine_file_path = LaunchConfiguration('engine_file_path')\n    input_tensor_names = LaunchConfiguration('input_tensor_names')\n    input_binding_names = LaunchConfiguration('input_binding_names')\n    input_tensor_formats = LaunchConfiguration('input_tensor_formats')\n    output_tensor_names = LaunchConfiguration('output_tensor_names')\n    output_binding_names = LaunchConfiguration('output_binding_names')\n    output_tensor_formats = LaunchConfiguration('output_tensor_formats')\n    tensorrt_verbose = LaunchConfiguration('tensorrt_verbose')\n    force_engine_update = LaunchConfiguration('force_engine_update')\n\n    # DOPE Decoder parameters\n    object_name = LaunchConfiguration('object_name')\n\n    encoder_node = ComposableNode(\n        name='encoder_node',\n        package='custom_nitros_dnn_image_encoder',\n        plugin='custom_nitros_dnn_image_encoder::ImageEncoderNode',\n        parameters=[{\n            'output_image_width': network_image_width,\n            'output_image_height': network_image_height,\n            'image_mean': encoder_image_mean,\n            'image_stddev': encoder_image_stddev,\n            'input_image_height': input_image_height,\n            'input_image_width': input_image_width\n        }],\n        remappings=[('/encoded_tensor', '/tensor_pub'),\n                    ('/image', '/image_rect')]\n    )\n\n    dope_inference_node = ComposableNode(\n        name='dope_inference',\n        package='isaac_ros_tensor_rt',\n        plugin='nvidia::isaac_ros::dnn_inference::TensorRTNode',\n        parameters=[{\n            'model_file_path': model_file_path,\n            'engine_file_path': engine_file_path,\n            'input_tensor_names': input_tensor_names,\n            'input_binding_names': input_binding_names,\n            'input_tensor_formats': input_tensor_formats,\n            'output_tensor_names': output_tensor_names,\n            'output_binding_names': output_binding_names,\n            'output_tensor_formats': output_tensor_formats,\n            'verbose': tensorrt_verbose,\n            'force_engine_update': force_engine_update\n        }])\n\n    dope_decoder_node = ComposableNode(\n        name='dope_decoder',\n        package='isaac_ros_dope',\n        plugin='nvidia::isaac_ros::dope::DopeDecoderNode',\n        parameters=[{\n            'object_name': object_name,\n        }],\n        remappings=[('belief_map_array', 'tensor_sub'),\n                    ('dope/detections', 'detections'),\n                    ('camera_info', '/dope_encoder/crop/info')])\n\n    container = ComposableNodeContainer(\n        name='dope_container',\n        namespace='',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[encoder_node, dope_inference_node, dope_decoder_node],\n        output='screen',\n    )\n\n    final_launch_description = launch_args + [container]\n    return launch.LaunchDescription(final_launch_description)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_dnn_image_encoder/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\n SPDX-FileCopyrightText: Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\n Licensed under the Apache License, Version 2.0 (the \"License\");\n you may not use this file except in compliance with the License.\n You may obtain a copy of the License at\n\n http://www.apache.org/licenses/LICENSE-2.0\n\n Unless required by applicable law or agreed to in writing, software\n distributed under the License is distributed on an \"AS IS\" BASIS,\n WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n See the License for the specific language governing permissions and\n limitations under the License.\n\n SPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>custom_nitros_dnn_image_encoder</name>\n  <version>4.4.0</version>\n  <description>Custom NITROS Image Encoding</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Shubham Tyagi</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>std_msgs</depend>\n  <depend>sensor_msgs</depend>\n  <depend>isaac_ros_managed_nitros</depend>\n  <depend>isaac_ros_nitros_tensor_list_type</depend>\n  <depend>isaac_ros_tensor_list_interfaces</depend>\n  <depend>cvcuda0-dev</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_dnn_image_encoder/src/image_encoder_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"custom_nitros_dnn_image_encoder/image_encoder_node.hpp\"\n\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_builder.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list_builder.hpp\"\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n\nnamespace custom_nitros_dnn_image_encoder\n{\nstd::unordered_map<std::string, NVCVImageFormat> g_str_to_nvcv_image_format({\n    {\"rgb8\", NVCVImageFormat{nvcv::FMT_RGB8, nvcv::FMT_RGBf32p, nvcv::FMT_RGBf32}},\n    {\"bgr8\", NVCVImageFormat{nvcv::FMT_BGR8, nvcv::FMT_BGRf32p, nvcv::FMT_BGRf32}},\n    {\"rgba8\", NVCVImageFormat{nvcv::FMT_RGBA8, nvcv::FMT_RGBAf32p, nvcv::FMT_RGBAf32}},\n    {\"bgra8\", NVCVImageFormat{nvcv::FMT_BGRA8, nvcv::FMT_BGRAf32p, nvcv::FMT_BGRAf32}}\n  });\n\nstd::map<std::pair<nvcv::ImageFormat, nvcv::ImageFormat>, NVCVColorConversionCode>\nget_color_conversion_code({\n    {std::make_pair(nvcv::FMT_RGB8, nvcv::FMT_BGR8), NVCV_COLOR_RGB2BGR},\n    {std::make_pair(nvcv::FMT_RGB8, nvcv::FMT_RGBA8), NVCV_COLOR_RGB2RGBA},\n    {std::make_pair(nvcv::FMT_RGB8, nvcv::FMT_BGRA8), NVCV_COLOR_RGB2BGRA},\n    {std::make_pair(nvcv::FMT_BGR8, nvcv::FMT_RGB8), NVCV_COLOR_BGR2RGB},\n    {std::make_pair(nvcv::FMT_BGR8, nvcv::FMT_RGBA8), NVCV_COLOR_BGR2RGBA},\n    {std::make_pair(nvcv::FMT_BGR8, nvcv::FMT_BGRA8), NVCV_COLOR_BGR2BGRA},\n    {std::make_pair(nvcv::FMT_RGBA8, nvcv::FMT_BGR8), NVCV_COLOR_RGBA2BGR},\n    {std::make_pair(nvcv::FMT_RGBA8, nvcv::FMT_RGB8), NVCV_COLOR_RGBA2RGB},\n    {std::make_pair(nvcv::FMT_RGBA8, nvcv::FMT_BGRA8), NVCV_COLOR_RGBA2BGRA},\n    {std::make_pair(nvcv::FMT_BGRA8, nvcv::FMT_RGB8), NVCV_COLOR_BGRA2RGB},\n    {std::make_pair(nvcv::FMT_BGRA8, nvcv::FMT_RGBA8), NVCV_COLOR_BGRA2RGBA},\n    {std::make_pair(nvcv::FMT_BGRA8, nvcv::FMT_BGR8), NVCV_COLOR_BGRA2BGR}\n  });\n\nImageEncoderNode::ImageEncoderNode(const rclcpp::NodeOptions options)\n: rclcpp::Node(\"image_encoder_node\", options),\n  sub_{create_subscription<sensor_msgs::msg::Image>(\n      \"image\", 10, std::bind(&ImageEncoderNode::InputCallback, this,\n      std::placeholders::_1))},\n\n  nitros_pub_{std::make_shared<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n        nvidia::isaac_ros::nitros::NitrosTensorList>>(\n      this,\n      \"encoded_tensor\",\n      nvidia::isaac_ros::nitros::nitros_tensor_list_nchw_rgb_f32_t::supported_type_name)},\n\n  tensor_name_{declare_parameter<std::string>(\"tensor_name\", \"input_tensor\")},\n  input_image_encoding_{declare_parameter<std::string>(\"input_image_encoding\", \"bgr8\")},\n  output_image_encoding_{declare_parameter<std::string>(\"output_image_encoding\", \"rgb8\")},\n  output_image_width_{declare_parameter<uint16_t>(\"output_image_width\", 640)},\n  output_image_height_{declare_parameter<uint16_t>(\"output_image_height\", 480)},\n  input_image_width_{declare_parameter<uint16_t>(\"input_image_width\", 1920)},\n  input_image_height_{declare_parameter<uint16_t>(\"input_image_height\", 1080)},\n  image_mean_(declare_parameter<std::vector<double>>(\"image_mean\", {0.0, 0.0, 0.0})),\n  image_stddev_(declare_parameter<std::vector<double>>(\"image_stddev\", {1.0, 1.0, 1.0}))\n{\n  CHECK_CUDA_ERROR(\n    ::nvidia::isaac_ros::common::initNamedCudaStream(\n      cuda_stream_, \"isaac_ros_dnn_image_encoder_node\"),\n    \"Error initializing CUDA stream\");\n  auto iterator = g_str_to_nvcv_image_format.find(input_image_encoding_);\n  if (iterator == g_str_to_nvcv_image_format.end()) {\n    throw std::runtime_error(\n            \"Error: received unsupported input image encoding: \" +\n            input_image_encoding_);\n  }\n  iterator = g_str_to_nvcv_image_format.find(output_image_encoding_);\n  if (iterator == g_str_to_nvcv_image_format.end()) {\n    throw std::runtime_error(\n            \"Error: received unsupported convert to encoding \" +\n            output_image_encoding_);\n  }\n\n  NVCVImageFormat input_image_format = g_str_to_nvcv_image_format[input_image_encoding_];\n  NVCVImageFormat output_image_format = g_str_to_nvcv_image_format[output_image_encoding_];\n\n  input_image_channels_ = input_image_format.interleaved_format.numChannels();\n  output_image_channels_ = output_image_format.planar_float_format.numChannels();\n\n  if (input_image_format.interleaved_format != output_image_format.interleaved_format) {\n    color_conversion_code_ = get_color_conversion_code[std::make_pair(\n          input_image_format.interleaved_format, output_image_format.interleaved_format\n        )];\n  }\n  // Initialize input image tensor. The format of input image is expected to be NHWC.\n  // Please note, the assignment of the values at each stride index depends upon the format\n  // of the image.\n  nvcv::Tensor::Requirements reqs = nvcv::Tensor::CalcRequirements(\n    batch_size_,\n    {input_image_width_, input_image_height_}, input_image_format.interleaved_format);\n\n  input_image_buffer_.strides[3] = sizeof(uint8_t);\n  input_image_buffer_.strides[2] = input_image_channels_ * input_image_buffer_.strides[3];\n  input_image_buffer_.strides[1] = input_image_width_ * input_image_buffer_.strides[2];\n  input_image_buffer_.strides[0] = input_image_height_ * input_image_buffer_.strides[1];\n  if (cudaMallocAsync(\n      &input_image_buffer_.basePtr,\n      batch_size_ * input_image_buffer_.strides[0],\n      cuda_stream_) != cudaSuccess)\n  {\n    throw std::runtime_error(\"Could not allocate input image tensor\");\n  }\n\n  nvcv::TensorDataStridedCuda in_data(nvcv::TensorShape{reqs.shape, reqs.rank, reqs.layout},\n    nvcv::DataType{reqs.dtype}, input_image_buffer_);\n  input_image_tensor_ = nvcv::TensorWrapData(in_data);\n\n  // Initialize output image tensor. The format of output is expected to be NCHW.\n  reqs = nvcv::Tensor::CalcRequirements(\n    batch_size_,\n    {output_image_width_, output_image_height_}, output_image_format.planar_float_format);\n\n  output_image_buffer_.strides[3] = sizeof(float);\n  output_image_buffer_.strides[2] = output_image_width_ * output_image_buffer_.strides[3];\n  output_image_buffer_.strides[1] = output_image_height_ * output_image_buffer_.strides[2];\n  output_image_buffer_.strides[0] = output_image_channels_ * output_image_buffer_.strides[1];\n  cudaMallocAsync(\n    &output_image_buffer_.basePtr, batch_size_ * output_image_buffer_.strides[0],\n    cuda_stream_);\n\n  nvcv::TensorDataStridedCuda out_data(nvcv::TensorShape{reqs.shape, reqs.rank, reqs.layout},\n    nvcv::DataType{reqs.dtype}, output_image_buffer_);\n  output_image_tensor_ = nvcv::TensorWrapData(out_data);\n\n  mean_tensor_ = nvcv::Tensor(1, {1, 1}, output_image_format.interleaved_float_format);\n  std_dev_tensor_ = nvcv::Tensor(1, {1, 1}, output_image_format.interleaved_float_format);\n  auto mean_data = mean_tensor_.exportData<nvcv::TensorDataStridedCuda>();\n  auto std_dev_data = std_dev_tensor_.exportData<nvcv::TensorDataStridedCuda>();\n\n  std::vector<float> image_mean_float(image_mean_.begin(), image_mean_.end());\n  std::vector<float> image_stddev_float(image_stddev_.begin(), image_stddev_.end());\n  if (\n    output_image_channels_ != image_mean_float.size() ||\n    output_image_channels_ != image_stddev_float.size())\n  {\n    throw std::runtime_error(\"Error: Incorrect length of image mean or image std dev vectors.\");\n  }\n  cudaMemcpyAsync(\n    mean_data->basePtr(), image_mean_float.data(),\n    output_image_channels_ * sizeof(float), cudaMemcpyHostToDevice,\n    cuda_stream_\n  );\n  cudaMemcpyAsync(\n    std_dev_data->basePtr(), image_stddev_float.data(),\n    output_image_channels_ * sizeof(float), cudaMemcpyHostToDevice,\n    cuda_stream_\n  );\n\n  resized_tensor_ = nvcv::Tensor(\n    batch_size_, {output_image_width_, output_image_height_},\n    input_image_format.interleaved_format);\n\n  color_converted_tensor_ = nvcv::Tensor(\n    batch_size_, {output_image_width_, output_image_height_},\n    output_image_format.interleaved_format);\n\n  float_tensor_ = nvcv::Tensor(\n    batch_size_, {output_image_width_, output_image_height_},\n    output_image_format.interleaved_float_format);\n\n  norm_tensor_ = nvcv::Tensor(\n    batch_size_, {output_image_width_, output_image_height_},\n    output_image_format.interleaved_float_format);\n\n  auto cuda_result = cudaStreamSynchronize(cuda_stream_);\n  if (cuda_result != cudaSuccess) {\n    throw std::runtime_error(\"Error: Unable to synchronize CUDA stream.\");\n  }\n}\n\nImageEncoderNode::~ImageEncoderNode() = default;\n\nvoid ImageEncoderNode::InputCallback(const sensor_msgs::msg::Image::SharedPtr msg)\n{\n  size_t buffer_size{msg->step * msg->height};\n  CHECK_CUDA_ERROR(\n    cudaMemcpyAsync(\n      input_image_buffer_.basePtr, msg->data.data(), buffer_size, cudaMemcpyDefault,\n      cuda_stream_),\n    \"Error copying data to input image buffer\");\n\n  // Resize\n  resize_op_(cuda_stream_, input_image_tensor_, resized_tensor_, NVCV_INTERP_LINEAR);\n\n  NVCVImageFormat input_image_format = g_str_to_nvcv_image_format[input_image_encoding_];\n  NVCVImageFormat output_image_format = g_str_to_nvcv_image_format[output_image_encoding_];\n  // Convert the color encoding only if the input and output image encodings are different.\n  if (input_image_format.interleaved_format != output_image_format.interleaved_format) {\n    // cvtColor operation converts the image from input_image_encoding to\n    // output_image_encoding\n    cvtColor_op_(\n      cuda_stream_, resized_tensor_,\n      color_converted_tensor_, color_conversion_code_\n    );\n\n    // ConvertTo operation converts the uint8 image to float32 image type.\n    // Also, it divides each pixel value by 255.0 i.e. scales the pixel values\n    // in range [0.0, 1.0]\n    convert_op_(\n      cuda_stream_, color_converted_tensor_,\n      float_tensor_, 1.0f / 255.f, 0.0f\n    );\n  } else {\n    convert_op_(cuda_stream_, resized_tensor_, float_tensor_, 1.0f / 255.f, 0.0f);\n  }\n\n  // Normalize\n  uint32_t flags = CVCUDA_NORMALIZE_SCALE_IS_STDDEV;\n  norm_op_(\n    cuda_stream_, float_tensor_, mean_tensor_, std_dev_tensor_,\n    norm_tensor_, 1.0f, 0.0f, 0.0f, flags\n  );\n\n  // Convert the data layout from interleaved to planar\n  reformat_op_(cuda_stream_, norm_tensor_, output_image_tensor_);\n\n  // Create the output buffer and copy the data\n  float * output_buffer;\n  size_t output_buffer_size{\n    output_image_width_ * output_image_height_ * output_image_channels_ * sizeof(float)};\n  cudaMallocAsync(&output_buffer, output_buffer_size, cuda_stream_);\n\n  cudaMemcpyAsync(\n    output_buffer, output_image_buffer_.basePtr, output_buffer_size, cudaMemcpyDefault,\n    cuda_stream_);\n\n  // Copy the header information.\n  std_msgs::msg::Header header;\n  header.stamp.sec = msg->header.stamp.sec;\n  header.stamp.nanosec = msg->header.stamp.nanosec;\n  header.frame_id = msg->header.frame_id;\n\n  auto cuda_result = cudaStreamSynchronize(cuda_stream_);\n  if (cuda_result != cudaSuccess) {\n    throw std::runtime_error(\"Error: Unable to synchronize CUDA stream.\");\n  }\n\n  nvidia::isaac_ros::nitros::NitrosTensorList tensor_list =\n    nvidia::isaac_ros::nitros::NitrosTensorListBuilder()\n    .WithHeader(header)\n    .AddTensor(\n    tensor_name_,\n    (\n      nvidia::isaac_ros::nitros::NitrosTensorBuilder()\n      .WithShape({batch_size_, output_image_channels_, output_image_height_, output_image_width_})\n      .WithDataType(nvidia::isaac_ros::nitros::NitrosDataType::kFloat32)\n      .WithData(output_buffer)\n      .Build()\n    )\n    )\n    .Build();\n\n  nitros_pub_->publish(tensor_list);\n}\n}  // namespace custom_nitros_dnn_image_encoder\n\n// Register as component\n#include \"rclcpp_components/register_node_macro.hpp\"\nRCLCPP_COMPONENTS_REGISTER_NODE(custom_nitros_dnn_image_encoder::ImageEncoderNode)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_dnn_image_encoder/test/custom_nitros_dnn_image_encoder_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_tensor_list_interfaces.msg import TensorList\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import Image\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    image_encoder_node = ComposableNode(\n        name='custom_image_encoder',\n        package='custom_nitros_dnn_image_encoder',\n        plugin='custom_nitros_dnn_image_encoder::ImageEncoderNode',\n        parameters=[{\n            'input_image_height': 460,\n            'input_image_width': 460\n        }],\n        namespace=CustomDNNImageEncoderPOLTest.generate_namespace(),\n       )\n\n    return CustomDNNImageEncoderPOLTest.generate_test_description([\n        ComposableNodeContainer(\n            name='custom_nitros_dnn_image_encoder_pol_test_container',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[image_encoder_node],\n            namespace=CustomDNNImageEncoderPOLTest.generate_namespace(),\n            output='screen',\n            arguments=['--ros-args', '--log-level', 'info']\n        )\n    ])\n\n\nclass CustomDNNImageEncoderPOLTest(IsaacROSBaseTest):\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case()\n    def test_message_passthrough(self, test_folder):\n        \"\"\"\n        Test DNN Image encoding.\n\n        Test that the a image can be encoded into a GPU tensor using Managed\n        Nitros and cvCUDA operations.\n        \"\"\"\n        TIMEOUT = 10\n        received_messages = {}\n\n        self.generate_namespace_lookup(['image', 'encoded_tensor'])\n\n        image_pub = self.node.create_publisher(\n            Image, self.namespaces['image'], self.DEFAULT_QOS)\n\n        subs = self.create_logging_subscribers([('encoded_tensor', TensorList)], received_messages)\n\n        try:\n            input_image = JSONConversion.load_image_from_json(test_folder / 'image.json')\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            end_time = time.time() + TIMEOUT\n            done = False\n\n            while time.time() < end_time:\n                rclpy.spin_once(self.node, timeout_sec=(0.1))\n                input_image.header.stamp.sec = 123456\n                input_image.header.stamp.nanosec = 789101112\n                input_image.header.frame_id = 'cuda_image'\n                image_pub.publish(input_image)\n\n                if 'encoded_tensor' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, 'Appropriate output not received')\n            output_tensor_list = received_messages['encoded_tensor']\n            self.assertEqual(\n                len(output_tensor_list.tensors), 1, 'Did not receive exactly 1 output tensor')\n            self.assertEqual(\n                output_tensor_list.tensors[0].shape.rank, 4, 'Rank mismatch.')\n            self.assertEqual(\n                output_tensor_list.tensors[0].shape.dims[0], 1, 'Batch size mismatch.')\n            self.assertEqual(\n                output_tensor_list.tensors[0].shape.dims[1], 3, 'No of channel mismatch.')\n            self.assertEqual(\n                output_tensor_list.tensors[0].shape.dims[2], 480, 'Tensor height mismatch.')\n            self.assertEqual(\n                output_tensor_list.tensors[0].shape.dims[3], 640, 'Tensor width mismatch.')\n\n        finally:\n            self.node.destroy_publisher(image_pub)\n            self.node.destroy_subscription(subs)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_dnn_image_encoder/test/test_cases/profile_image/image.json",
    "content": "{\n    \"image\": \"NVIDIAprofile.png\",\n    \"encoding\": \"rgb8\"\n}"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(custom_nitros_image LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# GPU Image Builder node\nament_auto_add_library(gpu_image_builder_node SHARED src/gpu_image_builder_node.cpp)\nrclcpp_components_register_nodes(gpu_image_builder_node \"custom_nitros_image::GpuImageBuilderNode\")\nset(node_plugins \"${node_plugins}custom_nitros_image::GpuImageBuilderNode;$<TARGET_FILE:gpu_image_builder_node>\\n\")\nset_target_properties(gpu_image_builder_node PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\n# GPU Image Viewer node\nament_auto_add_library(gpu_image_viewer_node SHARED src/gpu_image_viewer_node.cpp)\nrclcpp_components_register_nodes(gpu_image_viewer_node \"custom_nitros_image::GpuImageViewerNode\")\nset(node_plugins \"${node_plugins}custom_nitros_image::GpuImageViewerNode;$<TARGET_FILE:gpu_image_viewer_node>\\n\")\nset_target_properties(gpu_image_viewer_node PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\n# GPU Image Viewer node\nament_auto_add_library(nitros_image_switch_node SHARED src/nitros_image_switch_node.cpp)\nrclcpp_components_register_nodes(nitros_image_switch_node \"nvidia::isaac_ros::custom_nitros_image::NitrosImageSwitchNode\")\nset(node_plugins \"${node_plugins}nvidia::isaac_ros::custom_nitros_image::NitrosImageSwitchNode;$<TARGET_FILE:nitros_image_switch_node>\\n\")\nset_target_properties(nitros_image_switch_node PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  find_package(ament_lint_auto REQUIRED)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/custom_nitros_image_pol.py TIMEOUT \"600\")\n  add_launch_test(test/custom_nitros_image_builder_pol.py TIMEOUT \"600\")\n  add_launch_test(test/custom_nitros_image_viewer_pol.py TIMEOUT \"600\")\n  add_launch_test(test/test_nitros_image_switch_pol.py TIMEOUT \"600\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/include/custom_nitros_image/gpu_image_builder_node.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef CUSTOM_NITROS_IMAGE__GPU_IMAGE_BUILDER_NODE_HPP_\n#define CUSTOM_NITROS_IMAGE__GPU_IMAGE_BUILDER_NODE_HPP_\n\n#include <memory>\n\n#include \"rclcpp/rclcpp.hpp\"\n\n#include \"isaac_ros_managed_nitros/managed_nitros_publisher.hpp\"\n\n#include \"sensor_msgs/msg/image.hpp\"\n#include \"isaac_ros_nitros_image_type/nitros_image.hpp\"\n\n#include \"cuda_runtime.h\"  // NOLINT - include .h without directory\n\nnamespace custom_nitros_image\n{\n\nclass GpuImageBuilderNode : public rclcpp::Node\n{\npublic:\n  explicit GpuImageBuilderNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions());\n\n  ~GpuImageBuilderNode();\n\nprivate:\n  void InputCallback(const sensor_msgs::msg::Image::SharedPtr msg);\n\n  // Subscription to input Image messages\n  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;\n\n  // Publisher for output NitrosImage messages\n  std::shared_ptr<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n      nvidia::isaac_ros::nitros::NitrosImage>> nitros_pub_;\n\n  // CUDA stream to process dynamics detection on\n  cudaStream_t cuda_stream_;\n};\n\n}  // namespace custom_nitros_image\n\n#endif  // CUSTOM_NITROS_IMAGE__GPU_IMAGE_BUILDER_NODE_HPP_\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/include/custom_nitros_image/gpu_image_viewer_node.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef CUSTOM_NITROS_IMAGE__GPU_IMAGE_VIEWER_NODE_HPP_\n#define CUSTOM_NITROS_IMAGE__GPU_IMAGE_VIEWER_NODE_HPP_\n\n#include <memory>\n\n#include \"rclcpp/rclcpp.hpp\"\n\n#include \"isaac_ros_managed_nitros/managed_nitros_subscriber.hpp\"\n#include \"sensor_msgs/msg/image.hpp\"\n#include \"isaac_ros_nitros_image_type/nitros_image_view.hpp\"\n\n#include \"cuda_runtime.h\"  // NOLINT - include .h without directory\n\nnamespace custom_nitros_image\n{\n\nclass GpuImageViewerNode : public rclcpp::Node\n{\npublic:\n  explicit GpuImageViewerNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions());\n\n  ~GpuImageViewerNode();\n\nprivate:\n  void InputCallback(const nvidia::isaac_ros::nitros::NitrosImageView & msg);\n\n  // Subscription to input NitrosImage messages\n  std::shared_ptr<nvidia::isaac_ros::nitros::ManagedNitrosSubscriber<\n      nvidia::isaac_ros::nitros::NitrosImageView>> nitros_sub_;\n\n  // Publisher for output Image messages\n  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_;\n\n  // CUDA stream to process dynamics detection on\n  cudaStream_t cuda_stream_;\n};\n\n}  // namespace custom_nitros_image\n\n#endif  // CUSTOM_NITROS_IMAGE__GPU_IMAGE_VIEWER_NODE_HPP_\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/include/custom_nitros_image/nitros_image_switch_node.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef CUSTOM_NITROS_IMAGE__NITROS_IMAGE_SWITCH_NODE_HPP_\n#define CUSTOM_NITROS_IMAGE__NITROS_IMAGE_SWITCH_NODE_HPP_\n\n#include <memory>\n\n#include \"isaac_ros_common/qos.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_message_filters_subscriber.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_publisher.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_subscriber.hpp\"\n#include \"isaac_ros_nitros_image_type/nitros_image_view.hpp\"\n#include \"message_filters/subscriber.h\"\n#include \"message_filters/synchronizer.h\"\n#include \"message_filters/sync_policies/exact_time.h\"\n#include \"rclcpp/rclcpp.hpp\"\n#include \"sensor_msgs/msg/camera_info.hpp\"\n#include \"std_srvs/srv/set_bool.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace custom_nitros_image\n{\n\nclass NitrosImageSwitchNode : public rclcpp::Node\n{\npublic:\n  explicit NitrosImageSwitchNode(\n    const rclcpp::NodeOptions options = rclcpp::NodeOptions());\n\n  ~NitrosImageSwitchNode();\n\nprivate:\n  void InputCallback(\n    const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & nitros_image,\n    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info);\n\n  void TriggerSwitchService(\n    const std::shared_ptr<std_srvs::srv::SetBool::Request> request,\n    std::shared_ptr<std_srvs::srv::SetBool::Response> response);\n\n  uint16_t sync_queue_size_;\n  rclcpp::QoS output_qos_;\n\n  nvidia::isaac_ros::nitros::message_filters::Subscriber<nvidia::isaac_ros::nitros::NitrosImageView>\n  sub_image_;\n  message_filters::Subscriber<sensor_msgs::msg::CameraInfo> sub_camera_info_;\n\n  using ExactPolicy = message_filters::sync_policies::ExactTime<\n    nvidia::isaac_ros::nitros::NitrosImage,\n    sensor_msgs::msg::CameraInfo\n  >;\n  message_filters::Synchronizer<ExactPolicy> sync_;\n\n  std::shared_ptr<::nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n      ::nvidia::isaac_ros::nitros::NitrosImage>>\n  pub_image_;\n  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr pub_camera_info_;\n\n  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr empty_service_;\n\n  bool publish_{};\n};\n\n}  // namespace custom_nitros_image\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // CUSTOM_NITROS_IMAGE__NITROS_IMAGE_SWITCH_NODE_HPP_\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\n SPDX-FileCopyrightText: Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\n Licensed under the Apache License, Version 2.0 (the \"License\");\n you may not use this file except in compliance with the License.\n You may obtain a copy of the License at\n\n http://www.apache.org/licenses/LICENSE-2.0\n\n Unless required by applicable law or agreed to in writing, software\n distributed under the License is distributed on an \"AS IS\" BASIS,\n WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n See the License for the specific language governing permissions and\n limitations under the License.\n\n SPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>custom_nitros_image</name>\n  <version>4.4.0</version>\n  <description>Custom NITROS Image encoding and decoding</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Swapnesh Wani</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>std_msgs</depend>\n  <depend>message_filters</depend>\n  <depend>isaac_ros_managed_nitros</depend>\n  <depend>sensor_msgs</depend>\n  <depend>isaac_ros_nitros_image_type</depend>\n  <depend>std_srvs</depend>\n  <depend>isaac_ros_common</depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/src/gpu_image_builder_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"custom_nitros_image/gpu_image_builder_node.hpp\"\n\n#include <cuda_runtime.h>\n\n#include \"isaac_ros_nitros_image_type/nitros_image_builder.hpp\"\n#include \"sensor_msgs/image_encodings.hpp\"\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n\nnamespace custom_nitros_image\n{\n\nGpuImageBuilderNode::GpuImageBuilderNode(const rclcpp::NodeOptions options)\n: rclcpp::Node(\"image_builder_node\", options),\n  sub_{create_subscription<sensor_msgs::msg::Image>(\n      \"image_input\", 10,\n      std::bind(&GpuImageBuilderNode::InputCallback, this, std::placeholders::_1))},\n  nitros_pub_{std::make_shared<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n        nvidia::isaac_ros::nitros::NitrosImage>>(\n      this, \"gpu_image\",\n      nvidia::isaac_ros::nitros::nitros_image_rgb8_t::supported_type_name)}\n{\n  CHECK_CUDA_ERROR(\n    ::nvidia::isaac_ros::common::initNamedCudaStream(\n      cuda_stream_, \"isaac_ros_gpu_image_builder_node\"),\n    \"Error initializing CUDA stream\");\n}\n\nGpuImageBuilderNode::~GpuImageBuilderNode() = default;\n\nvoid GpuImageBuilderNode::InputCallback(const sensor_msgs::msg::Image::SharedPtr msg)\n{\n  // Get size of image\n  size_t buffer_size{msg->step * msg->height};\n\n  // Allocate CUDA buffer to store image\n  void * buffer;\n  CHECK_CUDA_ERROR(\n    cudaMallocAsync(&buffer, buffer_size, cuda_stream_),\n    \"Error allocating CUDA buffer\");\n\n  // Copy data bytes to CUDA buffer\n  CHECK_CUDA_ERROR(\n    cudaMemcpyAsync(\n      buffer, msg->data.data(), buffer_size, cudaMemcpyDefault,\n      cuda_stream_),\n    \"Error copying data to CUDA buffer\");\n\n  // Adding header data\n  std_msgs::msg::Header header;\n  header.stamp.sec = 123456;\n  header.stamp.nanosec = 789101112;\n  header.frame_id = \"cuda_image\";\n\n  CHECK_CUDA_ERROR(cudaStreamSynchronize(cuda_stream_), \"Error synchronizing CUDA stream\");\n\n  // Create NitrosImage wrapping CUDA buffer\n  nvidia::isaac_ros::nitros::NitrosImage nitros_image =\n    nvidia::isaac_ros::nitros::NitrosImageBuilder()\n    .WithHeader(header)\n    .WithEncoding(img_encodings::RGB8)\n    .WithDimensions(msg->height, msg->width)\n    .WithGpuData(buffer)\n    .Build();\n\n  nitros_pub_->publish(nitros_image);\n  RCLCPP_INFO(this->get_logger(), \"Sent CUDA buffer with memory at: %p\", buffer);\n}\n\n}  // namespace custom_nitros_image\n\n// Register as component\n#include \"rclcpp_components/register_node_macro.hpp\"\nRCLCPP_COMPONENTS_REGISTER_NODE(custom_nitros_image::GpuImageBuilderNode)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/src/gpu_image_viewer_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"custom_nitros_image/gpu_image_viewer_node.hpp\"\n\n#include <cuda_runtime.h>\n\n#include \"isaac_ros_nitros_image_type/nitros_image_view.hpp\"\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n\nnamespace custom_nitros_image\n{\n\nGpuImageViewerNode::GpuImageViewerNode(const rclcpp::NodeOptions options)\n: rclcpp::Node(\"image_viewer_node\", options),\n  nitros_sub_{std::make_shared<nvidia::isaac_ros::nitros::ManagedNitrosSubscriber<\n        nvidia::isaac_ros::nitros::NitrosImageView>>(\n      this, \"gpu_image\", nvidia::isaac_ros::nitros::nitros_image_rgb8_t::supported_type_name,\n      std::bind(&GpuImageViewerNode::InputCallback, this,\n      std::placeholders::_1))},\n  pub_{create_publisher<sensor_msgs::msg::Image>(\"image_output\", 10)}\n{\n  CHECK_CUDA_ERROR(\n    ::nvidia::isaac_ros::common::initNamedCudaStream(\n      cuda_stream_, \"isaac_ros_gpu_image_viewer_node\"),\n    \"Error initializing CUDA stream\");\n}\n\nGpuImageViewerNode::~GpuImageViewerNode() = default;\n\nvoid GpuImageViewerNode::InputCallback(const nvidia::isaac_ros::nitros::NitrosImageView & view)\n{\n  RCLCPP_INFO(this->get_logger(), \"Receiving CUDA buffer with memory at: %p\", view.GetGpuData());\n  RCLCPP_INFO(this->get_logger(), \"FrameId: %s\", view.GetFrameId().c_str());\n  RCLCPP_INFO(this->get_logger(), \"Timestamp seconds: %d\", view.GetTimestampSeconds());\n  RCLCPP_INFO(this->get_logger(), \"Timestamp nanoseconds: %u\", view.GetTimestampNanoseconds());\n  RCLCPP_INFO(this->get_logger(), \"Size: %lu\", view.GetSizeInBytes());\n\n  sensor_msgs::msg::Image img_msg;\n  img_msg.header.frame_id = view.GetFrameId();\n  img_msg.header.stamp.sec = view.GetTimestampSeconds();\n  img_msg.header.stamp.nanosec = view.GetTimestampNanoseconds();\n  img_msg.height = view.GetHeight();\n  img_msg.width = view.GetWidth();\n  img_msg.encoding = view.GetEncoding();\n  img_msg.step = view.GetSizeInBytes() / view.GetHeight();\n\n  img_msg.data.resize(view.GetSizeInBytes());\n  CHECK_CUDA_ERROR(\n    cudaMemcpyAsync(\n      img_msg.data.data(), view.GetGpuData(), view.GetSizeInBytes(),\n      cudaMemcpyDefault, cuda_stream_),\n    \"Error copying data to ROS message\");\n  CHECK_CUDA_ERROR(cudaStreamSynchronize(cuda_stream_), \"Error synchronizing CUDA stream\");\n  pub_->publish(std::move(img_msg));\n}\n\n}  // namespace custom_nitros_image\n\n// Register as component\n#include \"rclcpp_components/register_node_macro.hpp\"\nRCLCPP_COMPONENTS_REGISTER_NODE(custom_nitros_image::GpuImageViewerNode)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/src/nitros_image_switch_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"custom_nitros_image/nitros_image_switch_node.hpp\"\n\n#include \"isaac_ros_nitros_image_type/nitros_image_builder.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace custom_nitros_image\n{\n\nNitrosImageSwitchNode::NitrosImageSwitchNode(const rclcpp::NodeOptions options)\n: rclcpp::Node(\"nitros_image_switch\", options),\n  sync_queue_size_{declare_parameter<uint16_t>(\"sync_queue_size\", static_cast<uint16_t>(10))},\n  output_qos_{\n    ::isaac_ros::common::AddQosParameter(*this, \"DEFAULT\", \"output_qos\")},\n  sub_image_{},\n  sub_camera_info_{},\n  sync_{ExactPolicy{100}, sub_image_, sub_camera_info_},\n  pub_image_{std::make_shared<\n      nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n        nvidia::isaac_ros::nitros::NitrosImage>>(\n      this, \"switched_image\",\n      ::nvidia::isaac_ros::nitros::nitros_image_rgb8_t::supported_type_name,\n      nvidia::isaac_ros::nitros::NitrosDiagnosticsConfig{}, output_qos_)},\n  pub_camera_info_{create_publisher<sensor_msgs::msg::CameraInfo>(\n      \"switched_camera_info\",\n      output_qos_)},\n  empty_service_{create_service<std_srvs::srv::SetBool>(\n      \"switch\",\n      std::bind(&NitrosImageSwitchNode::TriggerSwitchService, this,\n      std::placeholders::_1, std::placeholders::_2))},\n  publish_{declare_parameter<bool>(\"initial_switch_state\", true)}\n{\n  sync_.registerCallback(\n    std::bind(\n      &NitrosImageSwitchNode::InputCallback, this,\n      std::placeholders::_1, std::placeholders::_2));\n\n  sub_image_.subscribe(this, \"image\");\n  sub_camera_info_.subscribe(this, \"camera_info\");\n}\n\nvoid NitrosImageSwitchNode::InputCallback(\n  const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & nitros_image,\n  const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info)\n{\n  if (publish_) {\n    pub_image_->publish(*nitros_image);\n    pub_camera_info_->publish(*camera_info);\n  }\n}\n\nvoid NitrosImageSwitchNode::TriggerSwitchService(\n  const std::shared_ptr<std_srvs::srv::SetBool::Request> request,\n  std::shared_ptr<std_srvs::srv::SetBool::Response> response)\n{\n  publish_ = request->data;\n  response->success = true;\n  response->message = \"\";\n  RCLCPP_DEBUG(\n    get_logger(),\n    \"Triggered service! Passthrough switch is set to: %s\",\n    publish_ ? \"ON\" : \"OFF\");\n}\n\nNitrosImageSwitchNode::~NitrosImageSwitchNode() = default;\n\n}  // namespace custom_nitros_image\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#include \"rclcpp_components/register_node_macro.hpp\"\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::custom_nitros_image::NitrosImageSwitchNode)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/test/custom_nitros_image_builder_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import Image\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    image_builder_node = ComposableNode(\n        name='image_builder',\n        package='custom_nitros_image',\n        plugin='custom_nitros_image::GpuImageBuilderNode',\n        namespace=CustomNitrosImagePOLTest.generate_namespace(),\n       )\n\n    return CustomNitrosImagePOLTest.generate_test_description([\n        ComposableNodeContainer(\n            name='custom_nitros_image_pol_test_container',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[image_builder_node],\n            namespace=CustomNitrosImagePOLTest.generate_namespace(),\n            output='screen',\n            arguments=['--ros-args', '--log-level', 'info']\n        )\n    ])\n\n\nclass CustomNitrosImagePOLTest(IsaacROSBaseTest):\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case()\n    def test_message_passthrough(self, test_folder):\n        \"\"\"\n        Test image building.\n\n        Test that the custom NITROS image built with GPU based buffer from the input CPU image and\n        view (access) the GPU image and make a CPU copy and send the reconstructed image.\n        \"\"\"\n        TIMEOUT = 10\n        received_messages = {}\n\n        self.generate_namespace_lookup(['image_input', 'gpu_image'])\n\n        image_pub = self.node.create_publisher(\n            Image, self.namespaces['image_input'], self.DEFAULT_QOS)\n\n        subs = self.create_logging_subscribers([('gpu_image', Image)], received_messages)\n\n        try:\n            input_image = JSONConversion.load_image_from_json(test_folder / 'image.json')\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            end_time = time.time() + TIMEOUT\n            done = False\n\n            while time.time() < end_time:\n                rclpy.spin_once(self.node, timeout_sec=(0.1))\n                input_image.header.stamp.sec = 123456\n                input_image.header.stamp.nanosec = 789101112\n                input_image.header.frame_id = 'cuda_image'\n                image_pub.publish(input_image)\n\n                if 'gpu_image' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, 'Appropriate output not received')\n\n            output_image = received_messages['gpu_image']\n\n            self.assertEqual(input_image, output_image, 'GPU image did not match input!')\n\n        finally:\n            self.node.destroy_publisher(image_pub)\n            self.node.destroy_subscription(subs)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/test/custom_nitros_image_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import Image\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    image_builder_node = ComposableNode(\n        name='image_builder',\n        package='custom_nitros_image',\n        plugin='custom_nitros_image::GpuImageBuilderNode',\n        namespace=CustomNitrosImagePOLTest.generate_namespace(),\n       )\n\n    image_viewer_node = ComposableNode(\n        name='image_viewer',\n        package='custom_nitros_image',\n        plugin='custom_nitros_image::GpuImageViewerNode',\n        namespace=CustomNitrosImagePOLTest.generate_namespace(),\n        )\n\n    return CustomNitrosImagePOLTest.generate_test_description([\n        ComposableNodeContainer(\n            name='custom_nitros_image_pol_test_container',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[image_viewer_node, image_builder_node],\n            namespace=CustomNitrosImagePOLTest.generate_namespace(),\n            output='screen',\n            arguments=['--ros-args', '--log-level', 'info']\n        )\n    ])\n\n\nclass CustomNitrosImagePOLTest(IsaacROSBaseTest):\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case()\n    def test_message_passthrough(self, test_folder):\n        \"\"\"\n        Test image building and viewing.\n\n        Test that the custom NITROS image built with GPU based buffer from the input CPU image and\n        view (access) the GPU image and make a CPU copy and send the reconstructed image.\n        \"\"\"\n        TIMEOUT = 10\n        received_messages = {}\n\n        self.generate_namespace_lookup(['image_input', 'image_output'])\n\n        image_pub = self.node.create_publisher(\n            Image, self.namespaces['image_input'], self.DEFAULT_QOS)\n\n        subs = self.create_logging_subscribers([('image_output', Image)], received_messages)\n\n        try:\n            input_image = JSONConversion.load_image_from_json(test_folder / 'image.json')\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            end_time = time.time() + TIMEOUT\n            done = False\n\n            while time.time() < end_time:\n                rclpy.spin_once(self.node, timeout_sec=(0.1))\n                input_image.header.stamp.sec = 123456\n                input_image.header.stamp.nanosec = 789101112\n                input_image.header.frame_id = 'cuda_image'\n                image_pub.publish(input_image)\n\n                image_pub.publish(input_image)\n\n                if 'image_output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, 'Appropriate output not received')\n\n            output_image = received_messages['image_output']\n\n            self.assertEqual(input_image, output_image, 'Output image did not match input!')\n\n        finally:\n            self.node.destroy_publisher(image_pub)\n            self.node.destroy_subscription(subs)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/test/custom_nitros_image_viewer_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import Image\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    image_viewer_node = ComposableNode(\n        name='image_viewer',\n        package='custom_nitros_image',\n        plugin='custom_nitros_image::GpuImageViewerNode',\n        namespace=CustomNitrosImagePOLTest.generate_namespace(),\n        )\n\n    return CustomNitrosImagePOLTest.generate_test_description([\n        ComposableNodeContainer(\n            name='custom_nitros_image_pol_test_container',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[image_viewer_node],\n            namespace=CustomNitrosImagePOLTest.generate_namespace(),\n            output='screen',\n            arguments=['--ros-args', '--log-level', 'info']\n        )\n    ])\n\n\nclass CustomNitrosImagePOLTest(IsaacROSBaseTest):\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case()\n    def test_message_passthrough(self, test_folder):\n        \"\"\"\n        Test image viewing.\n\n        Test that the custom NITROS image built with GPU based buffer from the input CPU image and\n        view (access) the GPU image and make a CPU copy and send the reconstructed image.\n        \"\"\"\n        TIMEOUT = 10\n        received_messages = {}\n\n        self.generate_namespace_lookup(['gpu_image', 'image_output'])\n\n        image_pub = self.node.create_publisher(\n            Image, self.namespaces['gpu_image'], self.DEFAULT_QOS)\n\n        subs = self.create_logging_subscribers([('image_output', Image)], received_messages)\n\n        try:\n            input_image = JSONConversion.load_image_from_json(test_folder / 'image.json')\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            end_time = time.time() + TIMEOUT\n            done = False\n\n            while time.time() < end_time:\n                rclpy.spin_once(self.node, timeout_sec=(0.1))\n                input_image.header.stamp.sec = 123456\n                input_image.header.stamp.nanosec = 789101112\n                input_image.header.frame_id = 'cuda_image'\n\n                image_pub.publish(input_image)\n\n                if 'image_output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, 'Appropriate output not received')\n\n            output_image = received_messages['image_output']\n\n            self.assertEqual(input_image, output_image, 'Output image did not match input!')\n\n        finally:\n            self.node.destroy_publisher(image_pub)\n            self.node.destroy_subscription(subs)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/test/test_cases/profile_image/image.json",
    "content": "{\n    \"image\": \"NVIDIAprofile.png\",\n    \"encoding\": \"rgb8\"\n}"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_image/test/test_nitros_image_switch_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport os\nimport pathlib\nimport time\n\nfrom cv_bridge import CvBridge\nfrom isaac_ros_test import IsaacROSBaseTest\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport numpy as np\n\nimport pytest\nimport rclpy\n\nfrom sensor_msgs.msg import CameraInfo, Image\nfrom std_srvs.srv import SetBool\n\n\nDIMENSION_WIDTH = 100\nDIMENSION_HEIGHT = 120\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    nitros_image_switch_node = ComposableNode(\n        name='nitros_image_switch_node',\n        package='custom_nitros_image',\n        plugin='nvidia::isaac_ros::custom_nitros_image::NitrosImageSwitchNode',\n        namespace=NitrosImageSwitchNodeTest.generate_namespace(),\n        parameters=[{'sync_queue_size': 100}]\n    )\n\n    return NitrosImageSwitchNodeTest.generate_test_description([\n        ComposableNodeContainer(\n            name='container',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[nitros_image_switch_node],\n            namespace=NitrosImageSwitchNodeTest.generate_namespace(),\n            output='screen',\n        )\n    ])\n\n\nclass NitrosImageSwitchNodeTest(IsaacROSBaseTest):\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    def test_nitros_image_switch(self):\n        \"\"\"Test that switch ON passes messages through and switch OFF blocks them.\"\"\"\n        received_messages = {}\n\n        self.generate_namespace_lookup(\n            ['image', 'camera_info', 'switched_image', 'switched_camera_info']\n        )\n\n        image_pub = self.node.create_publisher(\n            Image, self.namespaces['image'], self.DEFAULT_QOS)\n        camera_info_pub = self.node.create_publisher(\n            CameraInfo, self.namespaces['camera_info'], self.DEFAULT_QOS)\n\n        subs = self.create_logging_subscribers(\n            [('switched_image', Image), ('switched_camera_info', CameraInfo)],\n            received_messages, accept_multiple_messages=True)\n\n        switch_client = self.node.create_client(SetBool, '/isaac_ros_test/switch')\n        while not switch_client.wait_for_service(timeout_sec=1.0):\n            self.node.get_logger().info('Waiting for switch service...')\n\n        # Create test image\n        cv_image = np.zeros((DIMENSION_HEIGHT, DIMENSION_WIDTH, 3), np.uint8)\n        cv_image[:] = (1, 2, 3)\n        image = CvBridge().cv2_to_imgmsg(cv_image)\n        image.encoding = 'bgr8'\n        camera_info = CameraInfo()\n\n        def call_switch(state: bool):\n            future = switch_client.call_async(SetBool.Request(data=state))\n            rclpy.spin_until_future_complete(self.node, future)\n            self.assertTrue(future.result().success)\n\n        def publish_message(frame_id: str):\n            image.header.stamp = self.node.get_clock().now().to_msg()\n            image.header.frame_id = frame_id\n            camera_info.header = image.header\n            image_pub.publish(image)\n            camera_info_pub.publish(camera_info)\n\n        def spin_and_wait(duration: float = 0.5):\n            end_time = time.time() + duration\n            while time.time() < end_time:\n                rclpy.spin_once(self.node, timeout_sec=0.05)\n\n        try:\n            # Switch ON: publish messages that should pass through\n            call_switch(True)\n            for i in range(5):\n                publish_message(f'on_{i}')\n            spin_and_wait()\n\n            on_count = len(received_messages.get('switched_image', []))\n            self.assertGreaterEqual(on_count, 5, f'Switch ON: expected 5 messages, got {on_count}')\n\n            # Switch OFF: publish messages that should be blocked\n            call_switch(False)\n            for i in range(5):\n                publish_message(f'off_{i}')\n            spin_and_wait()\n\n            off_count = len(received_messages['switched_image'])\n            extra = off_count - on_count\n            self.assertEqual(off_count, on_count,\n                             f'Switch OFF: messages should be blocked, got {extra} extra')\n\n            # Verify all received messages are from the ON state\n            for received_image in received_messages['switched_image']:\n                self.assertEqual(received_image.height, DIMENSION_HEIGHT)\n                self.assertEqual(received_image.width, DIMENSION_WIDTH)\n                self.assertEqual(received_image.encoding, 'bgr8')\n                self.assertTrue(received_image.header.frame_id.startswith('on_'),\n                                f'Unexpected frame_id: {received_image.header.frame_id}')\n                self.assertTrue((received_image.data == cv_image.flatten()).all())\n\n            self.assertEqual(len(received_messages['switched_camera_info']), off_count)\n\n        finally:\n            self.node.destroy_subscription(subs)\n            self.node.destroy_publisher(image_pub)\n            self.node.destroy_publisher(camera_info_pub)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_message_filter/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(custom_nitros_message_filter LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# CameraInfo + Image synchronizer node\nament_auto_add_library(synchronizer_node SHARED src/synchronizer_node.cpp)\nrclcpp_components_register_nodes(synchronizer_node \"custom_nitros_message_filter::SynchronizerNode\")\nset(node_plugins \"${node_plugins}custom_nitros_message_filter::SynchronizerNode;$<TARGET_FILE:synchronizer_node>\\n\")\nset_target_properties(synchronizer_node PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  find_package(ament_lint_auto REQUIRED)\n  ament_lint_auto_find_test_dependencies()\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/custom_nitros_message_filter_pol.py TIMEOUT \"600\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_message_filter/include/custom_nitros_message_filter/synchronizer_node.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef CUSTOM_NITROS_MESSAGE_FILTER__SYNCHRONIZER_NODE_HPP_\n#define CUSTOM_NITROS_MESSAGE_FILTER__SYNCHRONIZER_NODE_HPP_\n\n#include <memory>\n\n#include \"rclcpp/rclcpp.hpp\"\n#include \"message_filters/subscriber.h\"\n#include \"message_filters/synchronizer.h\"\n#include \"message_filters/sync_policies/exact_time.h\"\n\n#include \"custom_nitros_message_filter_interfaces/msg/sync_status.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_message_filters_subscriber.hpp\"\n#include \"isaac_ros_nitros_image_type/nitros_image_view.hpp\"\n#include \"sensor_msgs/msg/camera_info.hpp\"\n\n\nnamespace custom_nitros_message_filter\n{\n\nclass SynchronizerNode : public rclcpp::Node\n{\npublic:\n  explicit SynchronizerNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions());\n\n  ~SynchronizerNode();\n\nprivate:\n  void InputCallback(\n    bool synchronized_inputs,\n    const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & nitros_image,\n    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info\n  );\n\n  // Synchronized NITROS Image and standard CameraInfo subscribers\n  nvidia::isaac_ros::nitros::message_filters::Subscriber<nvidia::isaac_ros::nitros::NitrosImageView>\n  sub_image_;\n  message_filters::Subscriber<sensor_msgs::msg::CameraInfo> sub_camera_info_;\n\n  using ExactPolicy = message_filters::sync_policies::ExactTime<\n    nvidia::isaac_ros::nitros::NitrosImage,\n    sensor_msgs::msg::CameraInfo\n  >;\n  message_filters::Synchronizer<ExactPolicy> sync_;\n\n  // Publisher for output status messages\n  rclcpp::Publisher<custom_nitros_message_filter_interfaces::msg::SyncStatus>::SharedPtr pub_;\n};\n\n}  // namespace custom_nitros_message_filter\n\n#endif  // CUSTOM_NITROS_MESSAGE_FILTER__SYNCHRONIZER_NODE_HPP_\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_message_filter/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\n SPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\n Licensed under the Apache License, Version 2.0 (the \"License\");\n you may not use this file except in compliance with the License.\n You may obtain a copy of the License at\n\n http://www.apache.org/licenses/LICENSE-2.0\n\n Unless required by applicable law or agreed to in writing, software\n distributed under the License is distributed on an \"AS IS\" BASIS,\n WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n See the License for the specific language governing permissions and\n limitations under the License.\n\n SPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>custom_nitros_message_filter</name>\n  <version>4.4.0</version>\n  <description>Synchronization of Camera Info and Custom NITROS Image using ROS 2 message_filters</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Jaiveer Singh</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>sensor_msgs</depend>\n  <depend>message_filters</depend>\n  <depend>custom_nitros_message_filter_interfaces</depend>\n  <depend>isaac_ros_managed_nitros</depend>\n  <depend>isaac_ros_nitros_image_type</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_message_filter/src/synchronizer_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"custom_nitros_message_filter/synchronizer_node.hpp\"\n\n#include <cuda_runtime.h>\n\nnamespace custom_nitros_message_filter\n{\n\nSynchronizerNode::SynchronizerNode(const rclcpp::NodeOptions options)\n: rclcpp::Node(\"synchronizer_node\", options),\n  sub_image_{},\n  sub_camera_info_{},\n  sync_{ExactPolicy{10}, sub_image_, sub_camera_info_},\n  pub_{create_publisher<custom_nitros_message_filter_interfaces::msg::SyncStatus>(\n      \"status\", 3)}\n{\n  sync_.registerCallback(\n    std::bind(\n      &SynchronizerNode::InputCallback, this, true,\n      std::placeholders::_1, std::placeholders::_2));\n  sync_.getPolicy()->registerDropCallback(\n    std::bind(\n      &SynchronizerNode::InputCallback, this, false,\n      std::placeholders::_1, std::placeholders::_2));\n\n  sub_image_.subscribe(this, \"image\");\n  sub_camera_info_.subscribe(this, \"camera_info\");\n}\n\nSynchronizerNode::~SynchronizerNode() = default;\n\nvoid SynchronizerNode::InputCallback(\n  bool synchronized_inputs,\n  const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & nitros_image,\n  const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info\n)\n{\n  RCLCPP_DEBUG(\n    this->get_logger(), \"%s callback fired!\",\n    (synchronized_inputs ? \"Synchronized\" : \"UNSYNCHRONIZED\"));\n\n  custom_nitros_message_filter_interfaces::msg::SyncStatus status{};\n  status.messages_present = std::vector<bool>(2, false);\n  status.exact_time_match = false;\n\n  if (nitros_image && camera_info) {\n    RCLCPP_DEBUG(this->get_logger(), \"Received both Nitros Image and Camera Info!\");\n    status.messages_present.at(0) = true;\n    status.messages_present.at(1) = true;\n\n    auto nitros_image_view = nvidia::isaac_ros::nitros::NitrosImageView(*nitros_image);\n    if (nitros_image_view.GetTimestampSeconds() == camera_info->header.stamp.sec &&\n      nitros_image_view.GetTimestampNanoseconds() == camera_info->header.stamp.nanosec)\n    {\n      status.exact_time_match = true;\n    } else {\n      RCLCPP_ERROR(this->get_logger(), \"Both messages received, but timestamps didn't match!\");\n    }\n\n    status.stamp = camera_info->header.stamp;\n  } else if (nitros_image) {\n    RCLCPP_DEBUG(this->get_logger(), \"Received only Nitros Image!\");\n    status.messages_present.at(0) = true;\n\n    auto nitros_image_view = nvidia::isaac_ros::nitros::NitrosImageView(*nitros_image);\n    status.stamp.sec = nitros_image_view.GetTimestampSeconds();\n    status.stamp.nanosec = nitros_image_view.GetTimestampNanoseconds();\n  } else if (camera_info) {\n    RCLCPP_DEBUG(this->get_logger(), \"Received only Camera Info!\");\n    status.messages_present.at(1) = true;\n\n    status.stamp = camera_info->header.stamp;\n  }\n\n  RCLCPP_DEBUG(\n    this->get_logger(), \"Sending Status message with stamp: %d.%u\", status.stamp.sec,\n    status.stamp.nanosec);\n\n  pub_->publish(status);\n}\n\n}  // namespace custom_nitros_message_filter\n\n// Register as component\n#include \"rclcpp_components/register_node_macro.hpp\"\nRCLCPP_COMPONENTS_REGISTER_NODE(custom_nitros_message_filter::SynchronizerNode)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_message_filter/test/custom_nitros_message_filter_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport os\nimport pathlib\nimport time\n\nfrom custom_nitros_message_filter_interfaces.msg import SyncStatus\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import CameraInfo, Image\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    synchronizer_node = ComposableNode(\n        name='synchronizer_node',\n        package='custom_nitros_message_filter',\n        plugin='custom_nitros_message_filter::SynchronizerNode',\n        namespace=CustomNitrosMessageFilterPOLTest.generate_namespace(),\n        )\n\n    return CustomNitrosMessageFilterPOLTest.generate_test_description([\n        ComposableNodeContainer(\n            name='custom_nitros_message_filter_pol_test_container',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[synchronizer_node],\n            namespace=CustomNitrosMessageFilterPOLTest.generate_namespace(),\n            output='screen',\n            arguments=['--ros-args', '--log-level', 'info']\n        )\n    ])\n\n\nclass CustomNitrosMessageFilterPOLTest(IsaacROSBaseTest):\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @staticmethod\n    def _format_sync_status(status: SyncStatus) -> str:\n        # Keep failure output compact but actionable.\n        try:\n            stamp_sec = status.stamp.sec\n        except Exception:\n            stamp_sec = '<unavailable>'\n        try:\n            present = list(status.messages_present)\n        except Exception:\n            present = '<unavailable>'\n        try:\n            exact = status.exact_time_match\n        except Exception:\n            exact = '<unavailable>'\n        return f'stamp.sec={stamp_sec}, messages_present={present}, exact_time_match={exact}'\n\n    @IsaacROSBaseTest.for_each_test_case()\n    def test_message_sync(self, test_folder):\n        \"\"\"\n        Test image + camera info synchronization.\n\n        Test that the custom NITROS image and standard camera info messages\n        are properly synchronized by comparing sent messages to response array.\n        \"\"\"\n        TIMEOUT = 5\n\n        # Masks indicating which subset of messages to send in each subtest\n        # To validate correct synchronization behavior, not all messages are sent\n        # in each subtest. At least one message must be sent to trigger a response\n        MESSAGE_MASKS = [\n            (True, True),\n            (True, False),\n            (False, True),\n            (True, True)\n        ]\n\n        # Timing offset for testing each mask without cross-talk\n        TIMING_OFFSET = 1000\n\n        received_messages = {}\n        self.generate_namespace_lookup(['image', 'camera_info', 'status'])\n\n        image_pub = self.node.create_publisher(\n            Image, self.namespaces['image'], self.DEFAULT_QOS)\n        camera_info_pub = self.node.create_publisher(\n            CameraInfo, self.namespaces['camera_info'], self.DEFAULT_QOS)\n\n        subs = self.create_logging_subscribers(\n            [('status', SyncStatus)], received_messages, accept_multiple_messages=True)\n\n        try:\n            # Ensure synchronizer subscriptions are connected before publishing.\n            end_time = time.time() + TIMEOUT\n            while time.time() < end_time:\n                image_ready = image_pub.get_subscription_count() > 0\n                camera_info_ready = camera_info_pub.get_subscription_count() > 0\n                if image_ready and camera_info_ready:\n                    break\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n            image = JSONConversion.load_image_from_json(\n                test_folder / 'image.json')\n            camera_info = JSONConversion.load_camera_info_from_json(\n                test_folder / 'camera_info.json')\n\n            # Conditionally send data to test synchronization\n            # Data will be sent with continually increasing timestamps\n            for i, (include_image, include_camera_info) in enumerate(MESSAGE_MASKS):\n\n                # Wait at most TIMEOUT seconds for subscriber to respond\n                end_time = time.time() + TIMEOUT\n                done = False\n                matched_status = None\n                status_index = len(received_messages['status'])\n                observed_status_total = 0\n                observed_in_window = 0\n                observed_out_of_window = 0\n                rejected_too_short = 0\n                rejected_image_presence = 0\n                rejected_camera_info_presence = 0\n                rejected_exact_time = 0\n                last_in_window_status = None\n                closest_out_of_window_status = None\n                closest_out_of_window_distance = None\n\n                # Record the timing window reserved for this mask's subtest\n                start_timestamp = i * TIMING_OFFSET\n                end_timestamp = (i + 1) * TIMING_OFFSET\n                current_timestamp = start_timestamp\n                while time.time() < end_time:\n\n                    # Apply desired image and camera_info timestamps\n                    if include_image:\n                        image.header.stamp.sec = current_timestamp\n                        image.header.stamp.nanosec = 0\n                        image_pub.publish(image)\n\n                    if include_camera_info:\n                        camera_info.header.stamp.sec = current_timestamp\n                        camera_info.header.stamp.nanosec = 0\n                        camera_info_pub.publish(camera_info)\n\n                    rclpy.spin_once(self.node, timeout_sec=(0.1))\n                    current_timestamp += 1\n\n                    # Continue sending messages until response received\n                    if 'status' not in received_messages or len(received_messages['status']) == 0:\n                        continue\n\n                    # Check any new status messages for a match to this subtest.\n                    while status_index < len(received_messages['status']):\n                        status = received_messages['status'][status_index]\n                        status_index += 1\n                        observed_status_total += 1\n\n                        if not (start_timestamp <= status.stamp.sec < end_timestamp):\n                            observed_out_of_window += 1\n                            # Track the closest stamp we saw outside the window to help debugging.\n                            # (Useful when offsets drift or another subtest's\n                            # messages are leaking in.)\n                            stamp_sec = status.stamp.sec\n                            if stamp_sec < start_timestamp:\n                                dist = start_timestamp - stamp_sec\n                            else:\n                                dist = stamp_sec - (end_timestamp - 1)\n                            if (\n                                (closest_out_of_window_distance is None)\n                                or (dist < closest_out_of_window_distance)\n                            ):\n                                closest_out_of_window_distance = dist\n                                closest_out_of_window_status = status\n                            continue\n\n                        observed_in_window += 1\n                        last_in_window_status = status\n\n                        if len(status.messages_present) < 2:\n                            rejected_too_short += 1\n                            continue\n\n                        if status.messages_present[0] != include_image:\n                            rejected_image_presence += 1\n                            continue\n\n                        if status.messages_present[1] != include_camera_info:\n                            rejected_camera_info_presence += 1\n                            continue\n\n                        if status.exact_time_match != (include_image and include_camera_info):\n                            rejected_exact_time += 1\n                            continue\n\n                        matched_status = status\n                        done = True\n                        break\n\n                    if done:\n                        break\n\n                if not done:\n                    expected_exact_time_match = include_image and include_camera_info\n                    failure_details = [\n                        f'Did not find matching SyncStatus for message mask '\n                        f'({include_image}, {include_camera_info}) within {TIMEOUT}s.',\n                        f'Expected: messages_present=[{include_image}, {include_camera_info}], '\n                        f'exact_time_match={expected_exact_time_match}.',\n                        f'Subtest time window (stamp.sec): [{start_timestamp}, {end_timestamp}).',\n                        f'New status msgs observed: total={observed_status_total}, '\n                        f'in_window={observed_in_window}, out_of_window={observed_out_of_window}.',\n                    ]\n\n                    if observed_status_total == 0:\n                        failure_details.append(\n                            'Failure case: no new `status` messages arrived for this subtest.'\n                        )\n                    elif observed_in_window == 0:\n                        failure_details.append(\n                            'Failure case: received `status` messages, but none fell within the '\n                            'reserved timestamp window for this subtest.'\n                        )\n                        if closest_out_of_window_status is not None:\n                            failure_details.append(\n                                'Closest out-of-window SyncStatus observed: '\n                                f'{self._format_sync_status(closest_out_of_window_status)}'\n                            )\n                    else:\n                        failure_details.append(\n                            'Rejections within window: '\n                            f'too_short_messages_present={rejected_too_short}, '\n                            f'image_presence_mismatch={rejected_image_presence}, '\n                            f'camera_info_presence_mismatch={rejected_camera_info_presence}, '\n                            f'exact_time_match_mismatch={rejected_exact_time}.'\n                        )\n                        if last_in_window_status is not None:\n                            failure_details.append(\n                                'Last in-window SyncStatus observed: '\n                                f'{self._format_sync_status(last_in_window_status)}'\n                            )\n\n                    self.fail('\\n'.join(failure_details))\n\n                status = matched_status\n                self.assertEqual(\n                    status.messages_present[0],\n                    include_image,\n                    'Unexpected mismatch in presence of image!'\n                )\n\n                self.assertEqual(\n                    status.messages_present[1],\n                    include_camera_info,\n                    'Unexpected mismatch in presence of camera info!'\n                )\n\n                if include_image and include_camera_info:\n                    self.assertTrue(\n                        status.exact_time_match,\n                        'Received both messages but with different timestamps!'\n                    )\n                else:\n                    self.assertFalse(\n                        status.exact_time_match,\n                        'Response claims that two timestamps matched, '\n                        'but only one message was sent!'\n                    )\n\n        finally:\n            self.node.destroy_publisher(image_pub)\n            self.node.destroy_subscription(subs)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_message_filter/test/test_cases/profile_image/camera_info.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"tf_camera\"\n  },\n  \"width\": 1920,\n  \"height\": 1080,\n  \"distortion_model\": \"plumb_bob\",\n  \"D\": [\n    0.0,\n    0.0,\n    0.0,\n    0.0,\n    0.0\n  ],\n  \"K\": [\n    434.943999,\n    0.000000,\n    651.073921,\n    0.0,\n    431.741273,\n    441.878037,\n    0.0,\n    0.0,\n    1.0\n  ],\n  \"R\": [\n    1.0,\n    0.0,\n    0.0,\n    0.0,\n    1.0,\n    0.0,\n    0.0,\n    0.0,\n    1.0\n  ],\n  \"P\": [\n    434.943999,\n    0.0,\n    651.073921,\n    160.0,\n    0.0,\n    431.741273,\n    441.878037,\n    0.0,\n    0.0,\n    0.0,\n    1.0,\n    0.0\n  ]\n}\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_message_filter/test/test_cases/profile_image/image.json",
    "content": "{\n    \"image\": \"NVIDIAprofile.png\",\n    \"encoding\": \"rgb8\"\n}"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_message_filter_interfaces/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(custom_nitros_message_filter_interfaces LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\n# The FindPythonInterp and FindPythonLibs modules are removed\nif(POLICY CMP0148)\n  cmake_policy(SET CMP0148 OLD)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Prepare custom SyncStatus interface\nfind_package(rosidl_default_generators REQUIRED)\nrosidl_generate_interfaces(${PROJECT_NAME}\n  msg/SyncStatus.msg\n  DEPENDENCIES builtin_interfaces\n)\nament_export_dependencies(rosidl_default_runtime)\n\nif(BUILD_TESTING)\n  find_package(ament_lint_auto REQUIRED)\n  ament_lint_auto_find_test_dependencies()\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_message_filter_interfaces/msg/SyncStatus.msg",
    "content": "# Timestamp for this observation\nbuiltin_interfaces/Time stamp\n\n# Boolean indicating whether all present messages shared exactly the same Timestamp\nbool exact_time_match\n\n# Boolean array indicating whether or not each message in the synchronization set was present\nbool[] messages_present\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_message_filter_interfaces/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\n SPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\n Licensed under the Apache License, Version 2.0 (the \"License\");\n you may not use this file except in compliance with the License.\n You may obtain a copy of the License at\n\n http://www.apache.org/licenses/LICENSE-2.0\n\n Unless required by applicable law or agreed to in writing, software\n distributed under the License is distributed on an \"AS IS\" BASIS,\n WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n See the License for the specific language governing permissions and\n limitations under the License.\n\n SPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>custom_nitros_message_filter_interfaces</name>\n  <version>4.4.0</version>\n  <description>Interfaces to validate the function of standard and custom message_filters</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Jaiveer Singh</author>\n\n  <build_depend>ament_cmake_auto</build_depend>\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>rosidl_default_generators</build_depend>\n  <exec_depend>rosidl_default_runtime</exec_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <member_of_group>rosidl_interface_packages</member_of_group>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_string/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(custom_nitros_string LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# String Encoder node\nament_auto_add_library(string_encoder_node SHARED src/string_encoder_node.cpp)\nrclcpp_components_register_nodes(string_encoder_node \"custom_nitros_string::StringEncoderNode\")\nset(node_plugins \"${node_plugins}custom_nitros_string::StringEncoderNode;$<TARGET_FILE:string_encoder_node>\\n\")\nset_target_properties(string_encoder_node PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\n# String Decoder node\nament_auto_add_library(string_decoder_node SHARED src/string_decoder_node.cpp)\nrclcpp_components_register_nodes(string_decoder_node \"custom_nitros_string::StringDecoderNode\")\nset(node_plugins \"${node_plugins}custom_nitros_string::StringDecoderNode;$<TARGET_FILE:string_decoder_node>\\n\")\nset_target_properties(string_decoder_node PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  find_package(ament_lint_auto REQUIRED)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/custom_nitros_string_pol.py TIMEOUT \"600\")\n  add_launch_test(test/custom_nitros_string_decoder_pol.py TIMEOUT \"600\")\n  add_launch_test(test/custom_nitros_string_encoder_pol.py TIMEOUT \"600\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_string/include/custom_nitros_string/string_decoder_node.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef CUSTOM_NITROS_STRING__STRING_DECODER_NODE_HPP_\n#define CUSTOM_NITROS_STRING__STRING_DECODER_NODE_HPP_\n\n#include <memory>\n#include <string>\n\n#include \"rclcpp/rclcpp.hpp\"\n\n#include \"isaac_ros_managed_nitros/managed_nitros_subscriber.hpp\"\n\n#include \"std_msgs/msg/string.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list_view.hpp\"\n\nnamespace custom_nitros_string\n{\n\nclass StringDecoderNode : public rclcpp::Node\n{\npublic:\n  explicit StringDecoderNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions());\n\n  ~StringDecoderNode();\n\nprivate:\n  void InputCallback(const nvidia::isaac_ros::nitros::NitrosTensorListView & msg);\n\n  // Subscription to input NitrosTensorList messages\n  std::shared_ptr<nvidia::isaac_ros::nitros::ManagedNitrosSubscriber<\n      nvidia::isaac_ros::nitros::NitrosTensorListView>> nitros_sub_;\n\n  // Publisher for output String messages\n  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;\n\n  // Name of tensor in NitrosTensorList\n  std::string tensor_name_{};\n};\n\n}  // namespace custom_nitros_string\n\n#endif  // CUSTOM_NITROS_STRING__STRING_DECODER_NODE_HPP_\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_string/include/custom_nitros_string/string_encoder_node.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef CUSTOM_NITROS_STRING__STRING_ENCODER_NODE_HPP_\n#define CUSTOM_NITROS_STRING__STRING_ENCODER_NODE_HPP_\n\n#include <memory>\n#include <string>\n\n#include \"rclcpp/rclcpp.hpp\"\n\n#include \"isaac_ros_managed_nitros/managed_nitros_publisher.hpp\"\n\n#include \"std_msgs/msg/string.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list.hpp\"\n\nnamespace custom_nitros_string\n{\n\nclass StringEncoderNode : public rclcpp::Node\n{\npublic:\n  explicit StringEncoderNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions());\n\n  ~StringEncoderNode();\n\nprivate:\n  void InputCallback(const std_msgs::msg::String::SharedPtr msg);\n\n  // Subscription to input String messages\n  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;\n\n  // Publisher for output NitrosTensorList messages\n  std::shared_ptr<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n      nvidia::isaac_ros::nitros::NitrosTensorList>> nitros_pub_;\n\n  // Name of tensor in NitrosTensorList\n  std::string tensor_name_{};\n};\n\n}  // namespace custom_nitros_string\n\n#endif  // CUSTOM_NITROS_STRING__STRING_ENCODER_NODE_HPP_\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_string/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\n SPDX-FileCopyrightText: Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\n Licensed under the Apache License, Version 2.0 (the \"License\");\n you may not use this file except in compliance with the License.\n You may obtain a copy of the License at\n\n http://www.apache.org/licenses/LICENSE-2.0\n\n Unless required by applicable law or agreed to in writing, software\n distributed under the License is distributed on an \"AS IS\" BASIS,\n WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n See the License for the specific language governing permissions and\n limitations under the License.\n\n SPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>custom_nitros_string</name>\n  <version>4.4.0</version>\n  <description>Custom NITROS String encoding and decoding</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Jaiveer Singh</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>std_msgs</depend>\n  <depend>isaac_ros_managed_nitros</depend>\n  <depend>isaac_ros_tensor_list_interfaces</depend>\n  <depend>isaac_ros_nitros_tensor_list_type</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_string/src/string_decoder_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"custom_nitros_string/string_decoder_node.hpp\"\n\n#include <cuda_runtime.h>\n\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list_view.hpp\"\n\nnamespace custom_nitros_string\n{\n\nStringDecoderNode::StringDecoderNode(const rclcpp::NodeOptions options)\n: rclcpp::Node(\"string_decoder_node\", options),\n  nitros_sub_{std::make_shared<nvidia::isaac_ros::nitros::ManagedNitrosSubscriber<\n        nvidia::isaac_ros::nitros::NitrosTensorListView>>(\n      this,\n      \"encoded_tensor\",\n      nvidia::isaac_ros::nitros::nitros_tensor_list_nchw_rgb_f32_t::supported_type_name,\n      std::bind(&StringDecoderNode::InputCallback, this,\n      std::placeholders::_1))},\n  pub_{create_publisher<std_msgs::msg::String>(\n      \"string_output\", 10)},\n  tensor_name_{declare_parameter<std::string>(\"tensor_name\", \"output_tensor\")}\n{}\n\nStringDecoderNode::~StringDecoderNode() = default;\n\nvoid StringDecoderNode::InputCallback(const nvidia::isaac_ros::nitros::NitrosTensorListView & msg)\n{\n  auto tensor = msg.GetNamedTensor(tensor_name_);\n\n  RCLCPP_INFO(this->get_logger(), \"Receiving CUDA buffer with memory at: %p\", tensor.GetBuffer());\n  RCLCPP_INFO(this->get_logger(), \"FrameId: %s\", msg.GetFrameId().c_str());\n  RCLCPP_INFO(this->get_logger(), \"Timestamp seconds: %d\", msg.GetTimestampSeconds());\n  RCLCPP_INFO(this->get_logger(), \"Timestamp nanoseconds: %u\", msg.GetTimestampNanoseconds());\n\n  size_t buffer_size{tensor.GetTensorSize()};\n\n  std::string buffer{};\n  buffer.resize(buffer_size);\n\n  cudaMemcpy(buffer.data(), tensor.GetBuffer(), buffer_size, cudaMemcpyDefault);\n\n  RCLCPP_INFO(this->get_logger(), \"String from CUDA buffer: '%s'\", buffer.c_str());\n\n  std_msgs::msg::String string_msg;\n  string_msg.data = buffer.c_str();\n  RCLCPP_INFO(this->get_logger(), \"Output String message: '%s'\", string_msg.data.c_str());\n\n  pub_->publish(string_msg);\n}\n\n}  // namespace custom_nitros_string\n\n// Register as component\n#include \"rclcpp_components/register_node_macro.hpp\"\nRCLCPP_COMPONENTS_REGISTER_NODE(custom_nitros_string::StringDecoderNode)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_string/src/string_encoder_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"custom_nitros_string/string_encoder_node.hpp\"\n\n#include <cuda_runtime.h>\n#include <string>\n\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_builder.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list_builder.hpp\"\n\nnamespace custom_nitros_string\n{\n\nStringEncoderNode::StringEncoderNode(const rclcpp::NodeOptions options)\n: rclcpp::Node(\"string_encoder_node\", options),\n  sub_{create_subscription<std_msgs::msg::String>(\n      \"string_input\", 10, std::bind(&StringEncoderNode::InputCallback, this,\n      std::placeholders::_1))},\n  nitros_pub_{std::make_shared<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n        nvidia::isaac_ros::nitros::NitrosTensorList>>(\n      this, \"encoded_tensor\",\n      nvidia::isaac_ros::nitros::nitros_tensor_list_nchw_rgb_f32_t::supported_type_name)},\n  tensor_name_{declare_parameter<std::string>(\"tensor_name\", \"input_tensor\")}\n{}\n\nStringEncoderNode::~StringEncoderNode() = default;\n\nvoid StringEncoderNode::InputCallback(const std_msgs::msg::String::SharedPtr msg)\n{\n  RCLCPP_INFO(this->get_logger(), \"Input String message: '%s'\", msg->data.c_str());\n\n  // Get size of string, accounting for null terminator byte\n  size_t buffer_size{msg->data.size() + 1};\n\n  // Allocate CUDA buffer to encode string\n  void * buffer;\n  cudaMalloc(&buffer, buffer_size);\n\n  // Copy string's bytes to CUDA buffer\n  cudaMemcpy(buffer, msg->data.data(), buffer_size, cudaMemcpyDefault);\n\n  // Adding header data\n  std_msgs::msg::Header header;\n  header.stamp.sec = 123456;\n  header.stamp.nanosec = 789101112;\n  header.frame_id = tensor_name_;\n\n  // Create tensor list with tensor wrapping CUDA buffer\n  nvidia::isaac_ros::nitros::NitrosTensorList tensor_list =\n    nvidia::isaac_ros::nitros::NitrosTensorListBuilder()\n    .WithHeader(header)\n    .AddTensor(\n    tensor_name_,\n    (\n      nvidia::isaac_ros::nitros::NitrosTensorBuilder()\n      .WithShape({static_cast<int>(buffer_size)})\n      .WithDataType(nvidia::isaac_ros::nitros::NitrosDataType::kUnsigned8)  // sizeof(uint8_t) == 1\n      .WithData(buffer)\n      .Build()\n    )\n    )\n    .Build();\n\n  RCLCPP_INFO(this->get_logger(), \"Sending CUDA buffer with memory at: %p\", buffer);\n\n  nitros_pub_->publish(tensor_list);\n}\n\n}  // namespace custom_nitros_string\n\n// Register as component\n#include \"rclcpp_components/register_node_macro.hpp\"\nRCLCPP_COMPONENTS_REGISTER_NODE(custom_nitros_string::StringEncoderNode)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_string/test/custom_nitros_string_decoder_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\nimport time\n\nfrom isaac_ros_tensor_list_interfaces.msg import Tensor, TensorList, TensorShape\nfrom isaac_ros_test import IsaacROSBaseTest\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport pytest\nimport rclpy\nfrom std_msgs.msg import String\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    decoder_node = ComposableNode(\n        name='decoder',\n        package='custom_nitros_string',\n        plugin='custom_nitros_string::StringDecoderNode',\n        namespace=CustomNitrosStringDecoderPOLTest.generate_namespace(),\n        parameters=[{\n            'tensor_name': 'pol_tensor'\n        }])\n\n    return CustomNitrosStringDecoderPOLTest.generate_test_description([\n        ComposableNodeContainer(\n            name='custom_nitros_string_pol_test_container',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[decoder_node],\n            namespace=CustomNitrosStringDecoderPOLTest.generate_namespace(),\n            output='screen'\n        )\n    ])\n\n\nclass CustomNitrosStringDecoderPOLTest(IsaacROSBaseTest):\n\n    def test_string_decoding(self):\n        \"\"\"\n        Test string decoding.\n\n        Test that the custom NITROS string Decoder correctly decodes the input tensor.\n        \"\"\"\n        TIMEOUT = 300\n        received_messages = {}\n\n        self.generate_namespace_lookup(['string_output', 'encoded_tensor'])\n\n        tensor_pub = self.node.create_publisher(\n            TensorList, self.namespaces['encoded_tensor'], self.DEFAULT_QOS)\n\n        subs = self.create_logging_subscribers(\n            [('string_output', String)], received_messages)\n\n        try:\n            # Construct test tensor list\n            DATA_TYPE = 2  # uint8_t\n            INPUT_STRING = 'Hello, world!'\n            INPUT_TENSOR_DIMENSIONS = [len(INPUT_STRING)]\n            INPUT_TENSOR_NAME = 'pol_tensor'\n            INPUT_TENSOR_STRIDES = [1]\n\n            tensor_list = TensorList()\n            tensor = Tensor()\n            tensor_shape = TensorShape()\n\n            tensor_shape.rank = len(INPUT_TENSOR_DIMENSIONS)\n            tensor_shape.dims = INPUT_TENSOR_DIMENSIONS\n\n            tensor.shape = tensor_shape\n            tensor.name = INPUT_TENSOR_NAME\n            tensor.data_type = DATA_TYPE\n            tensor.strides = INPUT_TENSOR_STRIDES\n            tensor.data = [ord(c) for c in INPUT_STRING]\n\n            tensor_list.tensors = [tensor]\n\n            end_time = time.time() + TIMEOUT\n            done = False\n\n            while time.time() < end_time:\n                rclpy.spin_once(self.node, timeout_sec=(0.1))\n\n                tensor_pub.publish(tensor_list)\n\n                if 'string_output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, 'Appropriate output not received')\n\n            output_string = received_messages['string_output']\n            self.assertEqual(\n                INPUT_STRING, output_string.data, 'Output string did not match input!')\n\n        finally:\n            self.node.destroy_publisher(tensor_pub)\n            self.node.destroy_subscription(subs)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_string/test/custom_nitros_string_encoder_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\nimport time\n\nfrom isaac_ros_tensor_list_interfaces.msg import TensorList\nfrom isaac_ros_test import IsaacROSBaseTest\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport pytest\nimport rclpy\nfrom std_msgs.msg import String\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    encoder_node = ComposableNode(\n        name='encoder',\n        package='custom_nitros_string',\n        plugin='custom_nitros_string::StringEncoderNode',\n        namespace=CustomNitrosStringEncoderPOLTest.generate_namespace(),\n        parameters=[{\n            'tensor_name': 'pol_tensor'\n        }])\n\n    return CustomNitrosStringEncoderPOLTest.generate_test_description([\n        ComposableNodeContainer(\n            name='custom_nitros_string_pol_test_container',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[encoder_node],\n            namespace=CustomNitrosStringEncoderPOLTest.generate_namespace(),\n            output='screen'\n        )\n    ])\n\n\nclass CustomNitrosStringEncoderPOLTest(IsaacROSBaseTest):\n\n    def test_string_encoding(self):\n        \"\"\"\n        Test string encoding.\n\n        Test that the custom NITROS string encoder correctly encodes the input string.\n        \"\"\"\n        TIMEOUT = 300\n        received_messages = {}\n\n        self.generate_namespace_lookup(['string_input', 'encoded_tensor'])\n\n        string_pub = self.node.create_publisher(\n            String, self.namespaces['string_input'], self.DEFAULT_QOS)\n\n        subs = self.create_logging_subscribers(\n            [('encoded_tensor', TensorList)], received_messages)\n\n        try:\n            input_string_msg = String()\n            input_string_msg.data = 'Hello, world!'\n\n            end_time = time.time() + TIMEOUT\n            done = False\n\n            while time.time() < end_time:\n                rclpy.spin_once(self.node, timeout_sec=(0.1))\n\n                string_pub.publish(input_string_msg)\n\n                if 'encoded_tensor' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, 'Appropriate output not received')\n\n            output_tensor_list = received_messages['encoded_tensor']\n            self.assertEqual(\n                len(output_tensor_list.tensors), 1, 'Did not receive exactly 1 output tensor')\n\n            output_tensor = output_tensor_list.tensors[0]\n            output_string = ''.join([chr(num) for num in output_tensor.data[:-1]])\n\n            self.assertEqual(\n                input_string_msg.data, output_string, 'Output string did not match input!')\n\n        finally:\n            self.node.destroy_publisher(string_pub)\n            self.node.destroy_subscription(subs)\n"
  },
  {
    "path": "isaac_ros_managed_nitros_examples/custom_nitros_string/test/custom_nitros_string_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport pytest\nimport rclpy\nfrom std_msgs.msg import String\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    encoder_node = ComposableNode(\n        name='encoder',\n        package='custom_nitros_string',\n        plugin='custom_nitros_string::StringEncoderNode',\n        namespace=CustomNitrosStringPOLTest.generate_namespace(),\n        parameters=[{\n            'tensor_name': 'pol_tensor'\n        }])\n\n    decoder_node = ComposableNode(\n        name='decoder',\n        package='custom_nitros_string',\n        plugin='custom_nitros_string::StringDecoderNode',\n        namespace=CustomNitrosStringPOLTest.generate_namespace(),\n        parameters=[{\n            'tensor_name': 'pol_tensor'\n        }])\n\n    return CustomNitrosStringPOLTest.generate_test_description([\n        ComposableNodeContainer(\n            name='custom_nitros_string_pol_test_container',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[decoder_node, encoder_node],\n            namespace=CustomNitrosStringPOLTest.generate_namespace(),\n            output='screen'\n        )\n    ])\n\n\nclass CustomNitrosStringPOLTest(IsaacROSBaseTest):\n\n    def test_message_passthrough(self):\n        \"\"\"\n        Test string encoding and decoding.\n\n        Test that the custom NITROS string encoder and decoder correctly encode and then\n        decode the input string.\n        \"\"\"\n        TIMEOUT = 300\n        received_messages = {}\n\n        self.generate_namespace_lookup(['string_input', 'string_output'])\n\n        string_pub = self.node.create_publisher(\n            String, self.namespaces['string_input'], self.DEFAULT_QOS)\n\n        subs = self.create_logging_subscribers(\n            [('string_output', String)], received_messages)\n\n        try:\n            input_string = String()\n            input_string.data = 'Hello, world!'\n\n            end_time = time.time() + TIMEOUT\n            done = False\n\n            while time.time() < end_time:\n                rclpy.spin_once(self.node, timeout_sec=(0.1))\n\n                string_pub.publish(input_string)\n\n                if 'string_output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, 'Appropriate output not received')\n\n            output_string = received_messages['string_output']\n\n            self.assertEqual(input_string, output_string, 'Output string did not match input!')\n\n        finally:\n            self.node.destroy_publisher(string_pub)\n            self.node.destroy_subscription(subs)\n"
  },
  {
    "path": "isaac_ros_nitros/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\ninstall(PROGRAMS scripts/diagnostic_viewer.py DESTINATION lib/${PROJECT_NAME})\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3 / CUDA::nvToolsExt) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# cuVSLAM, cuApriltags, and cuMotion (register paths and install)\n\n# Determine the architecture\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"aarch64\")\n    set(CUVSLAM_LIB_PATH \"lib/cuvslam/lib_aarch64_jetpack70\")\n    set(CUAPRILTAGS_LIB_PATH \"lib/cuapriltags/lib_aarch64_jetpack61\")\n    set(CUMOTION_PATH \"lib/cumotion/aarch64_jetpack70\")\nelseif(CMAKE_SYSTEM_PROCESSOR MATCHES \"x86_64\")\n    set(CUVSLAM_LIB_PATH \"lib/cuvslam/lib_x86_64_cuda_13_0\")\n    set(CUAPRILTAGS_LIB_PATH \"lib/cuapriltags/lib_x86_64_cuda_12_6\")\n    set(CUMOTION_PATH \"lib/cumotion/x86_64_cuda_13_0\")\nelse()\n    message(FATAL_ERROR \"Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nendif()\n\n# Install exported libraries\ninstall(FILES ${CUAPRILTAGS_LIB_PATH}/libcuapriltags.a\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/cuapriltags/lib\")\ninstall(FILES\n        ${CUVSLAM_LIB_PATH}/libcuvslam.so\n        ${CUVSLAM_LIB_PATH}/cuvslam_api_launcher\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/cuvslam/lib\")\ninstall(DIRECTORY ${CUMOTION_PATH}/lib\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/cumotion\")\n\n# Install header files for cuvslam\ninstall(FILES lib/cuvslam/include/cuvslam/cuvslam2.h\n              lib/cuvslam/include/cuvslam/ground_constraint2.h\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/cuvslam/include/cuvslam\")\n\n# cuVSLAM (register path)\nament_index_register_resource(cuvslam CONTENT share/${PROJECT_NAME}/cuvslam)\n\n# Install the header file for cuapriltags\ninstall(FILES lib/cuapriltags/cuapriltags/cuAprilTags.h\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/cuapriltags/cuapriltags\")\n\n# cuApriltags (register path)\nament_index_register_resource(cuapriltags CONTENT share/${PROJECT_NAME}/cuapriltags)\n\n# Install the header files for cuMotion\ninstall(DIRECTORY ${CUMOTION_PATH}/include\n        DESTINATION \"${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/cumotion\")\n\n# cuMotion (register path)\nament_index_register_resource(cumotion CONTENT share/${PROJECT_NAME}/cumotion)\n\n# Dependencies\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(vpi REQUIRED)\nfind_package(yaml-cpp)\n\n# NitrosNode\nament_auto_add_library(${PROJECT_NAME} SHARED\n  src/nitros_node.cpp\n  src/nitros_context.cpp\n  src/types/type_adapter_nitros_context.cpp\n  src/types/nitros_type_base.cpp\n  src/types/nitros_empty.cpp\n  src/nitros_publisher.cpp\n  src/nitros_subscriber.cpp\n  src/nitros_publisher_subscriber_group.cpp\n)\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  vpi\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosEmptyForwardNode\n  ament_auto_add_library(nitros_empty_forward_node SHARED test/src/nitros_empty_forward_node.cpp)\n  target_link_libraries(nitros_empty_forward_node ${PROJECT_NAME})\n  set_target_properties(nitros_empty_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(nitros_empty_forward_node \"nvidia::isaac_ros::nitros::NitrosEmptyForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosEmpty;$<TARGET_FILE:nitros_empty_forward_node>\\n\")\n\nfind_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_test_pol.py TIMEOUT \"15\")\n  add_launch_test(test/isaac_ros_nitros_diagnostics_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE config)\n"
  },
  {
    "path": "isaac_ros_nitros/config/type_adapter_nitros_context_graph.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2025, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: memory_pool\ncomponents:\n- name: unbounded_allocator\n  type: nvidia::gxf::UnboundedAllocator\n---\nname: cuda_stream_pool\ncomponents:\n- name: stream\n  type: nvidia::gxf::CudaStreamPool\n  parameters:\n    stream_flags: 1\n    stream_priority: 0\n    reserved_size: 3\n    max_size: 3\n    nvtx_identifier: \"nitros_type_adapter_stream_pool\"\n---\n#############\n# Pose Tree #\n#############\nname: global_pose_tree\ncomponents:\n  - name: pose_tree\n    type: nvidia::isaac::PoseTree\n  - type: nvidia::isaac::PoseTreeSetup\n    parameters:\n      pose_tree: global_pose_tree/pose_tree\n  - name: world_frame\n    type: nvidia::isaac::PoseTreeFrame\n    parameters:\n      pose_tree: global_pose_tree/pose_tree\n  - name: map_frame\n    type: nvidia::isaac::PoseTreeFrame\n    parameters:\n      pose_tree: global_pose_tree/pose_tree\n      parent_frame: global_pose_tree/world_frame\n      initial_pose:\n        translation: [0.0, 0.0, 0.0]\n        rotation_rpy: [0.0, 0.0, 0.0]\n  - name: robot_frame\n    type: nvidia::isaac::PoseTreeFrame\n    parameters:\n      pose_tree: global_pose_tree/pose_tree\n      parent_frame: global_pose_tree/world_frame\n      initial_pose:\n        translation: [0.0, 0.0, 0.0]\n        rotation_rpy: [0.0, 0.0, 0.0]\n  - name: composite_schema_server\n    type: nvidia::isaac::CompositeSchemaServer\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/nitros_context.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__NITROS_CONTEXT_HPP_\n#define ISAAC_ROS_NITROS__NITROS_CONTEXT_HPP_\n\n#include <mutex>\n#include <set>\n#include <string>\n#include <vector>\n\n#include \"cuda_runtime.h\" // NOLINT - include .h without directory\n\n#include \"gxf/core/gxf.h\"\n#include \"gxf/cuda/cuda_stream_pool.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n#include \"std_msgs/msg/header.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// An abstraction layer for GXF context\nclass NitrosContext\n{\npublic:\n  // Constructor\n  NitrosContext();\n\n  // Setter for graph_namespace_\n  void setGraphNamespace(const std::string & graph_namespace);\n\n  // Setter for node_ that is used to get ROS logger if set\n  void setNode(const rclcpp::Node * node);\n\n  // Getter for the created local context\n  gxf_context_t getContext();\n\n  // Get the pointer of the specified component\n  gxf_result_t getComponentPointer(\n    const std::string & entity_name,\n    const std::string & component_name,\n    const std::string & component_type,\n    void ** pointer);\n\n  // Get the EID for the given entity name\n  gxf_result_t getEid(\n    const std::string & entity_name,\n    gxf_uid_t & eid);\n\n  // Get the CID for a component of the given type in the given entity name\n  gxf_result_t getCid(\n    const std::string & entity_name,\n    const std::string & component_type,\n    gxf_uid_t & cid);\n\n  // Get the CID for a component of the given name and type in the given entity name\n  gxf_result_t getCid(\n    const std::string & entity_name,\n    const std::string & component_name,\n    const std::string & component_type,\n    gxf_uid_t & cid);\n\n  // Override a parameter value in the graph to be loaded\n  void preLoadGraphSetParameter(\n    const std::string & entity_name,\n    const std::string & component_name,\n    const std::string & parameter_name,\n    const std::string & value\n  );\n\n  // Load an extension so file\n  gxf_result_t loadExtension(\n    const std::string & base_dir,\n    const std::string & extension);\n\n  // Load a list of extension so files\n  gxf_result_t loadExtensions(\n    const std::string & base_dir,\n    const std::vector<std::string> & extensions);\n\n  // Load the given graph(s) to the context\n  gxf_result_t loadApplication(const std::string & list_of_files);\n\n  // Activate and asynchronously run the loaded graph\n  gxf_result_t runGraphAsync();\n\n  // Initialize CUDA Stream Pool, this will intialize and allocate all available CUDA streams\n  // from the pool. This is used to avoid the overhead of creating a new CUDA stream for each\n  // type adaptation.\n  gxf_result_t initCudaStreamPool();\n\n  // Terminate the running graph(s)\n  gxf_result_t destroy();\n\n  // Get a timestamp stored in the given entity and assign to the given ROS header\n  gxf_result_t getEntityTimestamp(\n    const gxf_uid_t eid,\n    std_msgs::msg::Header & ros_header);\n\n  // Get CUDA stream from the stream pool of the graph, this is used by the type adaptation of\n  // Nitros graph\n  cudaStream_t getCudaStreamFromNitrosGraph();\n\n  // Set GXF log level for the context\n  void setExtensionLogSeverity(gxf_severity_t severity_level);\n\n  // Setter for the default CUDA memory pool size on device 0.\n  // Only take effect when the requested CUDA memory pool size is larger than the current\n  gxf_result_t setCUDAMemoryPoolSize(uint64_t cuda_mem_pool_size);\n\n  // APIs for setting a parameter value in a given component\n  gxf_result_t setParameterInt64(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const int64_t parameter_value);\n  gxf_result_t setParameterInt64(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const int64_t parameter_value);\n\n  gxf_result_t setParameterUInt64(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const uint64_t parameter_value);\n  gxf_result_t setParameterUInt64(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const uint64_t parameter_value);\n\n  gxf_result_t setParameterInt32(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const int32_t parameter_value);\n  gxf_result_t setParameterInt32(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const int32_t parameter_value);\n\n  gxf_result_t setParameterUInt32(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const uint32_t parameter_value);\n  gxf_result_t setParameterUInt32(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const uint32_t parameter_value);\n\n  gxf_result_t setParameterUInt16(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const uint16_t parameter_value);\n  gxf_result_t setParameterUInt16(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const uint16_t parameter_value);\n\n  gxf_result_t setParameterFloat32(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const float parameter_value);\n  gxf_result_t setParameterFloat32(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const float parameter_value);\n\n  gxf_result_t setParameterFloat64(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const double parameter_value);\n  gxf_result_t setParameterFloat64(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const double parameter_value);\n\n  gxf_result_t setParameterStr(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const std::string & parameter_value);\n  gxf_result_t setParameterStr(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const std::string & parameter_value);\n\n  gxf_result_t setParameterHandle(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const gxf_uid_t & uid);\n  gxf_result_t setParameterHandle(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const gxf_uid_t & uid);\n\n  gxf_result_t setParameterBool(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const bool parameter_value);\n  gxf_result_t setParameterBool(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const bool parameter_value);\n\n  gxf_result_t setParameter1DStrVector(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const std::vector<std::string> & parameter_value);\n  gxf_result_t setParameter1DStrVector(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    const std::vector<std::string> & parameter_value);\n\n  gxf_result_t setParameter1DInt32Vector(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    std::vector<int32_t> & parameter_value);\n  gxf_result_t setParameter1DInt32Vector(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    std::vector<int32_t> & parameter_value);\n\n  gxf_result_t setParameter1DInt64Vector(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    std::vector<int64_t> & parameter_value);\n  gxf_result_t setParameter1DInt64Vector(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    std::vector<int64_t> & parameter_value);\n\n  gxf_result_t setParameter1DFloat64Vector(\n    const std::string & entity_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    std::vector<double> & parameter_value);\n  gxf_result_t setParameter1DFloat64Vector(\n    const std::string & entity_name,\n    const std::string & codelet_name,\n    const std::string & codelet_type,\n    const std::string & parameter_name,\n    std::vector<double> & parameter_value);\n\nprivate:\n  // Get the entity name with the configured graph namespace\n  std::string getNamespacedEntityName(const std::string & entity_name);\n\n  // Get the node's logger if set. Forward to rclcpp::get_logger() otherwise.\n  rclcpp::Logger get_logger();\n\n  // The associated ROS node (for logging purpose)\n  const rclcpp::Node * node_ = nullptr;\n\n  // Shared context across all NitrosContext\n  static gxf_context_t main_context_;\n  static gxf_context_t shared_context_;\n  static std::mutex shared_context_mutex_;\n\n  // A shared loaded extension list for avoiding duplicate loadings\n  static std::set<std::string> loaded_extension_file_paths_;\n\n  // Local context created from the shared context\n  gxf_context_t context_;\n\n  // Graph namespace for avoiding conflict names in the shared context\n  std::string graph_namespace_;\n\n  // GXF graph parameter overriding strings\n  std::vector<std::string> graph_param_override_string_list_;\n\n  // Extension log severity level\n  static gxf_severity_t extension_log_severity_;\n\n  // Stores pool of streams type adaptation can use, streams are allocated randomly from the pool\n  std::vector<cudaStream_t> cuda_streams_;\n  // Whether the CUDA stream pool has been initialized\n  bool is_cuda_stream_initialized_ = false;\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__NITROS_CONTEXT_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/nitros_node.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__NITROS_NODE_HPP_\n#define ISAAC_ROS_NITROS__NITROS_NODE_HPP_\n\n#include <map>\n#include <memory>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"extensions/gxf_optimizer/core/optimizer.hpp\"\n#include \"extensions/gxf_optimizer/exporter/graph_types.hpp\"\n\n#include \"isaac_ros_nitros/nitros_context.hpp\"\n#include \"isaac_ros_nitros/nitros_publisher_subscriber_group.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nusing gxf::optimizer::GraphIOGroupSupportedDataTypesInfoList;\nusing gxf::optimizer::GraphIOGroupDataTypeConfigurations;\nusing gxf::optimizer::ComponentInfo;\nusing gxf::optimizer::ComponentKey;\nusing NitrosPubSubGroupPointerList = std::vector<std::shared_ptr<NitrosPublisherSubscriberGroup>>;\n\n// A hardware-accelerated ROS node base class\nclass NitrosNode : public rclcpp::Node\n{\npublic:\n  ~NitrosNode();\n\n  NitrosNode(const NitrosNode & node) = delete;\n\n  NitrosNode & operator=(const NitrosNode & node) = delete;\n\nprotected:\n  // Constructor with user-defined IO groups\n  NitrosNode(\n    const rclcpp::NodeOptions & options,\n    const std::string & app_yaml_filename,\n    const GraphIOGroupSupportedDataTypesInfoList & gxf_io_group_info_list,\n    const NitrosPublisherSubscriberConfigMap & config_map,\n    const std::vector<std::string> & extension_spec_filenames,\n    const std::vector<std::string> & generator_rule_filenames,\n    const std::vector<std::pair<std::string, std::string>> & extensions,\n    const std::string & package_name);\n\n  // Constructor with user-defined IO groups (with preset specs)\n  NitrosNode(\n    const rclcpp::NodeOptions & options,\n    const std::string & app_yaml_filename,\n    const GraphIOGroupSupportedDataTypesInfoList & gxf_io_group_info_list,\n    const NitrosPublisherSubscriberConfigMap & config_map,\n    const std::vector<std::string> & extension_spec_preset_names,\n    const std::vector<std::string> & extension_spec_filenames,\n    const std::vector<std::string> & generator_rule_filenames,\n    const std::vector<std::pair<std::string, std::string>> & extensions,\n    const std::string & package_name);\n\n  // Constructor without user-defined IO groups\n  NitrosNode(\n    const rclcpp::NodeOptions & options,\n    const std::string & app_yaml_filename,\n    const NitrosPublisherSubscriberConfigMap & config_map,\n    const std::vector<std::string> & extension_spec_filenames,\n    const std::vector<std::string> & generator_rule_filenames,\n    const std::vector<std::pair<std::string, std::string>> & extensions,\n    const std::string & package_name);\n\n  // Constructor without user-defined IO groups (with preset specs)\n  NitrosNode(\n    const rclcpp::NodeOptions & options,\n    const std::string & app_yaml_filename,\n    const NitrosPublisherSubscriberConfigMap & config_map,\n    const std::vector<std::string> & extension_spec_preset_names,\n    const std::vector<std::string> & extension_spec_filenames,\n    const std::vector<std::string> & generator_rule_filenames,\n    const std::vector<std::pair<std::string, std::string>> & extensions,\n    const std::string & package_name);\n\n  // Complete constructor\n  NitrosNode(\n    const rclcpp::NodeOptions & options,\n    const std::string & app_yaml_filename,\n    const GraphIOGroupSupportedDataTypesInfoList & gxf_io_group_info_list,\n    const bool use_custom_io_group_info_list,\n    const bool use_raw_graph_no_optimizer,\n    const NitrosPublisherSubscriberConfigMap & config_map,\n    const std::vector<std::string> & extension_spec_preset_names,\n    const std::vector<std::string> & extension_spec_filenames,\n    const std::vector<std::string> & generator_rule_filenames,\n    const std::vector<std::pair<std::string, std::string>> & extensions,\n    const std::string & package_name);\n\n  // Getter for the underlying Nitros context object\n  NitrosContext & getNitrosContext();\n\n  // Initialize and start the NitrosNode\n  void startNitrosNode();\n\n  // The callback to be implemented by users for any required initialization before graph\n  // is loaded (but after negotiation results are available)\n  virtual void preLoadGraphCallback() {}\n\n  // The callback to be implemented by users for any required initialization after graph is loaded\n  virtual void postLoadGraphCallback() {}\n\n  // Override a parameter value in the graph to be loaded\n  void preLoadGraphSetParameter(\n    const std::string & entity_name,\n    const std::string & component_name,\n    const std::string & parameter_name,\n    const std::string & value\n  );\n\n  // Find the corresponding Nitros publisher of the given component\n  std::shared_ptr<NitrosPublisher> findNitrosPublisher(\n    const gxf::optimizer::ComponentInfo & comp_info);\n\n  // Find the corresponding Nitros subscriber of the given component\n  std::shared_ptr<NitrosSubscriber> findNitrosSubscriber(\n    const gxf::optimizer::ComponentInfo & comp_info);\n\n  // Get the negotiated data format for the given component\n  std::string getNegotiatedDataFormat(const ComponentInfo comp_info) const;\n\n  // Get the negotiated data format or the compatible format if negotiation failed\n  // for the given component\n  std::string getFinalDataFormat(const ComponentInfo comp_info) const;\n\n  // Setter for use_custom_io_group_info_list_ that determines whether to use a user-defined\n  // custom IO group list (in contrast to letting the optimizer create the IO group list)\n  void setUseCustomIOGroupInfoList(const bool use_custom);\n\n  // Add an extension spec file to be loaded into the graph optimizer\n  void addExtensionSpecFilename(std::string package_relative_filepath);\n\n  // Add a generator rule file to be loaded into the graph optimizer\n  void addGeneratorRuleFilename(std::string package_relative_filepath);\n\n  // Set the graph YAML file to be loaded\n  void setAppYamlFilename(std::string package_relative_filepath);\n\n  // Set the frame id for a custom frame id publisher\n  void setFrameIdSource(std::string source_frame_id_map_key, std::string frame_id);\n\n  // Register a supported type\n  template<typename T>\n  void registerSupportedType()\n  {\n    nitros_type_manager_->registerSupportedType<T>();\n  }\n\n  // Nitros type manager\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager_;\n\n  // A list of I/O port groups\n  GraphIOGroupSupportedDataTypesInfoList gxf_io_group_info_list_;\n\n  // Determine if a user-defined IO group info list should be used\n  // (in contrast to creating the IO group list by using the optimizer)\n  bool use_custom_io_group_info_list_ = false;\n\n  // A map for specifying the configurations for all I/O ports\n  NitrosPublisherSubscriberConfigMap config_map_;\n\n  // Path to the shared directory under this (isaac_ros_nitros) package\n  std::string nitros_package_share_directory_;\n\n  // Path to the shared directory under the sub-class package\n  std::string package_share_directory_;\n\nprivate:\n  // The function to be called after negotiation timeout\n  void postNegotiationCallback();\n\n  // The function for checking the status of the underlying graph\n  void gxfHeartbeatCallback();\n\n  // A randomly generated namespace that's unique for the current node\n  std::string graph_namespace_;\n\n  // An application graph to be loaded in the node's context\n  std::string app_yaml_filename_;\n\n  // A list of preset component spec to be loaded in the graph optimizer\n  std::vector<std::string> extension_spec_preset_names_;\n\n  // A list of component specs to be loaded in the graph optimizer\n  std::vector<std::string> extension_spec_filenames_;\n\n  // A list of rules to be loaded in the graph optimizer\n  std::vector<std::string> generator_rule_filenames_;\n\n  // A list of component extension so files to be loaded in the node's context\n  std::vector<std::pair<std::string, std::string>> extensions_;\n\n  // A list of pub/sub groups in which the data formats of a group of publishers and subscribers\n  // are dependent on each other\n  NitrosPubSubGroupPointerList nitros_pub_sub_groups_;\n\n  // The graph optimizer\n  nvidia::gxf::optimizer::Optimizer optimizer_;\n\n  // The node's package name\n  std::string package_name_;\n\n  // Negotiation wait timer\n  rclcpp::TimerBase::SharedPtr negotiation_timer_;\n\n  // The GXF graph heartbeat timer\n  rclcpp::TimerBase::SharedPtr gxf_heartbeat_timer_;\n\n  // The eid for checking the graph status\n  gxf_uid_t heartbeat_eid_ = -1;\n\n  // Map for frame_id passthrough\n  std::shared_ptr<std::map<ComponentKey, std::string>> frame_id_map_ptr_;\n\n  // Configurations for a Nitros diagnostics\n  std::shared_ptr<NitrosDiagnosticsConfig> diagnostics_config_;\n\n  // When enabled namespacing and the graph optimizer are ignored\n  // In such a case, graph(s) will be loaded intact\n  bool use_raw_graph_no_optimizer_ = false;\n\n  // Nitros context that has a shared underlying context across all NitrosNodes in the same process\n  std::shared_ptr<NitrosContext> nitros_context_ptr_;\n\n  // Wait time in seconds before concluding negotiation results\n  int64_t type_negotiation_duration_s_ = 1;\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__NITROS_NODE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__NITROS_PUBLISHER_HPP_\n#define ISAAC_ROS_NITROS__NITROS_PUBLISHER_HPP_\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"cuda_runtime.h\"  // NOLINT - include .h without directory\n\n#include \"message_compositor/message_relay.hpp\"\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#include \"gxf/std/vault.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros/nitros_publisher_subscriber_base.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"negotiated/negotiated_publisher.hpp\"\n#include \"rclcpp/waitable.hpp\"\n#include \"rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp\"\n#include \"std_msgs/msg/header.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Forward declaration\nclass NitrosPublisher;\n\n\n// A waitable for publishing messages triggered from GXF graph\nclass NitrosPublisherWaitable : public rclcpp::Waitable\n{\npublic:\n  NitrosPublisherWaitable(\n    rclcpp::Node & node,\n    NitrosPublisher & nitros_publisher);\n\n  size_t get_number_of_ready_guard_conditions()\n  {\n    return 1;\n  }\n\n  // Trigger guard_condition_\n  void trigger();\n\n  std::shared_ptr<void> take_data() override\n  {\n    return nullptr;\n  }\n\n  bool is_ready(rcl_wait_set_t * wait_set) override;\n  void execute(std::shared_ptr<void> & data) override;\n  void add_to_wait_set(rcl_wait_set_t * wait_set) override;\n\nprivate:\n  rclcpp::Node & node_;\n  NitrosPublisher & nitros_publisher_;\n  std::mutex guard_condition_mutex_;\n  rclcpp::GuardCondition guard_condition_;\n\n  // A flag for tracking if a trigger to the guard condition has been handled\n  bool is_trigger_pending_ = false;\n};\n\n\n// Nitros publisher that supports type adaptation/negotiation and enables hardware acceleration\nclass NitrosPublisher : public NitrosPublisherSubscriberBase\n{\npublic:\n  // Constructor\n  NitrosPublisher(\n    rclcpp::Node & node,\n    std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n    const gxf::optimizer::ComponentInfo & gxf_component_info,\n    const std::vector<std::string> & supported_data_formats,\n    const NitrosPublisherSubscriberConfig & config,\n    const negotiated::NegotiatedPublisherOptions & negotiated_pub_options);\n\n  NitrosPublisher(\n    rclcpp::Node & node,\n    const gxf_context_t context,\n    std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n    const gxf::optimizer::ComponentInfo & gxf_component_info,\n    const std::vector<std::string> & supported_data_formats,\n    const NitrosPublisherSubscriberConfig & config,\n    const negotiated::NegotiatedPublisherOptions & negotiated_pub_options);\n\n  NitrosPublisher(\n    rclcpp::Node & node,\n    const gxf_context_t context,\n    std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n    const gxf::optimizer::ComponentInfo & gxf_component_info,\n    const std::vector<std::string> & supported_data_formats,\n    const NitrosPublisherSubscriberConfig & config,\n    const negotiated::NegotiatedPublisherOptions & negotiated_pub_options,\n    const NitrosDiagnosticsConfig & diagnostics_config);\n\n  // Constructor for creating a publisher without an associated gxf egress port\n  NitrosPublisher(\n    rclcpp::Node & node,\n    const gxf_context_t context,\n    std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n    const std::vector<std::string> & supported_data_formats,\n    const NitrosPublisherSubscriberConfig & config,\n    const NitrosDiagnosticsConfig & diagnostics_config);\n\n  // Getter for the negotiated_pub_\n  std::shared_ptr<negotiated::NegotiatedPublisher> getNegotiatedPublisher();\n\n  // Add a supported data format to the underlying negotiated publisher\n  void addSupportedDataFormat(\n    const std::string & data_format,\n    const double weight);\n\n  // Start negotiation\n  void start();\n\n  // Create a compatible publisher\n  void createCompatiblePublisher();\n\n  // To be called after negotiation timer is up\n  void postNegotiationCallback();\n\n  // Getter of gxf_message_relay_callback_func_\n  std::function<void(void)> & getGxfMessageRelayCallbackFunc();\n\n  // Setter for gxf_vault_ptr_ pointing to a vault component in the running graph\n  void setVaultPointer(void * gxf_vault_ptr);\n\n  // Setter for gxf_vault_ptr_ pointing to a message relay component in the running graph\n  void setMessageRelayPointer(void * gxf_message_relay_ptr);\n\n  // Start a timer for polling data queued in the vault component in the running graph\n  void startGxfVaultPeriodicPollingTimer();\n\n  void enableNitrosPublisherWaitable();\n\n  // Publish the given handle for the negotiated format and compatible format\n  void publish(const int64_t handle);\n\n  // Publish the given handle for the negotiated format and compatible format while\n  // revising the enclosed timestamp components, if any, with the time set in the\n  // provided ROS header.\n  // This is mainly for benchmarking/testing purposes.\n  void publish(const int64_t handle, const std_msgs::msg::Header & ros_header);\n\n  // Publish the given Nitros-typed message for the negotiated format and compatible format\n  // while revising the enclosed timestamp components, if any, with the time set in the\n  // provided ROS header.\n  // This is mainly for benchmarking/testing purposes.\n  void publish(NitrosTypeBase & base_msg, const std_msgs::msg::Header & ros_header);\n\n  // Publish the given Nitros-typed message for the negotiated format and compatible format\n  void publish(NitrosTypeBase & base_msg);\n\n  // Extract message entities form Vault or MessageRelay in the running graph\n  void extractMessagesFromGXF();\n\nprivate:\n  // The callback function that is invoked when a MessageRelay component receives\n  // a message in the GXF graph.\n  void gxfMessageRelayCallback();\n\n  // The vault data polling timer\n  rclcpp::TimerBase::SharedPtr gxf_vault_periodic_polling_timer_;\n\n  // Negotiated publisher\n  std::shared_ptr<negotiated::NegotiatedPublisher> negotiated_pub_;\n\n  // A publisher for publishing data to the base topic\n  std::shared_ptr<rclcpp::PublisherBase> compatible_pub_{nullptr};\n\n  // A pointer to the associated vault component for retrieving data from the running graph\n  nvidia::gxf::Vault * gxf_vault_ptr_{nullptr};\n\n  // A pointer to the associated MessageRelay component for retrieving data from the running graph\n  nvidia::isaac_ros::MessageRelay * gxf_message_relay_ptr_{nullptr};\n\n  // The function pointer that is used by a MessageRelay component to call\n  // the actual callback function (gxfMessageRelayCallback)\n  std::function<void(void)> gxf_message_relay_callback_func_;\n\n  // NitrosPublisherWaitable waitable_;\n  // rcl_guard_condition_t guard_condition_;\n  std::shared_ptr<NitrosPublisherWaitable> waitable_;\n\n  // A CUDA stream for each publisher that can add stream handle and event object that can\n  // be used for synchronization and work scheduling for downstream nodes (type adapters)\n  // This should probably be a CUDA stream pool.\n  // Should this be allocated on the heap ?\n  cudaStream_t cuda_stream_ = 0;\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__NITROS_PUBLISHER_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher_subscriber_base.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__NITROS_PUBLISHER_SUBSCRIBER_BASE_HPP_\n#define ISAAC_ROS_NITROS__NITROS_PUBLISHER_SUBSCRIBER_BASE_HPP_\n\n#include <algorithm>\n#include <chrono>\n#include <limits>\n#include <map>\n#include <memory>\n#include <mutex>\n#include <queue>\n#include <string>\n#include <vector>\n\n#include \"extensions/gxf_optimizer/exporter/graph_types.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/timestamp.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_manager.hpp\"\n\n#include \"diagnostic_msgs/msg/diagnostic_array.hpp\"\n#include \"diagnostic_msgs/msg/key_value.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nnamespace\n{\nconstexpr float const kSecondsToMicroseconds = 1000000;\nconstexpr uint64_t const kMicrosecondsToNanoseconds = 1000;\nconstexpr uint64_t const kMillisecondsToMicroseconds = 1000;\nconst char * const nvidiaID = \"nvidia\";\nconstexpr int64_t const kDropWarnTimeoutSeconds = 5;\n}  // namespace\n\n// Enum for specifying vairous types of Nitros publishers/subscribers\nenum NitrosPublisherSubscriberType\n{\n  NOOP = 0,\n  NON_NEGOTIATED,\n  NEGOTIATED,\n};\n\n// Configurations for a Nitros publisher/subscriber\nstruct NitrosPublisherSubscriberConfig\n{\n  NitrosPublisherSubscriberType type{NitrosPublisherSubscriberType::NOOP};\n  rclcpp::QoS qos{rclcpp::QoS(1)};\n  std::string compatible_data_format{\"\"};\n  std::string topic_name{\"\"};\n\n  // Pin this pub/sub to use only the compatible format for negotiation\n  bool use_compatible_format_only{false};\n\n  // Enable NitrosNode to adjust the compatible format after an unsuccessful\n  // negotiation. The compatible format is adjusted if the existing compatible\n  // format, together with other selected data formats in the same pub/sub\n  // group, cannot form a valid data format combination.\n  bool use_flexible_compatible_format{true};\n\n  // ComponentKey whose frame_id this pub/sub should refer to\n  std::string frame_id_source_key{\"\"};\n\n  // User-defined callback function\n  std::function<void(const gxf_context_t, NitrosTypeBase &)> callback{nullptr};\n};\n\n// Configurations for a Nitros diagnostics\nstruct NitrosDiagnosticsConfig\n{\n  // diagnostics toggle\n  bool enable_diagnostics{false};\n\n  // corresponds to launch arguments\n  bool enable_all_diagnostics{false};\n  bool enable_node_time_diagnostics{false};\n  bool enable_msg_time_diagnostics{false};\n  bool enable_increasing_msg_time_diagnostics{false};\n\n  // enable basic diagnostics for all topics, triggered by an environment variable\n  bool enable_all_topic_diagnostics{false};\n\n  // Rate (Hz) at which to publish diagnostics to a ROS topic\n  float diagnostics_publish_rate{1.0};\n\n  // Window size of the mean filter in terms of number of messages received\n  int filter_window_size{100};\n\n  // Map of topic name and corresponding expected time difference between\n  // messages in microseconds\n  std::map<std::string, int> topic_name_expected_dt_map;\n\n  // Tolerance for jitter from expected frame rate in microseconds\n  int jitter_tolerance_us{5000};\n};\n\nusing NitrosPublisherSubscriberConfigMap =\n  std::map<gxf::optimizer::ComponentKey, NitrosPublisherSubscriberConfig>;\n\n// Nitros publisher/subscriber base class\nclass NitrosPublisherSubscriberBase\n{\npublic:\n  // Constructor\n  NitrosPublisherSubscriberBase(\n    rclcpp::Node & node,\n    std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n    const gxf::optimizer::ComponentInfo & gxf_component_info,\n    const std::vector<std::string> & supported_data_formats,\n    const NitrosPublisherSubscriberConfig & config)\n  : node_(node), nitros_type_manager_(nitros_type_manager),\n    gxf_component_info_(gxf_component_info),\n    supported_data_formats_(supported_data_formats), config_(config) {}\n\n  NitrosPublisherSubscriberBase(\n    rclcpp::Node & node, const gxf_context_t context,\n    std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n    const gxf::optimizer::ComponentInfo & gxf_component_info,\n    const std::vector<std::string> & supported_data_formats,\n    const NitrosPublisherSubscriberConfig & config)\n  : node_(node), context_(context),\n    nitros_type_manager_(nitros_type_manager),\n    gxf_component_info_(gxf_component_info),\n    supported_data_formats_(supported_data_formats), config_(config) {}\n\n  const NitrosPublisherSubscriberConfig getConfig() const {return config_;}\n\n  // Getter for the GXF component info\n  gxf::optimizer::ComponentInfo getComponentInfo()\n  {\n    return gxf_component_info_;\n  }\n\n  // Getter for the negotiated data format\n  std::string getNegotiatedDataFormat() const\n  {\n    return negotiated_data_format_;\n  }\n\n  // Getter for the compatible data format\n  std::string getCompatibleDataFormat() const\n  {\n    return config_.compatible_data_format;\n  }\n\n  // Setter for the compatible data format\n  void setCompatibleDataFormat(const std::string & compatible_data_format)\n  {\n    config_.compatible_data_format = compatible_data_format;\n  }\n\n  // Get the negotiated data format or the compatible format if negotiation\n  // failed\n  std::string getFinalDataFormat() const\n  {\n    if (negotiated_data_format_.empty()) {\n      return config_.compatible_data_format;\n    }\n    return negotiated_data_format_;\n  }\n\n  // Getter for the GXF context\n  gxf_context_t getContext() {return context_;}\n\n  // Setter for the parent GXF context\n  void setContext(const gxf_context_t context) {context_ = context;}\n\n  // Setter for the frame_id passthrough map\n  void setFrameIdMap(\n    std::shared_ptr<std::map<gxf::optimizer::ComponentKey, std::string>>\n    frame_id_map_ptr)\n  {\n    frame_id_map_ptr_ = frame_id_map_ptr;\n  }\n\n  uint64_t getTimestamp(NitrosTypeBase & base_msg) const\n  {\n    auto msg_entity = nvidia::gxf::Entity::Shared(context_, base_msg.handle);\n    if (msg_entity) {\n      auto timestamp = msg_entity->get<nvidia::gxf::Timestamp>();\n      if (timestamp) {\n        return timestamp.value()->acqtime;\n      }\n    } else {\n      RCLCPP_FATAL(\n        node_.get_logger(),\n        \"[NitrosPublisherSubscriberBase] Failed to resolve entity \"\n        \"(eid=%ld) (error=%s)\",\n        base_msg.handle, GxfResultStr(msg_entity.error()));\n      return 0;\n    }\n\n    RCLCPP_DEBUG(\n      node_.get_logger(),\n      \"[NitrosPublisherSubscriberBase] Failed to get timestamp from a NITROS\"\n      \" message (eid=%ld)\",\n      base_msg.handle);\n    return 0;\n  }\n\n  // Start negotiation\n  virtual void start() = 0;\n\n  // To be called after negotiation timer is up\n  virtual void postNegotiationCallback() = 0;\n\n  void throwOnUnsupportedCompatibleDataFormat()\n  {\n    if (std::find(\n        supported_data_formats_.begin(), supported_data_formats_.end(),\n        config_.compatible_data_format) == supported_data_formats_.end())\n    {\n      std::stringstream error_msg;\n      error_msg\n        << \"[NitrosPublisherSubscriberBase] Specified compatible format \"\n        << \"\\\"\" << config_.compatible_data_format.c_str()\n        << \"\\\" was not listed in \"\n        << \"the supported format list: [\";\n      for (size_t i = 0; i < supported_data_formats_.size(); i++) {\n        if (i > 0) {\n          error_msg << \", \";\n        }\n        error_msg << supported_data_formats_[i].c_str();\n      }\n      error_msg << \"]\";\n\n      // Fix format security issue by using static format string\n      RCLCPP_ERROR(\n        node_.get_logger(),\n        \"[NitrosPublisherSubscriberBase] Specified compatible format \\\"%s\\\" was\"\n        \" not listed in the supported format list: %s\",\n        config_.compatible_data_format.c_str(),\n        error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n\n  // update node (pub/sub) time queue which is used for frame rate calculation\n  void updateNodeTimeQueue(const int64_t timestamp_diff_node_us)\n  {\n    interarrival_time_queue_node_.push(timestamp_diff_node_us);\n    sum_interarrival_time_node_ += timestamp_diff_node_us;\n    if (static_cast<int>(interarrival_time_queue_node_.size()) >\n      diagnostics_config_.filter_window_size)\n    {\n      sum_interarrival_time_node_ -= interarrival_time_queue_node_.front();\n      interarrival_time_queue_node_.pop();\n    }\n  }\n\n  // Update node (pub/sub) time diagnostics\n  bool updateNodeTimeDiagnostics(const int64_t timestamp_diff_node_us)\n  {\n    bool error_found = false;\n    // calculate difference between time between msgs(using node clock)\n    // and expected time between msgs\n    int abs_jitter_node =\n      std::abs(\n      timestamp_diff_node_us -\n      diagnostics_config_.topic_name_expected_dt_map[topic_name_]);\n    if (abs_jitter_node > diagnostics_config_.jitter_tolerance_us) {\n      // Increment jitter outlier count\n      num_jitter_outliers_node_++;\n      RCLCPP_DEBUG(\n        node_.get_logger(),\n        \"[NitrosDiagnostics Node Time]\"\n        \" Difference of time between messages(%li) and expected time between\"\n        \" messages(%i) is out of tolerance(%i) by %i for topic %s. Units \"\n        \"are microseconds.\",\n        timestamp_diff_node_us,\n        diagnostics_config_.topic_name_expected_dt_map[topic_name_],\n        diagnostics_config_.jitter_tolerance_us,\n        abs_jitter_node, topic_name_.c_str());\n    }\n    // Update max abs jitter\n    max_abs_jitter_node_ = std::max(max_abs_jitter_node_, abs_jitter_node);\n\n    jitter_queue_node_.push(abs_jitter_node);\n    sum_jitter_node_ += abs_jitter_node;\n    if (static_cast<int>(jitter_queue_node_.size()) >\n      diagnostics_config_.filter_window_size)\n    {\n      sum_jitter_node_ -= jitter_queue_node_.front();\n      jitter_queue_node_.pop();\n    }\n    return error_found;\n  }\n\n  void updateMsgTimeQueue(const int64_t timestamp_diff_msg_us)\n  {\n    interarrival_time_queue_msg_.push(timestamp_diff_msg_us);\n    sum_interarrival_time_msg_ += timestamp_diff_msg_us;\n    if (static_cast<int>(interarrival_time_queue_msg_.size()) >\n      diagnostics_config_.filter_window_size)\n    {\n      sum_interarrival_time_msg_ -= interarrival_time_queue_msg_.front();\n      interarrival_time_queue_msg_.pop();\n    }\n  }\n\n  bool updateMsgTimeDiagnostics(const int64_t timestamp_diff_msg_us)\n  {\n    bool error_found = false;\n    // calculate difference between time between msgs(using msg clock)\n    // and expected time between msgs\n    int abs_jitter_msg =\n      std::abs(\n      static_cast<int>(timestamp_diff_msg_us) -\n      diagnostics_config_.topic_name_expected_dt_map[topic_name_]);\n    if (abs_jitter_msg > diagnostics_config_.jitter_tolerance_us) {\n      // Increment jitter outlier count\n      num_jitter_outliers_msg_++;\n      frames_dropped_since_last_pub_++;\n      prev_drop_ts_ = clock_.now();\n      RCLCPP_DEBUG(\n        node_.get_logger(),\n        \"[NitrosDiagnostics Message Timestamp]\"\n        \" Difference of time between messages(%lu) and expected \"\n        \"time between\"\n        \" messages(%i) is out of tolerance(%i) by %i for topic %s. \"\n        \"Units are microseconds.\",\n        timestamp_diff_msg_us,\n        diagnostics_config_.topic_name_expected_dt_map[topic_name_],\n        diagnostics_config_.jitter_tolerance_us,\n        abs_jitter_msg, topic_name_.c_str());\n    }\n\n    // Update max abs jitter\n    max_abs_jitter_msg_ = std::max(max_abs_jitter_msg_, abs_jitter_msg);\n    jitter_queue_msg_.push(abs_jitter_msg);\n\n    sum_jitter_msg_ += abs_jitter_msg;\n    if (static_cast<int>(jitter_queue_msg_.size()) >\n      diagnostics_config_.filter_window_size)\n    {\n      sum_jitter_msg_ -= jitter_queue_msg_.front();\n      jitter_queue_msg_.pop();\n    }\n\n    if (prev_drop_ts_ != std::chrono::steady_clock::time_point::min()) {\n      auto time_since_drop =\n        std::chrono::duration_cast<std::chrono::seconds>(clock_.now() - prev_drop_ts_);\n      if (time_since_drop.count() < kDropWarnTimeoutSeconds) {\n        error_found = true;\n        status_vec_[0].level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;\n        frames_dropped_since_last_pub_ = 0;\n        update_status_message(status_vec_[0], \"FRAME DROP DETECTED\");\n      }\n    }\n    return error_found;\n  }\n\n  bool updateIncreasingMsgTimeDiagnostics(const uint64_t current_timestamp_msg_us)\n  {\n    bool error_found = false;\n    // Check if message timestamp is increasing\n    if (current_timestamp_msg_us <= prev_timestamp_msg_us_) {\n      // Increment non increasing message count\n      num_non_increasing_msg_++;\n      prev_noninc_msg_ts_ = clock_.now();\n      RCLCPP_WARN(\n        node_.get_logger(),\n        \"[NitrosDiagnostics Message Timestamp Non Increasing]\"\n        \" Message timestamp is not increasing. Current timestamp: \"\n        \"%lu, Previous timestamp: %lu\"\n        \" for topic %s. Units are microseconds.\",\n        current_timestamp_msg_us, prev_timestamp_msg_us_, topic_name_.c_str());\n    }\n    if (prev_noninc_msg_ts_ != std::chrono::steady_clock::time_point::min()) {\n      auto time_since_noninc =\n        std::chrono::duration_cast<std::chrono::seconds>(clock_.now() - prev_noninc_msg_ts_);\n      if (time_since_noninc.count() < kDropWarnTimeoutSeconds) {\n        error_found = true;\n        status_vec_[0].level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;\n        frames_dropped_since_last_pub_ = 0;\n        update_status_message(status_vec_[0], \"NONINCREASING TIMESTAMP\");\n      }\n    }\n    return error_found;\n  }\n\n\n// Update diagnostics numbers. To be called in nitros Subscriber and Publisher\n  void updateDiagnostics(uint64_t msg_timestamp_ns)\n  {\n    // Mutex lock to prevent simultaneous access of common parameters\n    // used by updateDiagnostics() and publishDiagnostics()\n    const std::lock_guard<std::mutex> lock(nitros_diagnostics_mutex_);\n    // NITROS diagnostics checks message intervals both using the node clock\n    // and the message timestamp.\n    // All variables name _node refers to the node timestamp checks.\n    // All variables name _msg refers to the message timestamp checks.\n    status_vec_[0].message = \"\";\n    bool error_found = false;\n\n    // Get the current timestamps in microseconds\n    uint64_t current_timestamp_msg_us =\n      msg_timestamp_ns / kMicrosecondsToNanoseconds;\n    uint64_t current_timestamp_node_us = std::chrono::duration_cast<std::chrono::microseconds>(\n      clock_.now().time_since_epoch()).count();\n\n    // we can only calculate frame rate after 2 messages have been received\n    if (prev_timestamp_node_us_ != std::numeric_limits<uint64_t>::min()) {\n      const int64_t timestamp_diff_node_us = current_timestamp_node_us - prev_timestamp_node_us_;\n      updateNodeTimeQueue(timestamp_diff_node_us);\n      if (diagnostics_config_.enable_node_time_diagnostics) {\n        error_found |= updateNodeTimeDiagnostics(timestamp_diff_node_us);\n      }\n    }\n\n    rclcpp::Time time_from_node = node_.get_clock()->now();\n    uint64_t ros_node_system_time_us = time_from_node.nanoseconds() / kMicrosecondsToNanoseconds;\n\n    const uint64_t latency_wrt_current_timestamp_node_ms =\n      (ros_node_system_time_us - current_timestamp_msg_us) / kMillisecondsToMicroseconds;\n\n    if (prev_timestamp_msg_us_ != std::numeric_limits<uint64_t>::min()) {\n      const int64_t timestamp_diff_msg_us = current_timestamp_msg_us - prev_timestamp_msg_us_;\n      updateMsgTimeQueue(timestamp_diff_msg_us);\n      // Do the same checks as above, but for message timestamp\n      if (diagnostics_config_.enable_msg_time_diagnostics) {\n        error_found |= updateMsgTimeDiagnostics(timestamp_diff_msg_us);\n      }\n      if (diagnostics_config_.enable_increasing_msg_time_diagnostics) {\n        error_found |= updateIncreasingMsgTimeDiagnostics(current_timestamp_msg_us);\n      }\n    }\n\n    prev_timestamp_node_us_ = current_timestamp_node_us;\n    prev_timestamp_msg_us_ = current_timestamp_msg_us;\n\n    // calculate key values for diagnsotics status\n    if (sum_interarrival_time_node_ != 0) {\n      frame_rate_node_ = kSecondsToMicroseconds /\n        (static_cast<float>(sum_interarrival_time_node_) /\n        interarrival_time_queue_node_.size());\n    } else {\n      frame_rate_node_ = 0.0;\n    }\n\n    if (jitter_queue_node_.size() != 0) {\n      mean_abs_jitter_node_ = sum_jitter_node_ / jitter_queue_node_.size();\n    } else {\n      mean_abs_jitter_node_ = 0;\n    }\n\n    if (sum_interarrival_time_msg_ != 0) {\n      frame_rate_msg_ = kSecondsToMicroseconds /\n        (static_cast<float>(sum_interarrival_time_msg_) /\n        interarrival_time_queue_msg_.size());\n    } else {\n      frame_rate_msg_ = 0.0;\n    }\n    message_latency_msg_ms_ = latency_wrt_current_timestamp_node_ms;\n\n    if (jitter_queue_msg_.size() != 0) {\n      mean_abs_jitter_msg_ = sum_jitter_msg_ / jitter_queue_msg_.size();\n    } else {\n      mean_abs_jitter_msg_ = 0;\n    }\n\n    // frame dropping warnings\n    if (!error_found) {\n      status_vec_[0].level = diagnostic_msgs::msg::DiagnosticStatus::OK;\n      status_vec_[0].message = \"OK\";\n    }\n    outdated_msg_ = false;\n  }\n\n  // Function to publish diagnostics to a ROS topic\n  void publishDiagnostics()\n  {\n    // Mutex lock to prevent simultaneous access of common parameters\n    // used by updateDiagnostics() and publishDiagnostics()\n    const std::lock_guard<std::mutex> lock(nitros_diagnostics_mutex_);\n\n    std::vector<diagnostic_msgs::msg::KeyValue> values;\n    // publish diagnostics stale if message has not been updated since the last call\n    if (outdated_msg_) {\n      status_vec_[0].level = diagnostic_msgs::msg::DiagnosticStatus::STALE;\n      status_vec_[0].message = \"DIAGNOSTICS STALE\";\n    } else {\n      values.push_back(\n        diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()\n        .key(\"num_non_increasing_msg\")\n        .value(std::to_string(num_non_increasing_msg_)));\n      values.push_back(\n        diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()\n        .key(\"num_jitter_outliers_msg\")\n        .value(std::to_string(num_jitter_outliers_msg_)));\n      values.push_back(\n        diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()\n        .key(\"num_jitter_outliers_node\")\n        .value(std::to_string(num_jitter_outliers_node_)));\n      values.push_back(\n        diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()\n        .key(\"max_abs_jitter_msg\")\n        .value(std::to_string(max_abs_jitter_msg_)));\n      values.push_back(\n        diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()\n        .key(\"max_abs_jitter_node\")\n        .value(std::to_string(max_abs_jitter_node_)));\n      values.push_back(\n        diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()\n        .key(\"mean_abs_jitter_msg\")\n        .value(std::to_string(mean_abs_jitter_msg_)));\n      values.push_back(\n        diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()\n        .key(\"mean_abs_jitter_node\")\n        .value(std::to_string(mean_abs_jitter_node_)));\n      values.push_back(\n        diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()\n        .key(\"frame_rate_msg\")\n        .value(std::to_string(frame_rate_msg_)));\n      values.push_back(\n        diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()\n        .key(\"current_delay_from_realtime_ms\")\n        .value(std::to_string(message_latency_msg_ms_)));\n      values.push_back(\n        diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()\n        .key(\"frame_rate_node\")\n        .value(std::to_string(frame_rate_node_)));\n      values.push_back(\n        diagnostic_msgs::build<diagnostic_msgs::msg::KeyValue>()\n        .key(\"total_dropped_frames\")\n        .value(std::to_string(num_jitter_outliers_msg_)));\n    }\n    status_vec_[0].values = values;\n\n    // timestamp from std::chrono\n    uint64_t time = std::chrono::duration_cast<std::chrono::nanoseconds>(\n      clock_.now() - t_start_)\n      .count();\n    uint32_t time_seconds =\n      time / (kSecondsToMicroseconds * kMicrosecondsToNanoseconds);\n    uint32_t time_ns = time - time_seconds;\n\n    diagnostic_msgs::msg::DiagnosticArray diagnostic_msg;\n    std_msgs::msg::Header header;\n    builtin_interfaces::msg::Time timestamp;\n    timestamp.sec = time_seconds;\n    timestamp.nanosec = time_ns;\n    header.stamp = timestamp;\n    diagnostic_msg.header = header;\n    diagnostic_msg.status = status_vec_;\n\n    diagnostic_publisher_->publish(diagnostic_msg);\n    outdated_msg_ = true;\n  }\n\n  // Initialize diagnostics variables\n  void initDiagnostics()\n  {\n    topic_name_ = config_.topic_name;\n    diagnostic_msgs::msg::DiagnosticStatus topic_status;\n    topic_status.name = node_.get_name();\n    topic_status.name.append(node_.get_namespace());\n    topic_status.name.append(\"/\");\n    topic_status.name.append(config_.topic_name);\n    topic_status.hardware_id = nvidiaID;\n    topic_status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;\n    topic_status.message = \"UNDEFINED STATE\";\n    status_vec_.push_back(topic_status);\n    frames_dropped_since_last_pub_ = 0;\n\n    t_start_ = clock_.now();\n\n    // Initialize varibles to min and zero as a flag to detect no messages have\n    // been received\n    prev_drop_ts_ = std::chrono::steady_clock::time_point::min();\n    prev_noninc_msg_ts_ = std::chrono::steady_clock::time_point::min();\n    prev_timestamp_node_us_ = std::numeric_limits<uint64_t>::min();\n    prev_timestamp_msg_us_ = std::numeric_limits<uint64_t>::min();\n    sum_interarrival_time_node_ = 0;\n    sum_interarrival_time_msg_ = 0;\n    sum_jitter_node_ = 0;\n    sum_jitter_msg_ = 0;\n    max_abs_jitter_node_ = 0;\n    max_abs_jitter_msg_ = 0;\n    num_jitter_outliers_node_ = 0;\n    num_jitter_outliers_msg_ = 0;\n    num_non_increasing_msg_ = 0;\n    message_latency_msg_ms_ = 0;\n    outdated_msg_ = true;\n\n    diagnostic_publisher_ =\n      node_.create_publisher<diagnostic_msgs::msg::DiagnosticArray>(\n      \"/diagnostics\", 10);\n    diagnostics_publisher_timer_ = node_.create_wall_timer(\n      std::chrono::milliseconds(\n        static_cast<int>(\n          1000 / diagnostics_config_.diagnostics_publish_rate)),\n      [this]() -> void {publishDiagnostics();});\n  }\n\nprotected:\n  // The parent ROS 2 node\n  rclcpp::Node & node_;\n\n  // The parent GXF context\n  gxf_context_t context_ = nullptr;\n\n  // Nitros type manager\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager_;\n\n  // The info of the GXF component that's associated to this Nitros\n  // publisher/subscriber\n  gxf::optimizer::ComponentInfo gxf_component_info_;\n\n  // Supported data formats\n  std::vector<std::string> supported_data_formats_;\n\n  // Configurations for creating the corresponding publisher/subscriber\n  NitrosPublisherSubscriberConfig config_;\n\n  // Negotiated data format\n  std::string negotiated_data_format_;\n\n  // Frame ID map\n  std::shared_ptr<std::map<gxf::optimizer::ComponentKey, std::string>>\n  frame_id_map_ptr_;\n\n  // Mutex lock to prevent simultaneous access of common parameters\n  // used by updateDiagnostics() and publishDiagnostics()\n  std::mutex nitros_diagnostics_mutex_;\n\n  // NITROS diagnostics variables\n  // Configurations for a Nitros diagnostics\n  NitrosDiagnosticsConfig diagnostics_config_;\n\n  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr\n    diagnostic_publisher_;\n\n  // diagnostics message elements\n  std::vector<diagnostic_msgs::msg::DiagnosticStatus> status_vec_;\n\n  // Clock object used to retrived current timestamps\n  std::chrono::steady_clock clock_;\n  std::chrono::steady_clock::time_point t_start_;\n\n  // Queue to store time between messages to implement windowed mean filter\n  std::queue<int> interarrival_time_queue_node_;\n  std::queue<uint64_t> interarrival_time_queue_msg_;\n\n  // Queue to store message jitter to implement windowed mean filter\n  // Jitter is the difference between the time between msgs(dt)\n  // calculated from fps specified in NITROS diagnostics ROS param\n  // and measured using node clock\n  std::queue<int> jitter_queue_node_;\n  std::queue<int> jitter_queue_msg_;\n\n\n  // Sum of the message interarrival times received on this topic within the\n  // configured window\n  int64_t sum_interarrival_time_node_;\n  uint64_t sum_interarrival_time_msg_;\n\n  // Sum of the message jitter on this topic within the configured window\n  int64_t sum_jitter_node_;\n  int sum_jitter_msg_;\n\n  // Max absolute jitter\n  int max_abs_jitter_node_;\n  int max_abs_jitter_msg_;\n\n  // Number of messages outside the jitter tolerance\n  uint64_t num_jitter_outliers_node_;\n  uint64_t num_jitter_outliers_msg_;\n\n  // Number of non-increasing messages\n  uint64_t num_non_increasing_msg_;\n\n  // Number of dropped frames without error warnings\n  // idk if we need a full 64 bit integer here?\n  uint64_t frames_dropped_since_last_pub_;\n\n  // Prev timestamp stored for calculating frame rate\n  // Prev node timstamp\n  std::chrono::time_point<std::chrono::steady_clock> prev_drop_ts_, prev_noninc_msg_ts_;\n  // Prev message timestamp in microseconds\n  uint64_t prev_timestamp_msg_us_, prev_timestamp_node_us_;\n\n  // NITROS diagnostics publisher timer\n  rclcpp::TimerBase::SharedPtr diagnostics_publisher_timer_;\n\n  // data tracking variables for diagnostics\n  std::string topic_name_;\n  double frame_rate_node_, frame_rate_msg_;\n  double message_latency_msg_ms_;\n  int32_t mean_abs_jitter_node_, mean_abs_jitter_msg_;\n  bool outdated_msg_;\n\n  void update_status_message(diagnostic_msgs::msg::DiagnosticStatus & status, std::string update)\n  {\n    if (status.message.empty()) {\n      status.message = update;\n    } else {\n      status.message.append(\", \").append(update);\n    }\n  }\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__NITROS_PUBLISHER_SUBSCRIBER_BASE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher_subscriber_group.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__NITROS_PUBLISHER_SUBSCRIBER_GROUP_HPP_\n#define ISAAC_ROS_NITROS__NITROS_PUBLISHER_SUBSCRIBER_GROUP_HPP_\n\n#include <map>\n#include <memory>\n#include <string>\n#include <unordered_set>\n#include <vector>\n\n#include \"extensions/gxf_optimizer/exporter/graph_types.hpp\"\n\n#include \"isaac_ros_nitros/nitros_publisher.hpp\"\n#include \"isaac_ros_nitros/nitros_subscriber.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nusing gxf::optimizer::ComponentKey;\n\n// A pub/sub group that consists of associated Nitros publishers and subscribers\nclass NitrosPublisherSubscriberGroup\n{\npublic:\n  // Constructor\n  NitrosPublisherSubscriberGroup(\n    rclcpp::Node & node,\n    const gxf_context_t context,\n    std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n    const gxf::optimizer::GraphIOGroupSupportedDataTypesInfo & gxf_io_supported_data_formats_info,\n    const NitrosPublisherSubscriberConfigMap & nitros_pub_sub_configs,\n    const std::shared_ptr<std::map<ComponentKey, std::string>> frame_id_map_ptr,\n    const NitrosDiagnosticsConfig & diagnostics_config);\n\n  // Find the corresponding Nitros publisher of the given component\n  std::shared_ptr<NitrosPublisher> findNitrosPublisher(\n    const gxf::optimizer::ComponentInfo & comp_info);\n\n  // Find the corresponding Nitros subscriber of the given component\n  std::shared_ptr<NitrosSubscriber> findNitrosSubscriber(\n    const gxf::optimizer::ComponentInfo & comp_info);\n\n  // Find the Nitros subscriber that holds the given negotiated subscriber\n  std::shared_ptr<NitrosSubscriber> findNitrosSubscriber(\n    const std::shared_ptr<negotiated::NegotiatedSubscription> negotiated_sub);\n\n  // Start negotiation\n  void start();\n\n  // Get negotiated configurations\n  gxf::optimizer::GraphIOGroupDataTypeConfigurations getDataFormatConfigurations();\n\n  // Negotiated publisher's callback for informing its upstream subscriber changes in its\n  // supported data formats when receiving an update from its downstream negotiated subscribers\n  void updateUpstreamSubscriberDownstreamSupportedFormatsCallback(\n    const std::map<std::string, negotiated::detail::SupportedTypeInfo> & key_to_supported_types,\n    const std::shared_ptr<std::map<negotiated::detail::PublisherGid,\n    std::vector<std::string>>> & negotiated_subscription_type_gids,\n    const std::unordered_set<\n      std::shared_ptr<\n        negotiated::detail::UpstreamNegotiatedSubscriptionHandle>> &\n    upstream_negotiated_subscriptions,\n    const negotiated_interfaces::msg::SupportedTypes & downstream_types_to_add,\n    const negotiated_interfaces::msg::SupportedTypes & downstream_types_to_remove,\n    negotiated::detail::PublisherGid gid_key,\n    gxf::optimizer::ComponentInfo comp_info);\n\n  // Get the upstream component's supported data formats mapped from the given data format of\n  // the given downstream component\n  std::unordered_set<std::string> mapDownstreamToUpstreamDataFormats(\n    const gxf::optimizer::ComponentInfo & downstream_comp_info,\n    const gxf::optimizer::ComponentInfo & upstream_comp_info,\n    const std::string & downstream_data_format) const;\n\n  // Get the downstream component's supported data format mapped from the given data format of\n  // the given upstream component\n  std::unordered_set<std::string> mapUpstreamToDownstreamDataFormats(\n    const gxf::optimizer::ComponentInfo & upstream_comp_info,\n    const gxf::optimizer::ComponentInfo & downstream_comp_info,\n    const std::string & upstream_data_format) const;\n\n  // Negotiated publisher's callback invoked for running a negotiation algorithm\n  std::vector<negotiated_interfaces::msg::SupportedType> publisherNegotiationCallback(\n    const std::map<negotiated::detail::PublisherGid,\n    std::vector<std::string>> & negotiated_sub_gid_to_keys,\n    const std::map<std::string, negotiated::detail::SupportedTypeInfo> & key_to_supported_types,\n    const std::unordered_set<\n      std::shared_ptr<\n        negotiated::detail::UpstreamNegotiatedSubscriptionHandle>> &\n    upstream_negotiated_subscriptions,\n    size_t maximum_solutions,\n    gxf::optimizer::ComponentInfo pub_info);\n\n  // Getter for all the Nitros publishers\n  std::vector<std::shared_ptr<NitrosPublisher>> getNitrosPublishers();\n\n  // Getter for all the Nitros subscribers\n  std::vector<std::shared_ptr<NitrosSubscriber>> getNitrosSubscribers();\n\n  // Get all publisher/subscriber component infos\n  std::vector<gxf::optimizer::ComponentInfo> getAllComponentInfos() const;\n\n  // Expand \"any\" format in gxf_io_supported_data_formats_info_ with registered formats\n  void expandAnyDataFormats();\n\n  // For each component that has .use_compatible_format_only as true, remove all supported\n  // formats except for the compatible format\n  bool applyUseCompatibleFormatOnly();\n\n  // Validate if the final data format combination is valid or not.\n  // In the case that some pubs/subs have sucessful negotiation and some don't (and hence\n  // compatible formats are chosen for these pubs/subs), the resulting group format combaination\n  // may not be valid. In such a case, those pubs/subs that have use_flexible_compatible_format\n  // set as true will have their compatible formats adjusted to form a valid format combination\n  // for the group.\n  void postNegotiationAdjustCompatibleFormats();\n\n  // To be called after negotiation timer is up\n  void postNegotiationCallback();\n\nprivate:\n  // Create all Nitros subscribers in this group based on information provided in group_info_\n  void createNitrosSubscribers();\n\n  // Create all Nitros publishers in this group based on information provided in group_info_\n  // This must be called after Nitros subscribers are created (by calling createNitrosSubscribers)\n  // to correctly set the upstream subscribers for each publisher.\n  void createNitrosPublishers();\n\n  // The ROS node that holds this group\n  rclcpp::Node & node_;\n\n  // The parent GXF context\n  gxf_context_t context_;\n\n  // Nitros type manager\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager_;\n\n  // GXF graph information\n  gxf::optimizer::GraphIOGroupSupportedDataTypesInfo gxf_io_supported_data_formats_info_;\n\n  // Configurations for all Nitros publishers/subscribers\n  NitrosPublisherSubscriberConfigMap nitros_pub_sub_configs_;\n\n  // Nitros publishers and subscribers created in this group\n  std::vector<std::shared_ptr<NitrosPublisher>> nitros_pubs_;\n  std::vector<std::shared_ptr<NitrosSubscriber>> nitros_subs_;\n\n  // Frame ID map\n  std::shared_ptr<std::map<ComponentKey, std::string>> frame_id_map_ptr_;\n\n  // Configurations for a Nitros diagnostics\n  NitrosDiagnosticsConfig diagnostics_config_;\n};\n\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__NITROS_PUBLISHER_SUBSCRIBER_GROUP_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/nitros_subscriber.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__NITROS_SUBSCRIBER_HPP_\n#define ISAAC_ROS_NITROS__NITROS_SUBSCRIBER_HPP_\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#include \"gxf/std/receiver.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros/nitros_publisher_subscriber_base.hpp\"\n\n#include \"negotiated/negotiated_subscription.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Nitros subscriber that supports type adaptation/negotiation and enables hardware acceleration\nclass NitrosSubscriber : public NitrosPublisherSubscriberBase\n{\npublic:\n  // Constructor\n  NitrosSubscriber(\n    rclcpp::Node & node,\n    const gxf_context_t context,\n    std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n    const gxf::optimizer::ComponentInfo & gxf_component_info,\n    const std::vector<std::string> & supported_data_formats,\n    const NitrosPublisherSubscriberConfig & config,\n    const NitrosDiagnosticsConfig & diagnostics_config,\n    const bool use_callback_group = false);\n\n  NitrosSubscriber(\n    rclcpp::Node & node,\n    const gxf_context_t context,\n    std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n    const gxf::optimizer::ComponentInfo & gxf_component_info,\n    const std::vector<std::string> & supported_data_formats,\n    const NitrosPublisherSubscriberConfig & config);\n\n  // Constructor for creating a subscriber without an associated gxf ingress port\n  NitrosSubscriber(\n    rclcpp::Node & node,\n    const gxf_context_t context,\n    std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n    const std::vector<std::string> & supported_data_formats,\n    const NitrosPublisherSubscriberConfig & config,\n    const NitrosDiagnosticsConfig & diagnostics_config);\n\n  // Getter for the negotiated_sub_\n  std::shared_ptr<negotiated::NegotiatedSubscription> getNegotiatedSubscriber();\n\n  // Add a supported data format to the underlying negotiated subscriber\n  void addSupportedDataFormat(\n    const std::string & data_format,\n    const double weight);\n\n  // Start negotiation\n  void start();\n\n  void setIsGxfRunning(const bool is_gxf_running);\n\n  // Create a compatible subscriber\n  void createCompatibleSubscriber();\n\n  // To be called after negotiation timer is up\n  void postNegotiationCallback();\n\n  // Set the gxf_receiver_ptr_ pointing to a receiver component in the running graph\n  void setReceiverPointer(void * gxf_receiver_ptr);\n\n  // Set the frame drop policy for the underlying GXF receiver\n  void setReceiverPolicy(const size_t policy);\n\n  // Set the capacity for the underlying GXF receiver\n  void setReceiverCapacity(const size_t capacity);\n\n  // Push an entity into the corresponding receiver in the underlying graph\n  bool pushEntity(const int64_t eid, bool should_block = false);\n\n  // The subscriber callback\n  void subscriberCallback(\n    NitrosTypeBase & msg_base,\n    const std::string data_format_name);\n\nprivate:\n  // Only either of the following two subscribers will be active after negotiation at runtime\n\n  // A negotiated subscriber for receiving data from a negotiated topic channel\n  std::shared_ptr<negotiated::NegotiatedSubscription> negotiated_sub_;\n\n  // A subscriber for receiving data from the base topic\n  std::shared_ptr<rclcpp::SubscriptionBase> compatible_sub_{nullptr};\n\n  std::shared_ptr<rclcpp::CallbackGroup> callback_group_;\n\n  // A flag to specifiy if this subscriber is associated with a gxf receiver\n  bool use_gxf_receiver_{true};\n\n  // A pinter to the associated receiver component for sending data into the running graph\n  nvidia::gxf::Receiver * gxf_receiver_ptr_{nullptr};\n\n  // A flag to specify if the underlying GXF graph has been running\n  bool is_gxf_running_{false};\n\n  // A flag to specify if a callback group should be used in this NITROS subscriber\n  // If enabled, different NITROS sbuscriber callbacks can be executed in parallel\n  // and there is only one callback instance executed at a time in each NITROS subscriber\n  bool use_callback_group_{false};\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__NITROS_SUBSCRIBER_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_empty.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__TYPES__NITROS_EMPTY_HPP_\n#define ISAAC_ROS_NITROS__TYPES__NITROS_EMPTY_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosEmpty\n *   ROS type:    std_msgs::msg::Empty\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"std_msgs/msg/empty.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosEmpty;\n\n// Formats\nstruct nitros_empty\n{\n  using MsgT = NitrosEmpty;\n  static const inline std::string supported_type_name = \"nitros_empty\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosEmpty)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_empty)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosEmpty,\n  std_msgs::msg::Empty>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosEmpty;\n  using ros_message_type = std_msgs::msg::Empty;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosEmpty,\n  std_msgs::msg::Empty);\n\n#endif  // ISAAC_ROS_NITROS__TYPES__NITROS_EMPTY_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_format_agent.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__TYPES__NITROS_FORMAT_AGENT_HPP_\n#define ISAAC_ROS_NITROS__TYPES__NITROS_FORMAT_AGENT_HPP_\n\n#include <functional>\n#include <map>\n#include <memory>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"negotiated/negotiated_publisher.hpp\"\n#include \"negotiated/negotiated_subscription.hpp\"\n\n#include \"rclcpp/logger.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n#include \"rclcpp/serialization.hpp\"\n\n\nconstexpr char LOGGER_SUFFIX[] = \"NitrosFormatAgent\";\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nstruct NitrosFormatCallbacks\n{\n  // Publisher callbacks\n  // Create a compatible publisher for T\n  std::function<\n    void(\n      rclcpp::Node & node,\n      std::shared_ptr<rclcpp::PublisherBase> & compatible_pub,\n      const std::string & topic_name,\n      const rclcpp::QoS & qos,\n      const rclcpp::PublisherOptions & options)>\n  createCompatiblePublisherCallback{nullptr};\n\n  // Add a compatible publisher of type T to a negotiated publisher\n  std::function<\n    void(\n      rclcpp::Node & node,\n      std::shared_ptr<negotiated::NegotiatedPublisher> negotiated_pub,\n      std::shared_ptr<rclcpp::PublisherBase> compatible_pub,\n      const double weight)>\n  addCompatiblePublisherCallback{nullptr};\n\n  // Add T to a negotiated publisher as a supported format\n  std::function<\n    void(\n      rclcpp::Node & node,\n      std::shared_ptr<negotiated::NegotiatedPublisher> negotiated_pub,\n      const double weight,\n      const rclcpp::QoS & qos,\n      const rclcpp::PublisherOptions & options)>\n  addPublisherSupportedFormatCallback{nullptr};\n\n  // Publish a message of type T via a negotiated publisher\n  std::function<\n    void(\n      rclcpp::Node & node,\n      std::shared_ptr<negotiated::NegotiatedPublisher> negotiated_pub,\n      NitrosTypeBase & base_msg)>\n  negotiatedPublishCallback{nullptr};\n\n  // Publish a message of type T via a compatible publisher\n  std::function<\n    void(\n      rclcpp::Node & node,\n      std::shared_ptr<rclcpp::PublisherBase> compatible_pub,\n      NitrosTypeBase & base_msg)>\n  compatiblePublishCallback{nullptr};\n\n\n  // Subscriber callbacks\n  // Create a compatible subscriber for T\n  std::function<\n    void(\n      rclcpp::Node & node,\n      std::shared_ptr<rclcpp::SubscriptionBase> & compatible_sub,\n      const std::string & topic_name,\n      const rclcpp::QoS & qos,\n      // std::function<void(std::shared_ptr<DATA_TYPE_NAME::MsgT>)> & subscriber_callback,\n      std::function<void(NitrosTypeBase &, const std::string data_format_name)> subscriber_callback,\n      const rclcpp::SubscriptionOptions & options)>\n  createCompatibleSubscriberCallback{nullptr};\n\n  // Remove a compatible subscriber of type T from a negotiated subscriber\n  std::function<\n    void(\n      rclcpp::Node & node,\n      std::shared_ptr<negotiated::NegotiatedSubscription> negotiated_sub,\n      std::shared_ptr<rclcpp::SubscriptionBase> compatible_sub)>\n  removeCompatibleSubscriberCallback {nullptr};\n\n  // Add a compatible subscriber of type T to a negotiated subscriber\n  std::function<\n    void(\n      rclcpp::Node & node,\n      std::shared_ptr<negotiated::NegotiatedSubscription> negotiated_sub,\n      std::shared_ptr<rclcpp::SubscriptionBase> compatible_sub,\n      const double weight)>\n  addCompatibleSubscriberCallback{nullptr};\n\n  // Add T to a negotiated subscriber as a supported format\n  std::function<\n    void(\n      rclcpp::Node & node,\n      std::shared_ptr<negotiated::NegotiatedSubscription> negotiated_sub,\n      const double weight,\n      const rclcpp::QoS & qos,\n      // std::function<void(std::shared_ptr<DATA_TYPE_NAME::MsgT>)> subscriber_callback,\n      std::function<void(NitrosTypeBase &, const std::string data_format_name)> subscriber_callback,\n      const rclcpp::SubscriptionOptions & options)>\n  addSubscriberSupportedFormatCallback{nullptr};\n\n  std::function<\n    std::shared_ptr<rclcpp::SerializedMessage>(NitrosTypeBase & base_msg)>\n  convertToRosSerializedMessage{nullptr};\n\n  // Utilities\n  // Get T's extension list\n  std::function<\n    std::vector<std::pair<std::string, std::string>>()>\n  getExtensions{nullptr};\n\n  // Get the corresponding ROS type name for the format T\n  std::function<std::string()> getROSTypeName{nullptr};\n};\n\ntemplate<typename T>\nclass NitrosFormatAgent\n{\npublic:\n  // Everything in this class is static and templated\n  NitrosFormatAgent() = delete;\n\n  static NitrosFormatCallbacks GetFormatCallbacks()\n  {\n    return {\n      // createCompatiblePublisherCallback\n      std::bind(\n        &NitrosFormatAgent<T>::createCompatiblePublisherCallback,\n        std::placeholders::_1,\n        std::placeholders::_2,\n        std::placeholders::_3,\n        std::placeholders::_4,\n        std::placeholders::_5\n      ),\n\n      // addCompatiblePublisherCallback\n      std::bind(\n        &NitrosFormatAgent<T>::addCompatiblePublisherCallback,\n        std::placeholders::_1,\n        std::placeholders::_2,\n        std::placeholders::_3,\n        std::placeholders::_4\n      ),\n\n      // addPublisherSupportedFormatCallback\n      std::bind(\n        &NitrosFormatAgent<T>::addPublisherSupportedFormatCallback,\n        std::placeholders::_1,\n        std::placeholders::_2,\n        std::placeholders::_3,\n        std::placeholders::_4,\n        std::placeholders::_5\n      ),\n\n      // negotiatedPublishCallback\n      std::bind(\n        &NitrosFormatAgent<T>::negotiatedPublishCallback,\n        std::placeholders::_1,\n        std::placeholders::_2,\n        std::placeholders::_3\n      ),\n\n      // compatiblePublishCallback\n      std::bind(\n        &NitrosFormatAgent<T>::compatiblePublishCallback,\n        std::placeholders::_1,\n        std::placeholders::_2,\n        std::placeholders::_3\n      ),\n\n      // createCompatibleSubscriberCallback\n      std::bind(\n        &NitrosFormatAgent<T>::createCompatibleSubscriberCallback,\n        std::placeholders::_1,\n        std::placeholders::_2,\n        std::placeholders::_3,\n        std::placeholders::_4,\n        std::placeholders::_5,\n        std::placeholders::_6\n      ),\n\n      // removeCompatibleSubscriberCallback\n      std::bind(\n        &NitrosFormatAgent<T>::removeCompatibleSubscriberCallback,\n        std::placeholders::_1,\n        std::placeholders::_2,\n        std::placeholders::_3\n      ),\n\n      // addCompatibleSubscriberCallback\n      std::bind(\n        &NitrosFormatAgent<T>::addCompatibleSubscriberCallback,\n        std::placeholders::_1,\n        std::placeholders::_2,\n        std::placeholders::_3,\n        std::placeholders::_4\n      ),\n\n      // addSubscriberSupportedFormatCallback\n      std::bind(\n        &NitrosFormatAgent<T>::addSubscriberSupportedFormatCallback,\n        std::placeholders::_1,\n        std::placeholders::_2,\n        std::placeholders::_3,\n        std::placeholders::_4,\n        std::placeholders::_5,\n        std::placeholders::_6\n      ),\n\n      // convertToRosSerializedMessage\n      std::bind(\n        &NitrosFormatAgent<T>::convertToRosSerializedMessage,\n        std::placeholders::_1\n      ),\n\n      // getExtensions\n      std::bind(\n        &NitrosFormatAgent<T>::getExtensions\n      ),\n\n      // getROSTypeName\n      std::bind(\n        &NitrosFormatAgent<T>::getROSTypeName\n      ),\n    };\n  }\n\n  // Publisher callbacks\n  // Create a compatible publisher for T\n  static void createCompatiblePublisherCallback(\n    rclcpp::Node & node,\n    std::shared_ptr<rclcpp::PublisherBase> & compatible_pub,\n    const std::string & topic_name,\n    const rclcpp::QoS & qos,\n    const rclcpp::PublisherOptions & options)\n  {\n    compatible_pub = node.create_publisher<typename T::MsgT>(\n      topic_name,\n      qos,\n      options);\n\n    RCLCPP_DEBUG(\n      node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name),\n      \"Created a compatible publisher (topic_name=%s)\",\n      compatible_pub->get_topic_name());\n  }\n\n  // Add a compatible publisher of type T to a negotiated publisher\n  static void addCompatiblePublisherCallback(\n    rclcpp::Node & node,\n    std::shared_ptr<negotiated::NegotiatedPublisher> negotiated_pub,\n    std::shared_ptr<rclcpp::PublisherBase> compatible_pub,\n    const double weight)\n  {\n    auto cast_compatible_pub =\n      std::static_pointer_cast<rclcpp::Publisher<typename T::MsgT>>(compatible_pub);\n    negotiated_pub->add_compatible_publisher(\n      cast_compatible_pub,\n      T::supported_type_name,\n      weight);\n\n    RCLCPP_DEBUG(\n      node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name),\n      \"Added a compatible publisher (topic_name=%s) to a negotiated publisher\",\n      compatible_pub->get_topic_name());\n  }\n\n  // Add T to a negotiated publisher as a supported format\n  static void addPublisherSupportedFormatCallback(\n    rclcpp::Node & node,\n    std::shared_ptr<negotiated::NegotiatedPublisher> negotiated_pub,\n    const double weight,\n    const rclcpp::QoS & qos,\n    const rclcpp::PublisherOptions & options)\n  {\n    negotiated_pub->add_supported_type<T>(\n      weight,\n      qos,\n      options);\n\n    RCLCPP_DEBUG(\n      node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name),\n      \"Added a supported format \\\"%s\\\" to a negotiated publisher\",\n      T::supported_type_name.c_str());\n  }\n\n  // Publish a message of type T via a negotiated publisher\n  static void negotiatedPublishCallback(\n    rclcpp::Node & node,\n    std::shared_ptr<negotiated::NegotiatedPublisher> negotiated_pub,\n    NitrosTypeBase & base_msg)\n  {\n    auto msg = static_cast<typename T::MsgT &>(base_msg);\n    negotiated_pub->publish<T>(msg);\n\n    RCLCPP_DEBUG(\n      node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name),\n      \"Published a message via the negotiated publisher\");\n  }\n\n  // Publish a message of type T via a compatible publisher\n  static void compatiblePublishCallback(\n    rclcpp::Node & node,\n    std::shared_ptr<rclcpp::PublisherBase> compatible_pub,\n    NitrosTypeBase & base_msg)\n  {\n    auto msg = static_cast<typename T::MsgT &>(base_msg);\n    auto cast_compatible_pub =\n      static_cast<rclcpp::Publisher<typename T::MsgT> *>(compatible_pub.get());\n    cast_compatible_pub->publish(msg);\n\n    RCLCPP_DEBUG(\n      node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name),\n      \"Published a message via the compatible publisher (topic_name=%s)\",\n      compatible_pub->get_topic_name());\n  }\n\n  // Subscriber callbacks\n  // Create a compatible subscriber for T\n  static void createCompatibleSubscriberCallback(\n    rclcpp::Node & node,\n    std::shared_ptr<rclcpp::SubscriptionBase> & compatible_sub,\n    const std::string & topic_name,\n    const rclcpp::QoS & qos,\n    std::function<void(NitrosTypeBase &, const std::string data_format_name)> subscriber_callback,\n    const rclcpp::SubscriptionOptions & options)\n  {\n    std::function<void(std::shared_ptr<const typename T::MsgT>)> internal_subscriber_callback =\n      std::bind(\n      &NitrosFormatAgent<T>::subscriberCallback,\n      std::placeholders::_1,\n      subscriber_callback);\n    compatible_sub = node.create_subscription<typename T::MsgT>(\n      topic_name,\n      qos,\n      internal_subscriber_callback,\n      options);\n\n    RCLCPP_DEBUG(\n      node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name),\n      \"Created a compatible subscriber (topic_name=%s, format_name=%s)\",\n      compatible_sub->get_topic_name(),\n      T::supported_type_name.c_str());\n  }\n\n  // Remove a compatible subscriber of type T from a negotiated subscriber\n  static void removeCompatibleSubscriberCallback(\n    rclcpp::Node & node,\n    std::shared_ptr<negotiated::NegotiatedSubscription> negotiated_sub,\n    std::shared_ptr<rclcpp::SubscriptionBase> compatible_sub)\n  {\n    auto cast_compatible_sub =\n      std::static_pointer_cast<rclcpp::Subscription<typename T::MsgT>>(compatible_sub);\n    negotiated_sub->remove_compatible_subscription(\n      cast_compatible_sub, T::supported_type_name);\n\n    RCLCPP_DEBUG(\n      node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name),\n      \"Removed a compatible subscriber (topic_name=%s) from a negotiated subscriber\",\n      compatible_sub->get_topic_name());\n  }\n\n  // Add a compatible subscriber of type T to a negotiated subscriber\n  static void addCompatibleSubscriberCallback(\n    rclcpp::Node & node,\n    std::shared_ptr<negotiated::NegotiatedSubscription> negotiated_sub,\n    std::shared_ptr<rclcpp::SubscriptionBase> compatible_sub,\n    const double weight)\n  {\n    auto cast_compatible_sub =\n      std::static_pointer_cast<rclcpp::Subscription<typename T::MsgT>>(compatible_sub);\n    negotiated_sub->add_compatible_subscription(\n      cast_compatible_sub,\n      T::supported_type_name,\n      weight);\n\n    RCLCPP_DEBUG(\n      node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name),\n      \"Added a compatible subscriber (topic_name=%s) to a negotiated subscriber\",\n      compatible_sub->get_topic_name());\n  }\n\n  // Add T to a negotiated subscriber as a supported format\n  static void addSubscriberSupportedFormatCallback(\n    rclcpp::Node & node,\n    std::shared_ptr<negotiated::NegotiatedSubscription> negotiated_sub,\n    const double weight,\n    const rclcpp::QoS & qos,\n    // std::function<void(std::shared_ptr<DATA_TYPE_NAME::MsgT>)> subscriber_callback,\n    std::function<void(NitrosTypeBase &, const std::string data_format_name)> subscriber_callback,\n    const rclcpp::SubscriptionOptions & options)\n  {\n    std::function<void(std::shared_ptr<const typename T::MsgT>)> internal_subscriber_callback =\n      std::bind(\n      &NitrosFormatAgent<T>::subscriberCallback,\n      std::placeholders::_1,\n      subscriber_callback);\n    negotiated_sub->add_supported_callback<T>(\n      weight,\n      qos,\n      internal_subscriber_callback,\n      options);\n\n    RCLCPP_DEBUG(\n      node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name),\n      \"Added a supported format \\\"%s\\\" to a negotiated subscriber\",\n      T::supported_type_name.c_str());\n  }\n\n  static std::shared_ptr<rclcpp::SerializedMessage> convertToRosSerializedMessage(\n    NitrosTypeBase & base_msg)\n  {\n    typename rclcpp::TypeAdapter<typename T::MsgT>::ros_message_type ros_message;\n    auto cast_msg = static_cast<typename T::MsgT &>(base_msg);\n    rclcpp::TypeAdapter<typename T::MsgT>::convert_to_ros_message(cast_msg, ros_message);\n\n    std::shared_ptr<rclcpp::SerializedMessage> serialized_ros_message =\n      std::make_shared<rclcpp::SerializedMessage>();\n\n    static rclcpp::Serialization<\n      typename rclcpp::TypeAdapter<typename T::MsgT>::ros_message_type> serializer;\n    serializer.serialize_message(&ros_message, serialized_ros_message.get());\n\n    return serialized_ros_message;\n  }\n\n  static std::vector<std::pair<std::string, std::string>> getExtensions()\n  {\n    return T::MsgT::GetExtensions();\n  }\n\n  // Utilities\n  // Get the corresponding ROS type name for the format T\n  static std::string getROSTypeName()\n  {\n    using ROSMessageType = typename rclcpp::TypeAdapter<typename T::MsgT>::ros_message_type;\n    return rosidl_generator_traits::name<ROSMessageType>();\n  }\n\nprivate:\n  static void subscriberCallback(\n    const std::shared_ptr<const typename T::MsgT> msg,\n    std::function<void(\n      NitrosTypeBase &,\n      const std::string data_format_name)> subscriber_callback)\n  {\n    auto base_msg = (NitrosTypeBase)(*msg.get());\n    subscriber_callback(\n      base_msg,\n      T::supported_type_name\n    );\n  }\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__TYPES__NITROS_FORMAT_AGENT_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_base.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_BASE_HPP_\n#define ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_BASE_HPP_\n\n#include <map>\n#include <memory>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"cuda_runtime.h\"  // NOLINT - include .h without directory\n\n#include \"isaac_ros_nitros/types/type_utility.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// The base struct for all Nitros-based data types/formats\nstruct NitrosTypeBase\n{\n  NitrosTypeBase() = default;\n\n  // Constructor\n  NitrosTypeBase(\n    const int64_t handle,\n    const std::string data_format_name,\n    const std::string compatible_data_format_name,\n    const std::string frame_id,\n    const cudaStream_t cuda_stream);\n\n  // Copy constructor\n  NitrosTypeBase(const NitrosTypeBase & other);\n\n  // Destructor\n  ~NitrosTypeBase();\n\n  int64_t handle;\n  std::string data_format_name;\n  std::string compatible_data_format_name;\n  std::string frame_id;\n  cudaStream_t cuda_stream;\n};\n\n/* *INDENT-OFF* */\n// Factory macros that help define NITROS data type\n// Mark the beginning of a factory for TYPE_TYPENAME\n#define NITROS_TYPE_FACTORY_BEGIN(TYPE_TYPENAME) \\\nstruct TYPE_TYPENAME : NitrosTypeBase \\\n{ \\\n  using NitrosTypeBase::NitrosTypeBase; \\\n  \\\n  using RawPtr = TYPE_TYPENAME *; \\\n  using ConstRawPtr = const TYPE_TYPENAME *; \\\n  using SharedPtr = std::shared_ptr<TYPE_TYPENAME>; \\\n  using ConstSharedPtr = std::shared_ptr<TYPE_TYPENAME const>; \\\n  using UniquePtr = std::unique_ptr<TYPE_TYPENAME>; \\\n  using ConstUniquePtr = std::unique_ptr<TYPE_TYPENAME const>; \\\n  using WeakPtr = std::weak_ptr<TYPE_TYPENAME>; \\\n  using ConstWeakPtr = std::weak_ptr<TYPE_TYPENAME const>;\n\n// Mark the end of the type factory\n#define NITROS_TYPE_FACTORY_END() \\\n};\n\n// Mark the beginning of a format factory for the current data type\n#define NITROS_FORMAT_FACTORY_BEGIN() \\\nstatic std::map<std::string, NitrosFormatCallbacks> GetFormatCallbacks() \\\n{ \\\n  std::map<std::string, NitrosFormatCallbacks> format_callback_map;\n\n// Add a supported format under the current data type\n#define NITROS_FORMAT_ADD(FORMAT_TYPENAME) \\\n  format_callback_map.emplace( \\\n    FORMAT_TYPENAME::supported_type_name, \\\n    NitrosFormatAgent<FORMAT_TYPENAME>::GetFormatCallbacks());\n\n// Mark the end of the format factory\n#define NITROS_FORMAT_FACTORY_END() \\\n  return format_callback_map; \\\n}\n\n#define NITROS_DEFAULT_COMPATIBLE_FORMAT(FORMAT_TYPENAME) \\\nstatic std::string GetDefaultCompatibleFormat() \\\n{ \\\n return FORMAT_TYPENAME::supported_type_name; \\\n}\n\n// Mark the beginning of an extension factory for the current data type\n#define NITROS_TYPE_EXTENSION_FACTORY_BEGIN() \\\nstatic std::vector<std::pair<std::string, std::string>> GetExtensions() \\\n{ \\\n  std::vector<std::pair<std::string, std::string>> extensions;\n\n// Add a required extension for the current data type\n#define NITROS_TYPE_EXTENSION_ADD(PACKAGE_NAME, SO_FILE_RELATIVE_PATH) \\\n  extensions.push_back({PACKAGE_NAME, SO_FILE_RELATIVE_PATH});\n\n// Mark the end of the extension factory\n#define NITROS_TYPE_EXTENSION_FACTORY_END() \\\n  return extensions; \\\n}\n/* *INDENT-ON* */\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_BASE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_manager.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_MANAGER_HPP_\n#define ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_MANAGER_HPP_\n\n#include <map>\n#include <string>\n#include <vector>\n#include <utility>\n\n#include <ament_index_cpp/get_package_share_directory.hpp>\n\n#include \"gxf/core/gxf.h\"\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/logger.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nclass NitrosTypeManager\n{\npublic:\n  // Constructor\n  NitrosTypeManager() = default;\n  explicit NitrosTypeManager(const rclcpp::Node * node)\n  {\n    setNode(node);\n  }\n\n  // Setter for node_ that is used to get ROS logger if set\n  void setNode(const rclcpp::Node * node)\n  {\n    node_ = node;\n  }\n\n  // Register supported data formats from the given data type\n  template<typename T>\n  void registerSupportedType()\n  {\n    auto type_name = typeid(T).name();\n\n    if (type_format_map_.count(type_name) != 0) {\n      RCLCPP_INFO(\n        get_logger(),\n        \"[NitrosTypeManager] Skipped registering duplicate type \\\"%s\\\"\",\n        type_name);\n      return;\n    }\n\n    // Register format callbacks\n    auto type_format_callback_map = T::GetFormatCallbacks();\n    format_callback_map_.insert(\n      type_format_callback_map.begin(), type_format_callback_map.end());\n\n    // Register formats to the type\n    if (type_format_map_.count(type_name) == 0) {\n      type_format_map_.insert({type_name, {}});\n    }\n    for (auto const & it : type_format_callback_map) {\n      type_format_map_.at(type_name).push_back(it.first);\n    }\n\n    // Register the type's required extensions\n    auto type_extension_list = T::GetExtensions();\n    type_extension_map_.insert({type_name, type_extension_list});\n  }\n\n  // Get all extension paths for all registered types\n  std::vector<std::pair<std::string, std::string>> getExtensions() const\n  {\n    std::vector<std::pair<std::string, std::string>> extensions;\n    for (const auto & extension_pairs : type_extension_map_) {\n      extensions.insert(\n        extensions.end(),\n        extension_pairs.second.begin(),\n        extension_pairs.second.end());\n    }\n    return extensions;\n  }\n\n  // Load all extensions of all registered types\n  void loadExtensions()\n  {\n    auto extensions = getExtensions();\n    for (const auto & extension_pair : extensions) {\n      loadExtenstion(extension_pair.first, extension_pair.second);\n    }\n  }\n\n  // Load extensions for the specified, registered format\n  bool loadExtensions(const std::string & format_name)\n  {\n    if (!hasFormat(format_name)) {\n      return false;\n    }\n    auto extensions = getFormatCallbacks(format_name).getExtensions();\n    for (const auto & extension_pair : extensions) {\n      loadExtenstion(extension_pair.first, extension_pair.second);\n    }\n    return true;\n  }\n\n  // Load extension of the given name in the given package\n  void loadExtenstion(\n    const std::string & package_name,\n    const std::string & extension_name)\n  {\n    auto nitros_context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext();\n    const std::string package_directory =\n      ament_index_cpp::get_package_share_directory(package_name);\n    gxf_result_t code = nitros_context.loadExtension(package_directory, extension_name);\n    if (code != GXF_SUCCESS) {\n      std::stringstream error_msg;\n      error_msg << \"loadExtensions Error: \" << GxfResultStr(code);\n      RCLCPP_ERROR(get_logger(), \"%s\", error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n\n  // Check if the given format name has been registered\n  bool hasFormat(const std::string format_name) const\n  {\n    return format_callback_map_.count(format_name) > 0;\n  }\n\n  // Get a data format's callback functions\n  const NitrosFormatCallbacks & getFormatCallbacks(const std::string format_name) const\n  {\n    return format_callback_map_.at(format_name);\n  }\n\n  // Get a list of data format strings of all the registered data types\n  std::vector<std::string> getAllRegisteredDataFormats()\n  {\n    std::vector<std::string> format_list;\n    for (const auto & format_callback : format_callback_map_) {\n      format_list.push_back(format_callback.first);\n    }\n    return format_list;\n  }\n\nprivate:\n  // Get the manager's logger if set. Forward to rclcpp::get_logger() otherwise.\n  rclcpp::Logger get_logger()\n  {\n    if (node_ != nullptr) {\n      return node_->get_logger();\n    }\n    return rclcpp::get_logger(\"NitrosTypeManager\");\n  }\n\n  // The associated ROS node (for logging purpose)\n  const rclcpp::Node * node_ = nullptr;\n\n  // A map storing callback functions for each registered format\n  std::map<std::string, NitrosFormatCallbacks> format_callback_map_;\n\n  // A map for a type's formats\n  std::map<std::string, std::vector<std::string>> type_format_map_;\n\n  // A map for a type's list of required component extension so files\n  std::map<std::string, std::vector<std::pair<std::string, std::string>>>\n  type_extension_map_;\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_MANAGER_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_message_filter_traits.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_MESSAGE_FILTER_TRAITS_HPP_\n#define ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_MESSAGE_FILTER_TRAITS_HPP_\n\n#include <map>\n#include <string>\n#include <type_traits>\n#include <utility>\n#include <vector>\n\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n#include \"isaac_ros_nitros/types/type_utility.hpp\"\n#include \"std_msgs/msg/header.hpp\"\n\n#include \"message_filters/message_traits.h\"\n#include \"message_filters/subscriber.h\"\n\n// Nitros type support for message_filters package\n// https://github.com/ros2/message_filters.git\n\nnamespace message_filters\n{\nnamespace message_traits\n{\n\ntemplate<typename M, typename = void>\nstruct IsNitrosType : public std::false_type {};\n\ntemplate<typename M>\nstruct IsNitrosType<M,\n  typename std::enable_if<std::is_base_of<nvidia::isaac_ros::nitros::NitrosTypeBase,\n  M>::value>::type>: public std::true_type {};\n\ntemplate<typename M>\nstruct TimeStamp<M, typename std::enable_if<IsNitrosType<M>::value>::type>\n{\n  static rclcpp::Time value(const M & m)\n  {\n    std_msgs::msg::Header ros_header;\n    if (nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getEntityTimestamp(\n        m.handle, ros_header) != GXF_SUCCESS)\n    {\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"[NITROS message_filter_traits]\"),\n        \"[message_filter] getEntityTimestamp Error\");\n    }\n    return rclcpp::Time(ros_header.stamp, RCL_ROS_TIME);\n  }\n};\n\ntemplate<typename M>\nstruct FrameId<M, typename std::enable_if<IsNitrosType<M>::value>::type>\n{\n  static std::string * pointer(M & m) {return &m.frame_id;}\n  static std::string const * pointer(const M & m) {return &m.frame_id;}\n  static std::string value(const M & m) {return m.frame_id;}\n};\n\n}  // namespace message_traits\n\n}  // namespace message_filters\n\n#endif  // ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_MESSAGE_FILTER_TRAITS_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_view_factory.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_VIEW_FACTORY_HPP_\n#define ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_VIEW_FACTORY_HPP_\n\n#include <string>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\nnamespace\n{\nconstexpr uint64_t kNanosecondsInSeconds = 1e9;\n}\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n/* *INDENT-OFF* */\n// Factory macros that help define NITROS data type\n// Mark the beginning of a factory for TYPE_NAME\n#define MARK_PUBLIC_SECTION() \\\npublic:\n#define MARK_PROTECTED_SECTION() \\\nprotected:\n#define MARK_PRIVATE_SECTION() \\\nprivate:\n\n#define ADD_COMMON_METHODS() \\\nMARK_PRIVATE_SECTION() \\\nint64_t  GetTimestamp() const { \\\n  auto timestamp = msg_entity_->get<gxf::Timestamp>(\"timestamp\"); \\\n  if (!timestamp) { \\\n    timestamp = msg_entity_->get<gxf::Timestamp>(); \\\n  } \\\n  return timestamp.value()->acqtime; \\\n} \\\nMARK_PUBLIC_SECTION() \\\nint32_t GetTimestampSeconds() const {return GetTimestamp() / kNanosecondsInSeconds;} \\\nuint32_t GetTimestampNanoseconds() const {return GetTimestamp() % kNanosecondsInSeconds;} \\\nconst std::string GetFrameId() const {return msg_.frame_id;}\n\n#define NITROS_TYPE_VIEW_FACTORY_BEGIN(TYPE_NAME) \\\nclass TYPE_NAME##View  \\\n{ \\\nMARK_PUBLIC_SECTION() \\\nconst TYPE_NAME & GetMessage() const {return msg_;} \\\nvoid InitView(); \\\nexplicit TYPE_NAME##View(const TYPE_NAME & in_msg) \\\n: msg_{in_msg}, \\\n  msg_entity_{gxf::Entity::Shared(GetTypeAdapterNitrosContext().getContext(), in_msg.handle)} { \\\n    InitView(); \\\n  } \\\nconst TYPE_NAME##View & GetView() const {return *this;}  \\\nusing BaseType = TYPE_NAME; \\\nADD_COMMON_METHODS()\n\n#define NITROS_TYPE_VIEW_FACTORY_END(TYPE_NAME) \\\nMARK_PRIVATE_SECTION() \\\nconst TYPE_NAME msg_; \\\nconst gxf::Expected<gxf::Entity> msg_entity_; \\\n};\n/* *INDENT-ON* */\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__TYPES__NITROS_TYPE_VIEW_FACTORY_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/types/type_adapter_nitros_context.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__TYPES__TYPE_ADAPTER_NITROS_CONTEXT_HPP_\n#define ISAAC_ROS_NITROS__TYPES__TYPE_ADAPTER_NITROS_CONTEXT_HPP_\n\n#include <memory>\n#include <mutex>\n\n#include \"isaac_ros_nitros/nitros_context.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Global NitrosContext for data type adapter\nextern std::unique_ptr<NitrosContext> g_type_adapter_nitros_context;\n\n// Mutex for the type adapter's global NitrosContext\nextern std::mutex g_type_adapter_nitros_context_mutex;\n\n// Is the global type adapter's global NitrosContext initialized?\nextern bool g_type_adapter_nitros_context_initialized;\n\n// Is the global type adapter's global NitrosContext destroyed?\nextern bool g_type_adapter_nitros_context_destroyed;\n\n// Get the type adapter's global NitrosContext object\n// If not initialized, the first call creates an NitrosContext object and starts\n// the type adapter's graph\nNitrosContext & GetTypeAdapterNitrosContext();\n\n// Terminate the type adapter's graph and release the context\nvoid DestroyTypeAdapterNitrosContext();\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__TYPES__TYPE_ADAPTER_NITROS_CONTEXT_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/include/isaac_ros_nitros/types/type_utility.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS__TYPES__TYPE_UTILITY_HPP_\n#define ISAAC_ROS_NITROS__TYPES__TYPE_UTILITY_HPP_\n\n#include <string>\n#if defined(USE_NVTX)\n  #include <nvtx3/nvToolsExt.h>\n#endif\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr u_int CLR_RED = 0xFFFF0000;\nconstexpr u_int CLR_BLUE = 0xFF0000FF;\nconstexpr u_int CLR_GREEN = 0xFF008000;\nconstexpr u_int CLR_YELLOW = 0xFFFFFF00;\nconstexpr u_int CLR_CYAN = 0xFF00FFFF;\nconstexpr u_int CLR_MAGENTA = 0xFFFF00FF;\nconstexpr u_int CLR_GRAY = 0xFF808080;\nconstexpr u_int CLR_PURPLE = 0xFF800080;\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\nstatic inline void nvtxRangePushWrapper(const char * range_title, u_int range_color)\n{\n  #if defined(USE_NVTX)\n  nvtxEventAttributes_t eventAttrib = {\n    NVTX_VERSION,  // version\n    NVTX_EVENT_ATTRIB_STRUCT_SIZE,  // size\n    0,  // category\n    0,  // colorType\n    0,  // color\n    0,  // payloadType\n    0,  // reseverd0\n    0,  // payload\n    0,  // messageType\n    0,  // message\n  };\n  eventAttrib.messageType = NVTX_MESSAGE_TYPE_ASCII;\n  eventAttrib.colorType = NVTX_COLOR_ARGB;\n  eventAttrib.color = range_color;\n  eventAttrib.message.ascii = range_title;\n  nvtxRangePushEx(&eventAttrib);\n  #endif\n}\n#pragma GCC diagnostic pop\n\nstatic inline void nvtxRangePopWrapper()\n{\n  #if defined(USE_NVTX)\n  nvtxRangePop();\n  #endif\n}\n\nstatic inline void nvtxMarkExWrapper(const char * range_title, u_int range_color)\n{\n  #if defined(USE_NVTX)\n  nvtxEventAttributes_t eventAttrib = {\n    NVTX_VERSION,  // version\n    NVTX_EVENT_ATTRIB_STRUCT_SIZE,  // size\n    0,  // category\n    0,  // colorType\n    0,  // color\n    0,  // payloadType\n    0,  // reseverd0\n    0,  // payload\n    0,  // messageType\n    0,  // message\n  };\n  eventAttrib.messageType = NVTX_MESSAGE_TYPE_ASCII;\n  eventAttrib.colorType = NVTX_COLOR_ARGB;\n  eventAttrib.color = range_color;\n  eventAttrib.message.ascii = range_title;\n  nvtxMarkEx(&eventAttrib);\n  #endif\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS__TYPES__TYPE_UTILITY_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros/lib/AMENT_IGNORE",
    "content": "\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuapriltags/cuapriltags/cuAprilTags.h",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2018-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef __APRILTAGS__\n#define __APRILTAGS__\n\n#include <stdint.h>\n#include <stddef.h>\n#include <vector>\n#include <vector_types.h>\n\n// Forward declaration for CUDA API\n// CUstream and cudaStream_t are CUstream_st*\nstruct CUstream_st;\n\n// Decoded AprilTag\ntypedef struct cuAprilTagsID_st\n{\n    float2 corners[4];\n    uint16_t id;\n    uint8_t hamming_error;\n    float orientation[9];   //!< Rotation transform, when expressed as a 3x3 matrix acting on a column vector, is column major.\n    float translation[3];   //!< Translation vector from the camera, in the same units as used for the tag_size.\n}cuAprilTagsID_t;\n\n// Input data type for image buffer\ntypedef struct cuAprilTagsImageInput_st\n{\n    uchar3* dev_ptr;    //!< Device pointer to the buffer\n    size_t pitch;       //!< Pitch in bytes\n    uint16_t width;     //!< Width in pixels\n    uint16_t height;    //!< Buffer height\n}cuAprilTagsImageInput_t;\n\ntypedef struct cuAprilTagsCameraIntrinsics_st {\n    float fx, fy, cx, cy;\n}cuAprilTagsCameraIntrinsics_t;\n\ntypedef enum\n{\n    NVAT_TAG36H11,                 // Default, currently the only tag family supported\n    NVAT_ENUM_SIZE = 0x7fffffff    // Force int32_t\n}\ncuAprilTagsFamily;\n\n//! AprilTags Detector instance handle. Used to reference the detector after creation\ntypedef struct cuAprilTagsHandle_st* cuAprilTagsHandle;\n\n#ifdef __cplusplus\nextern \"C\" {\n#endif\n    // FUNCTION NAME:   nvCreateAprilTagsDetector\n    //\n    //! DESCRIPTION:    Creates and initializes an AprilTags detector instance that detects and decodes April tags\n    //!\n    //! \\param [out]    hApriltags          Pointer to the handle of newly created AprilTags detector\n    //! \\param [in]     img_width           Width of images to be fed in to AprilTags detector\n    //! \\param [in]     img_height          Height of images to be fed in to AprilTags detector\n    //! \\param [in]     tile_size           Tile size/ Window size used for adaptive thesholding\n    //! \\param [in]     tag_family          Enum representing the Tag Family to be detected; default NVAT_TAG36H11.\n    //! \\param [in]     cam                 Camera intrinsic parameters, or NULL, if the orientation and translation are not desired.\n    //! \\param [in]     tag_dim             The linear dimension of the square tag. The translation will be expressed in the same units.\n    //!\n    //! \\retval :: 0 - Success, else - Failure\n    int nvCreateAprilTagsDetector(cuAprilTagsHandle* hApriltags,    //!< TODO: We usually return the result in the last parameter, not first.\n        const uint32_t img_width, const uint32_t img_height,\n        const uint32_t tile_size,\n        const cuAprilTagsFamily tag_family,\n        const cuAprilTagsCameraIntrinsics_t *cam,\n        float tag_dim);\n\n    // FUNCTION NAME:   cuAprilTagsDetect\n    //\n    //! DESCRIPTION:    Runs the algorithms to detect potential April tags in the image and decodes valid April tags\n    //!\n    //! \\param [in]     hApriltags          AprilTags detector handle\n    //! \\param [in]     img_input           Input buffer containing the undistorted image on which to detect/decode April tags\n    //! \\param [out]    tags_out            C-array containing detected Tags, after detection and decoding\n    //! \\param [out]    num_tags            Number of tags detected\n    //! \\param [in]     max_tags            Maximum number of tags that can be returned, based on allocated size of tags_out array.\n    //! \\param [in]     input_stream        CUDA stream on which the computation is to occur, or 0 to use the default stream.\n    //!\n    //! \\retval :: 0 - Success, else - Failure\n    int cuAprilTagsDetect(cuAprilTagsHandle hApriltags, const cuAprilTagsImageInput_t *img_input,\n        cuAprilTagsID_t *tags_out, uint32_t *num_tags, const uint32_t max_tags,\n        CUstream_st* input_stream);\n\n    // FUNCTION NAME:   cuAprilTagsDestroy\n    //\n    //! DESCRIPTION:    Destroys an instance of AprilTags detector\n    //!\n    //! \\param [in]     hApriltags            AprilTags detector handle to be destroyed\n    //!\n    //! \\retval :: 0 - Success, else - Failure\n    int cuAprilTagsDestroy(cuAprilTagsHandle hApriltags);\n\n#ifdef __cplusplus\n}\n#endif\n\n#endif //__APRILTAGS__\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuapriltags/lib_aarch64_jetpack51/libcuapriltags.a",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:6b5d37afc13c72913bd96d576ae876ba2d4fe16bb1d0807fe4087f2995e69956\nsize 1159094\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuapriltags/lib_aarch64_jetpack61/libcuapriltags.a",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:e67298e8ee52d6253cd702f4c55b3727bea177f57236aa23797bde50d74dfc24\nsize 1262318\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuapriltags/lib_x86_64_cuda_12_6/libcuapriltags.a",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:a7f32f58782de61dd23e507fe6ce5a907379498deec505669fe339ecd4facba7\nsize 1575838\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/.gitattributes",
    "content": "# Python wheel files\n*.whl filter=lfs diff=lfs merge=lfs -text\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/LICENSE",
    "content": "Copyright (c) 2019-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nThis software is licensed under the NVIDIA Isaac ROS Software License (see below),\nexcept as otherwise noted in individual file headers.\n\nSee the accompanying NOTICE file for third-party attributions and licenses.\n\n\nNVIDIA ISAAC ROS SOFTWARE LICENSE\n\nThis license is a legal agreement between you and NVIDIA Corporation (\"NVIDIA\") and governs the use of the NVIDIA Isaac ROS software and materials provided hereunder (“SOFTWARE”).\n\nThis license can be accepted only by an adult of legal age of majority in the country in which the SOFTWARE is used.\n\nIf you are entering into this license on behalf of a company or other legal entity, you represent that you have the legal authority to bind the entity to this license, in which case “you” will mean the entity you represent.\n\nIf you don’t have the required age or authority to accept this license, or if you don’t accept all the terms and conditions of this license, do not download, install or use the SOFTWARE.\n\nYou agree to use the SOFTWARE only for purposes that are permitted by (a) this license, and (b) any applicable law, regulation or generally accepted practices or guidelines in the relevant jurisdictions.\n\n1. LICENSE. Subject to the terms of this license, NVIDIA hereby grants you a non-exclusive, non-transferable license, without the right to sublicense (except as expressly provided in this license) to:\na. Install and use the SOFTWARE,\nb. Modify and create derivative works of sample or reference source code delivered in the SOFTWARE, and\nc. Distribute any part of the SOFTWARE (i) as incorporated into a software application that has material additional functionality beyond the included portions of the SOFTWARE, or (ii) unmodified in binary format, in each case subject to the distribution requirements indicated in this license.\n\n2. DISTRIBUTION REQUIREMENTS. These are the distribution requirements for you to exercise the distribution grant above:\n    a. The following notice shall be included in modifications and derivative works of source code distributed: “This software contains source code provided by NVIDIA Corporation.”\n    b. You agree to distribute the SOFTWARE subject to the terms at least as protective as the terms of this license, including (without limitation) terms relating to the license grant, license restrictions and protection of NVIDIA’s intellectual property rights. Additionally, you agree that you will protect the privacy, security and legal rights of your application users.\n    c. You agree to notify NVIDIA in writing of any known or suspected distribution or use of the SOFTWARE not in compliance with the requirements of this license, and to enforce the terms of your agreements with respect to the distributed portions of the SOFTWARE.\n3. AUTHORIZED USERS. You may allow employees and contractors of your entity or of your subsidiary(ies) to access and use the SOFTWARE from your secure network to perform work on your behalf. If you are an academic institution you may allow users enrolled or employed by the academic institution to access and use the SOFTWARE from your secure network. You are responsible for the compliance with the terms of this license by your authorized users.\n\n4. LIMITATIONS. Your license to use the SOFTWARE is restricted as follows:\n    a. The SOFTWARE is licensed for you to develop applications only for their use in systems with NVIDIA GPUs.\n    b. You may not reverse engineer, decompile or disassemble, or remove copyright or other proprietary notices from any portion of the SOFTWARE or copies of the SOFTWARE.\n    c. Except as expressly stated above in this license, you may not sell, rent, sublicense, transfer, distribute, modify, or create derivative works of any portion of the SOFTWARE.\n    d.  Unless you have an agreement with NVIDIA for this purpose, you may not indicate that an application created with the SOFTWARE is sponsored or endorsed by NVIDIA.\n    e.  You may not bypass, disable, or circumvent any technical limitation, encryption, security, digital rights management or authentication mechanism in the SOFTWARE.\n    f. You may not use the SOFTWARE in any manner that would cause it to become subject to an open source software license. As examples, licenses that require as a condition of use, modification, and/or distribution that the SOFTWARE be: (i) disclosed or distributed in source code form; (ii) licensed for the purpose of making derivative works; or (iii) redistributable at no charge.\n    g.  You acknowledge that the SOFTWARE as delivered is not tested or certified by NVIDIA for use in connection with the design, construction, maintenance, and/or operation of any system where the use or failure of such system could result in a situation that threatens the safety of human life or results in catastrophic damages (each, a \"Critical Application\"). Examples of Critical Applications include use in avionics, navigation, autonomous vehicle applications, ai solutions for automotive products, military, medical, life support or other life critical applications. NVIDIA shall not be liable to you or any third party, in whole or in part, for any claims or damages arising from such uses. You are solely responsible for ensuring that any product or service developed with the SOFTWARE as a whole includes sufficient features to comply with all applicable legal and regulatory standards and requirements.\n    h. You agree to defend, indemnify and hold harmless NVIDIA and its affiliates, and their respective employees, contractors, agents, officers and directors, from and against any and all claims, damages, obligations, losses, liabilities, costs or debt, fines, restitutions and expenses (including but not limited to attorney’s fees and costs incident to establishing the right of indemnification) arising out of or related to your use of goods and/or services that include or utilize the SOFTWARE, or for use of the SOFTWARE outside of the scope of this license or not in compliance with its terms.\n\n5. UPDATES. NVIDIA may, at its option, make available patches, workarounds or other updates to this SOFTWARE. Unless the updates are provided with their separate governing terms, they are deemed part of the SOFTWARE licensed to you as provided in this license.\n\n6. PRE-RELEASE VERSIONS. SOFTWARE versions identified as alpha, beta, preview, early access or otherwise as pre-release may not be fully functional, may contain errors or design flaws, and may have reduced or different security, privacy, availability, and reliability standards relative to commercial versions of NVIDIA software and materials. You may use a pre-release SOFTWARE version at your own risk, understanding that these versions are not intended for use in production or business-critical systems.\n\n7. COMPONENTS UNDER OTHER LICENSES. The SOFTWARE may include NVIDIA or third-party components with separate legal notices or terms as may be described in proprietary notices accompanying the SOFTWARE, such as components governed by open source software licenses. If and to the extent there is a conflict between the terms in this license and the license terms associated with a component, the license terms associated with the component controls only to the extent necessary to resolve the conflict.\n\n8. OWNERSHIP.\n\n8.1 NVIDIA reserves all rights, title and interest in and to the SOFTWARE not expressly granted to you under this license. NVIDIA and its suppliers hold all rights, title and interest in and to the SOFTWARE, including their respective intellectual property rights. The SOFTWARE is copyrighted and protected by the laws of the United States and other countries, and international treaty provisions.\n\n8.2 Subject to the rights of NVIDIA and its suppliers in the SOFTWARE, you hold all rights, title and interest in and to your applications and your derivative works of the sample or reference source code delivered in the SOFTWARE including their respective intellectual property rights. With respect to source code samples or reference source code licensed to you, NVIDIA and its affiliates are free to continue independently developing source code samples and you covenant not to sue NVIDIA, its affiliates or their licensees with respect to later versions of NVIDIA released source code.\n\n9. FEEDBACK. You may, but are not obligated to, provide to NVIDIA Feedback. “Feedback” means suggestions, fixes, modifications, feature requests or other feedback regarding the SOFTWARE. Feedback, even if designated as confidential by you, shall not create any confidentiality obligation for NVIDIA. NVIDIA and its designees have a perpetual, non-exclusive, worldwide, irrevocable license to use, reproduce, publicly display, modify, create derivative works of, license, sublicense, and otherwise distribute and exploit Feedback as NVIDIA sees fit without payment and without obligation or restriction of any kind on account of intellectual property rights or otherwise.\n\n10. NO WARRANTIES. THE SOFTWARE IS PROVIDED AS-IS. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW NVIDIA AND ITS AFFILIATES EXPRESSLY DISCLAIM ALL WARRANTIES OF ANY KIND OR NATURE, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, OR FITNESS FOR A PARTICULAR PURPOSE. NVIDIA DOES NOT WARRANT THAT THE SOFTWARE WILL MEET YOUR REQUIREMENTS OR THAT THE OPERATION THEREOF WILL BE UNINTERRUPTED OR ERROR-FREE, OR THAT ALL ERRORS WILL BE CORRECTED.\n\n11. LIMITATIONS OF LIABILITY. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW NVIDIA AND ITS AFFILIATES SHALL NOT BE LIABLE FOR ANY SPECIAL, INCIDENTAL, PUNITIVE OR CONSEQUENTIAL DAMAGES, OR FOR ANY LOST PROFITS, PROJECT DELAYS, LOSS OF USE, LOSS OF DATA OR LOSS OF GOODWILL, OR THE COSTS OF PROCURING SUBSTITUTE PRODUCTS, ARISING OUT OF OR IN CONNECTION WITH THIS LICENSE OR THE USE OR PERFORMANCE OF THE SOFTWARE, WHETHER SUCH LIABILITY ARISES FROM ANY CLAIM BASED UPON BREACH OF CONTRACT, BREACH OF WARRANTY, TORT (INCLUDING NEGLIGENCE), PRODUCT LIABILITY OR ANY OTHER CAUSE OF ACTION OR THEORY OF LIABILITY, EVEN IF NVIDIA HAS PREVIOUSLY BEEN ADVISED OF, OR COULD REASONABLY HAVE FORESEEN, THE POSSIBILITY OF SUCH DAMAGES. IN NO EVENT WILL NVIDIA’S AND ITS AFFILIATES TOTAL CUMULATIVE LIABILITY UNDER OR ARISING OUT OF THIS LICENSE EXCEED US$10.00. THE NATURE OF THE LIABILITY OR THE NUMBER OF CLAIMS OR SUITS SHALL NOT ENLARGE OR EXTEND THIS LIMIT.\n\n12. TERMINATION. Your rights under this license will terminate automatically without notice from NVIDIA if you fail to comply with any term and condition of this license or if you commence or participate in any legal proceeding against NVIDIA with respect to the SOFTWARE. NVIDIA may terminate this license with advance written notice to you, if NVIDIA decides to no longer provide the SOFTWARE in a country or, in NVIDIA’s sole discretion, the continued use of it is no longer commercially viable. Upon any termination of this license, you agree to promptly discontinue use of the SOFTWARE and destroy all copies in your possession or control. Your prior distributions in accordance with this license are not affected by the termination of this license. All provisions of this license will survive termination, except for the license granted to you.\n\n13. APPLICABLE LAW. This license will be governed in all respects by the laws of the United States and of the State of Delaware, without regard to the conflicts of laws principles. The United Nations Convention on Contracts for the International Sale of Goods is specifically disclaimed. You agree to all terms of this license in the English language. The state or federal courts residing in Santa Clara County, California shall have exclusive jurisdiction over any dispute or claim arising out of this license. Notwithstanding this, you agree that NVIDIA shall still be allowed to apply for injunctive remedies or urgent legal relief in any jurisdiction.\n\n14. NO ASSIGNMENT. This license and your rights and obligations thereunder may not be assigned by you by any means or operation of law without NVIDIA’s permission. Any attempted assignment not approved by NVIDIA in writing shall be void and of no effect. NVIDIA may assign, delegate or transfer this license and its rights and obligations, and if to a non-affiliate you will be notified.\n\n15. EXPORT. The SOFTWARE is subject to United States export laws and regulations. You agree to comply with all applicable U.S. and international export laws, including the Export Administration Regulations (EAR) administered by the U.S. Department of Commerce and economic sanctions administered by the U.S. Department of Treasury’s Office of Foreign Assets Control (OFAC). These laws include restrictions on destinations, end-users and end-use. By accepting this license, you confirm that you are not currently residing in a country or region currently embargoed by the U.S. and that you are not otherwise prohibited from receiving the SOFTWARE.\n\n16. GOVERNMENT USE. The SOFTWARE is, and shall be treated as being, “Commercial Items” as that term is defined at 48 CFR § 2.101, consisting of “commercial computer software” and “commercial computer software documentation”, respectively, as such terms are used in, respectively, 48 CFR § 12.212 and 48 CFR §§ 227.7202 & 252.227-7014(a)(1). Use, duplication or disclosure by the U.S. Government or a U.S. Government subcontractor is subject to the restrictions in this license pursuant to 48 CFR § 12.212 or 48 CFR § 227.7202. In no event shall the US Government user acquire rights in the SOFTWARE beyond those specified in 48 C.F.R. 52.227-19(b)(1)-(2).\n\n17. NOTICES. Please direct your legal notices or other correspondence to NVIDIA Corporation, 2788 San Tomas Expressway, Santa Clara, California 95051, United States of America, Attention: Legal Department.\n\n18. ENTIRE AGREEMENT. This license is the final, complete and exclusive agreement between the parties relating to the subject matter of this license and supersedes all prior or contemporaneous understandings and agreements relating to this subject matter, whether oral or written. If any court of competent jurisdiction determines that any provision of this license is illegal, invalid or unenforceable, the remaining provisions will remain in full force and effect. Any amendment or waiver under this license shall be in writing and signed by representatives of both parties.\n\n(v. November 17, 2021)\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/NOTICE",
    "content": "cuMotion is distributed with the third-party components listed below.\n\nIn addition, installing the cuMotion Python package via pip will download and\ninstall additional third-party open source software projects. Review the\nlicense terms of those projects before use.\n\n\npybind11 3.0.1\n==============\n\nThe cuMotion Python package leverages pybind11 for its Python bindings.\n\n   Site: https://github.com/pybind/pybind11\nLicense: https://github.com/pybind/pybind11/blob/v3.0.1/LICENSE\n\nCopyright (c) 2016 Wenzel Jakob <wenzel.jakob@epfl.ch>, All rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\n   list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\n   this list of conditions and the following disclaimer in the documentation\n   and/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its contributors\n   may be used to endorse or promote products derived from this software\n   without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\nANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\nWARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n\nyaml-cpp 0.7.0\n==============\n\ncuMotion statically links against the yaml-cpp library, without modification.\n\n   Site: https://github.com/jbeder/yaml-cpp\nLicense: https://github.com/jbeder/yaml-cpp/blob/yaml-cpp-0.7.0/LICENSE\n\nCopyright (c) 2008-2015 Jesse Beder.\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in\nall copies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN\nTHE SOFTWARE.\n\n\nurdfdom 3.0.1\n=============\n\ncuMotion statically links against the urdfdom library, without modification.\n\n   Site: https://github.com/ros/urdfdom\nLicense: https://github.com/ros/urdfdom/blob/3.0.1/LICENSE\n\nCopyright (c) 2008, Willow Garage, Inc.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions\nare met:\n\n * Redistributions of source code must retain the above copyright\n   notice, this list of conditions and the following disclaimer.\n * Redistributions in binary form must reproduce the above\n   copyright notice, this list of conditions and the following\n   disclaimer in the documentation and/or other materials provided\n   with the distribution.\n * Neither the name of the Willow Garage nor the names of its\n   contributors may be used to endorse or promote products derived\n   from this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\nFOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\nCOPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\nINCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\nBUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\nLOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\nLIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\nANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGE.\n\n\nTinyXML 2.6.2\n=============\n\ncuMotion statically links against the TinyXML library, without modification.\n\n   Site: https://sourceforge.net/projects/tinyxml/\nLicense: https://sourceforge.net/p/tinyxml/git/ci/master/tree/readme.txt\n\nTinyXML is released under the zlib license:\n\nThis software is provided 'as-is', without any express or implied\nwarranty. In no event will the authors be held liable for any\ndamages arising from the use of this software.\n\nPermission is granted to anyone to use this software for any\npurpose, including commercial applications, and to alter it and\nredistribute it freely, subject to the following restrictions:\n\n1. The origin of this software must not be misrepresented; you must\nnot claim that you wrote the original software. If you use this\nsoftware in a product, an acknowledgment in the product documentation\nwould be appreciated but is not required.\n\n2. Altered source versions must be plainly marked as such, and\nmust not be misrepresented as being the original software.\n\n3. This notice may not be removed or altered from any source\ndistribution.\n\n\nconsole_bridge 1.0.1\n====================\n\ncuMotion statically links against the console_bridge library, without\nmodification.\n\n   Site: https://github.com/ros/console_bridge\nLicense: https://github.com/ros/console_bridge/blob/1.0.1/LICENSE\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n   * Redistributions of source code must retain the above copyright\n     notice, this list of conditions and the following disclaimer.\n\n   * Redistributions in binary form must reproduce the above copyright\n     notice, this list of conditions and the following disclaimer in the\n     documentation and/or other materials provided with the distribution.\n\n   * Neither the name of the copyright holder nor the names of its\n     contributors may be used to endorse or promote products derived from\n     this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\nARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\nLIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\nCONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\nSUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\nINTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\nCONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\nARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGE.\n\n\nur_description 1.2.7 and 1.3.0\n==============================\n\nThe URDF and mesh files in content/third_party/universal_robots/ are derived\nfrom those found in the \"universal_robot\" package of the ROS-Industrial\nproject.\n\nSite: http://wiki.ros.org/universal_robot\n\nCopyright 2013-2020 Felix Messmer, Kelsey Hawkins, Shaun Edwards,\n    Stuart Glaser, Wim Meeussen, and Contributors\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\nlist of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\nthis list of conditions and the following disclaimer in the documentation\nand/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its contributors\nmay be used to endorse or promote products derived from this software without\nspecific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\nANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\nWARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n\nros2_robotiq_gripper/robotiq_description\n========================================\n\nThe URDF and mesh files in content/third_party/robotiq/ are derived from those\nfound in the \"ros2_robotiq_gripper\" package provided by PickNik Robotics.\n\n   Site: https://github.com/PickNikRobotics/ros2_robotiq_gripper\nLicense: https://github.com/PickNikRobotics/ros2_robotiq_gripper/blob/12e62321/LICENSE\n\nCopyright (c) 2022, PickNik Robotics\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\n   list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\n   this list of conditions and the following disclaimer in the documentation\n   and/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its\n   contributors may be used to endorse or promote products derived from\n   this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n\nros-industrial/fanuc 0.5.1\n==========================\n\nThe URDF and mesh files in content/third_party/fanuc/ are derived from those\nfound in the \"fanuc\" package of the ROS-Industrial project.\n\n   Site: http://wiki.ros.org/fanuc\nLicense: https://github.com/ros-industrial/fanuc/blob/0.5.1/LICENSE\n\nCopyright (c) 2012-2015, TU Delft Robotics Institute\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions\nare met:\n\n * Redistributions of source code must retain the above copyright\n   notice, this list of conditions and the following disclaimer.\n * Redistributions in binary form must reproduce the above\n   copyright notice, this list of conditions and the following\n   disclaimer in the documentation and/or other materials provided\n   with the distribution.\n * Neither the name of the TU Delft Robotics Institute nor the names\n   of its contributors may be used to endorse or promote products\n   derived from this software without specific prior written\n   permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\nFOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\nCOPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\nINCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\nBUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\nLOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\nLIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\nANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGE.\n\n\nunitree_ros (g1_description)\n============================\n\nThe URDF and mesh files in content/third_party/unitree/ are derived from those\nfound in the \"g1_description\" package provided by Unitree Robotics.\n\n   Site: https://github.com/unitreerobotics/unitree_ros/\nLicense: https://github.com/unitreerobotics/unitree_ros/blob/ad5c7db5/LICENSE\n\nCopyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. (\"Unitree Robotics\")\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n* Redistributions of source code must retain the above copyright notice, this\n  list of conditions and the following disclaimer.\n\n* Redistributions in binary form must reproduce the above copyright notice,\n  this list of conditions and the following disclaimer in the documentation\n  and/or other materials provided with the distribution.\n\n* Neither the name of the copyright holder nor the names of its\n  contributors may be used to endorse or promote products derived from\n  this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n\nfranka_ros 0.7.0\n================\n\nThe Franka mesh files in content/third_party/franka/meshes/ were converted\n(with minor modifications) from those in the \"franka_description\" component\nof the franka_ros package.\n\nSite:    https://github.com/frankaemika/franka_ros/\nLicense: https://github.com/frankaemika/franka_ros/blob/0.7.0/NOTICE\n         https://github.com/frankaemika/franka_ros/blob/0.7.0/LICENSE\n\nCopyright 2017 Franka Emika GmbH\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\n    http://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/collision_free_ik_solver.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n#include <optional>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/rotation3.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Configuration parameters for a `CollisionFreeIkSolver`.\nclass CUMO_EXPORT CollisionFreeIkSolverConfig {\n public:\n  virtual ~CollisionFreeIkSolverConfig() = default;\n\n  //! Specify the value for a given parameter.\n  //!\n  //! The required `ParamValue` constructor for each param is detailed in the\n  //! documentation for `setParam()`.\n  struct CUMO_EXPORT ParamValue {\n    //! Create `ParamValue` from `int`.\n    ParamValue(int value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `double`.\n    ParamValue(double value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Set the value of the parameter.\n  //!\n  //! `setParam()` returns `true` if the parameter has been successfully updated and `false` if an\n  //! error has occurred (e.g., invalid parameter).\n  //!\n  //! The following parameters can be set for `CollisionFreeIkSolver`:\n  //!\n  //! `task_space_position_tolerance` [`double`]\n  //!   - Maximum allowable violation of the user-specified constraint on the position of the tool\n  //!     frame.\n  //!   - It determines when a solution is considered to have satisfied position targets.\n  //!     Smaller values require more precise solutions but may be harder to achieve.\n  //!   - Units are in meters.\n  //!   - A default value of 1e-3 (1mm) is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `task_space_orientation_tolerance` [`double`]\n  //!   - Maximum allowable violation of the user-specified constraint on the orientation of the\n  //!     tool frame.\n  //!   - It determines when a solution is considered to have satisfied orientation targets.\n  //!     Smaller values require more precise solutions but may be harder to achieve.\n  //!   - Units are in radians.\n  //!   - A default value of 5e-3 (approximately 0.29 degrees) is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `num_seeds` [`int`]\n  //!   - Number of seeds used to solve each inverse kinematics (IK) problem.\n  //!   - The IK solver generates multiple pseudo-random c-space configurations per problem and\n  //!     optimizes them to find diverse collision-free configurations for the desired tool pose.\n  //!     Higher values increase the likelihood of finding valid solutions but increase\n  //!     computational cost.\n  //!   - A default value of 32 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `max_reattempts` [`int`]\n  //!   - Maximum number of times to restart the IK problem with different random seeds, in case of\n  //!     failure, before giving up.\n  //!   - Higher values increase the likelihood of finding valid IK solutions but increase\n  //!     memory usage and the maximum possible time to find a solution. A value of 0 means no\n  //!     retries (i.e. only perform the initial attempt).\n  //!   - A default value of 10 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!   - NOTE: This parameter is not currently used for batch (multiple problems) planning.\n  //!\n  //! `lbfgs/max_iterations` [`int`]\n  //!   - Maximum number of L-BFGS iterations for the inverse kinematics solver.\n  //!   - Higher values allow the IK solver to converge more precisely but increase computational\n  //!     cost per IK attempt.\n  //!   - A default value of 100 is recommended for most use-cases.\n  //!   - Must be positive.\n  [[nodiscard]] virtual bool setParam(const std::string &param_name, ParamValue value) = 0;\n};\n\n//! Use default parameters to create a configuration for collision-free inverse kinematics.\n//!\n//! These default parameters are combined with `robot_description`, `tool_frame_name`, and\n//! `world_view`.\n//!\n//! A configuration will *NOT* be created if:\n//!   1. `robot_description` is invalid, *OR*\n//!   2. `tool_frame_name` is not a valid frame in `robot_description`.\n//!\n//! In the case of failure, a non-fatal error will be logged and a `nullptr` will be returned.\nCUMO_EXPORT std::unique_ptr<CollisionFreeIkSolverConfig>\nCreateDefaultCollisionFreeIkSolverConfig(const RobotDescription &robot_description,\n                                         const std::string &tool_frame_name,\n                                         const WorldViewHandle &world_view);\n\n//! Interface for using numerical optimization to generate collision-free c-space positions for\n//! task-space targets.\nclass CUMO_EXPORT CollisionFreeIkSolver {\n public:\n  virtual ~CollisionFreeIkSolver() = default;\n\n  //! Translation constraints restrict the position of the origin of a tool frame.\n  class CUMO_EXPORT TranslationConstraint {\n   public:\n    //! Create a `TranslationConstraint` such that the desired position is fully specified as\n    //! `translation_target`.\n    //!\n    //! The optional parameter `deviation_limit` can be used to allow deviation from the\n    //! `translation_target`. This limit specifies the maximum allowable deviation from the desired\n    //! position.\n    //!\n    //! If `deviation_limit` is not input, then the deviation limit is assumed to be zero\n    //! (i.e., it is desired that the tool frame position be exactly `translation_target`).\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `deviation_limit` is negative.\n    //!\n    //! NOTE: The `translation_target` is specified in world frame coordinates (i.e., relative to\n    //! the base of the robot).\n    static TranslationConstraint Target(const Eigen::Vector3d &translation_target,\n                                        std::optional<double> deviation_limit = std::nullopt);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `TranslationConstraint` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of `TranslationConstraint`s are considered concurrently. Each\n  //! `TranslationConstraint` in the goalset must have the same mode (e.g., \"target\") but may have\n  //! different data for each `TranslationConstraint`.\n  //!\n  //! DEPRECATED: This class is deprecated and will be removed in a future version. Use\n  //! `TranslationConstraintArray` instead.\n  class CUMO_EXPORT TranslationConstraintGoalset {\n   public:\n    //! Create a `TranslationConstraintGoalset` such that `translation_targets` are fully specified.\n    //!\n    //! See `TranslationConstraint::Target()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `TranslationConstraint::Target()` is not met, *OR*\n    //!   2. `translation_targets` is empty.\n    static TranslationConstraintGoalset Target(\n      const std::vector<Eigen::Vector3d> &translation_targets,\n      std::optional<double> deviation_limit = std::nullopt);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Jagged two-dimensional (2D) array of `TranslationConstraint`s for solving IK problem(s).\n  //!\n  //! The outer dimension of the array represents the number of problems to solve, and the inner\n  //! dimension represents the number of constraints for each problem, which may vary in size across\n  //! problems.  All constraints must have the same mode (e.g., \"target\") but may have different\n  //! data for each constraint.\n  //!\n  //! NOTE: The current version supports either multiple constraints for a single problem (goalset\n  //! planning) or a single constraint for multiple problems (single-target batch planning).\n  //! However, using multiple constraints for multiple problems (goalset batch planning) is not\n  //! currently supported.\n  class CUMO_EXPORT TranslationConstraintArray {\n   public:\n    //! Create a `TranslationConstraintArray` such that `translation_targets` are fully specified.\n    //!\n    //! Each element in the outer vector represents a separate problem, and each element in the\n    //! inner vector represents a separate constraint for that problem.\n    //!\n    //! See `TranslationConstraint::Target()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `TranslationConstraint::Target()` is not met,\n    //!   2. `translation_targets` is empty,\n    //!   3. `translation_targets[i]` is empty for any problem, *OR*\n    //!   4. The size of `translation_targets` is greater than `1`, and the size of any of its inner\n    //!      vectors is greater than `1`. (This restriction will be removed when support for\n    //!      \"goalset batch IK\" is added.)\n    static TranslationConstraintArray Target(\n      const std::vector<std::vector<Eigen::Vector3d>> &translation_targets,\n      std::optional<double> deviation_limit = std::nullopt);\n\n    //! Return the number of problems in the array.\n    [[nodiscard]] int numProblems() const;\n\n    //! Return the number of constraints for a specific problem in the array.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `problem_index` is out of bounds.\n    [[nodiscard]] int numConstraints(int problem_index) const;\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Orientation constraints restrict the orientation of a tool frame.\n  //!\n  //! Each constraint may fully or partially constrain the orientation.\n  class CUMO_EXPORT OrientationConstraint {\n   public:\n    //! Create an `OrientationConstraint` such that no tool frame orientation constraints are\n    //! active.\n    static OrientationConstraint None();\n\n    //! Create an `OrientationConstraint` such that a tool frame `orientation_target` is fully\n    //! specified.\n    //!\n    //! The optional parameter `deviation_limit` can be used to allow deviation from the\n    //! `orientation_target`. If input, `deviation_limit` is expressed in radians. This limit\n    //! specifies the maximum allowable deviation from the desired orientation.\n    //!\n    //! If `deviation_limit` is not input, then the deviation limit is assumed to be zero (i.e., it\n    //! is desired that orientation be exactly `orientation_target`).\n    //!\n    //! In general, it is suggested that the `deviation_limit` be set to a value less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `deviation_limit` is negative.\n    //!\n    //! NOTE: The `orientation_target` is specified in world frame coordinates (i.e., relative to\n    //! the base of the robot).\n    static OrientationConstraint Target(const Rotation3 &orientation_target,\n                                        std::optional<double> deviation_limit = std::nullopt);\n\n    //! Create an `OrientationConstraint` such that the tool frame orientation is constrained to\n    //! rotate about a \"free axis\".\n    //!\n    //! The \"free axis\" for rotation is defined by a `tool_frame_axis` (specified in the tool frame\n    //! coordinates) and a corresponding `world_target_axis` (specified in world frame coordinates).\n    //!\n    //! The optional `axis_deviation_limit` can be used to allow deviation from the\n    //! desired axis alignment. If input, `axis_deviation_limit` is expressed in radians and the\n    //! limit specifies the maximum allowable deviation of the rotation axis. If\n    //! `axis_deviation_limit` is not input, then the deviation limit is assumed to be zero\n    //! (i.e., it is desired that the tool frame axis be exactly aligned with `world_target_axis`).\n    //!\n    //! In general, it is suggested that the `axis_deviation_limit` be set to a value less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `axis_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `axis_deviation_limit` is negative,\n    //!   2. `tool_frame_axis` is (nearly) zero, *OR*\n    //!   3. `world_target_axis` is (nearly) zero.\n    //!\n    //! NOTE: `tool_frame_axis` and `world_target_axis` inputs will be normalized.\n    static OrientationConstraint Axis(const Eigen::Vector3d &tool_frame_axis,\n                                      const Eigen::Vector3d &world_target_axis,\n                                      std::optional<double> axis_deviation_limit = std::nullopt);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `OrientationConstraint` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of `OrientationConstraint`s are considered concurrently. Each\n  //! `OrientationConstraint` in the goalset must have the same mode (e.g., \"full target\") but\n  //! may have different data for each `OrientationConstraint`.\n  //!\n  //! DEPRECATED: This class is deprecated and will be removed in a future version. Use\n  //! `OrientationConstraintArray` instead.\n  class CUMO_EXPORT OrientationConstraintGoalset {\n   public:\n    //! Create an `OrientationConstraintGoalset` such that no tool frame orientation constraints\n    //! are active.\n    static OrientationConstraintGoalset None();\n\n    //! Create an `OrientationConstraintGoalset` such that tool frame `orientation_targets` are\n    //! fully specified.\n    //!\n    //! See `OrientationConstraint::Target()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::Target()` is not met, *OR*\n    //!   2. `orientation_targets` is empty.\n    static OrientationConstraintGoalset Target(\n        const std::vector<Rotation3> &orientation_targets,\n        std::optional<double> deviation_limit = std::nullopt);\n\n    //! Create an `OrientationConstraintGoalset` such that the tool frame orientation is constrained\n    //! to rotate about a \"free axis\".\n    //!\n    //! See `OrientationConstraint::Axis()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::Axis()` is not met,\n    //!   2. `tool_frame_axes` and `world_target_axes` do not have the same number of elements, *OR*\n    //!   3. `tool_frame_axes` and `world_target_axes` are empty.\n    static OrientationConstraintGoalset Axis(\n        const std::vector<Eigen::Vector3d> &tool_frame_axes,\n        const std::vector<Eigen::Vector3d> &world_target_axes,\n        std::optional<double> axis_deviation_limit = std::nullopt);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Jagged 2D array of `OrientationConstraint`s for solving IK problem(s).\n  //!\n  //! The outer dimension of the array represents the number of problems to solve, and the inner\n  //! dimension represents the number of constraints for each problem, which may vary in size across\n  //! problems.  All constraints must have the same mode (e.g., \"full target\") but may have\n  //! different data for each constraint.\n  //!\n  //! NOTE: The current version supports either multiple constraints for a single problem (goalset\n  //! planning) or a single constraint for multiple problems (single-target batch planning).\n  //! However, using multiple constraints for multiple problems (goalset batch planning) is not\n  //! currently supported.\n  class CUMO_EXPORT OrientationConstraintArray {\n   public:\n    //! Create an `OrientationConstraintArray` such that no tool frame orientation constraints\n    //! are active.\n    static OrientationConstraintArray None();\n\n    //! Create an `OrientationConstraintArray` such that tool frame `orientation_targets` are\n    //! fully specified.\n    //!\n    //! Each element in the outer vector represents a separate problem, and each element in the\n    //! inner vector represents a separate constraint for that problem.\n    //!\n    //! See `OrientationConstraint::Target()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::Target()` is not met,\n    //!   2. `orientation_targets` is empty,\n    //!   3. `orientation_targets[i]` is empty for any problem, *OR*\n    //!   4. The size of `orientation_targets` is greater than `1`, and the size of any of its inner\n    //!      vectors is greater than `1`. (This restriction will be removed when support for\n    //!      \"goalset batch IK\" is added.)\n    static OrientationConstraintArray Target(\n        const std::vector<std::vector<Rotation3>> &orientation_targets,\n        std::optional<double> deviation_limit = std::nullopt);\n\n    //! Create an `OrientationConstraintArray` such that the tool frame orientation is constrained\n    //! to rotate about a \"free axis\".\n    //!\n    //! Each element in the outer vector represents a separate problem, and each element in the\n    //! inner vector represents a separate constraint for that problem.\n    //!\n    //! See `OrientationConstraint::Axis()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::Axis()` is not met,\n    //!   2. `tool_frame_axes` and `world_target_axes` do not have the same number of elements,\n    //!   3. `tool_frame_axes[i]` and `world_target_axes[i]` do not have the same number of elements\n    //!      for each problem,\n    //!   4. `tool_frame_axes` and `world_target_axes` are empty,\n    //!   5. `tool_frame_axes[i]` and `world_target_axes[i]` are empty for any problem, *OR*\n    //!   6. The size of `tool_frame_axes` and `world_target_axes` are greater than `1`, and the\n    //!      size of any of their inner vectors is greater than `1`. (This restriction will be\n    //!      removed when support for \"goalset batch IK\" is added.)\n\n    static OrientationConstraintArray Axis(\n        const std::vector<std::vector<Eigen::Vector3d>> &tool_frame_axes,\n        const std::vector<std::vector<Eigen::Vector3d>> &world_target_axes,\n        std::optional<double> axis_deviation_limit = std::nullopt);\n\n    //! Return the number of problems in the array.\n    //!\n    //! If the array is unconstrained (i.e., created using `None()`), then `nullopt` will be\n    //! returned.\n    [[nodiscard]] std::optional<int> numProblems() const;\n\n    //! Return the number of constraints for a specific problem in the array.\n    //!\n    //! If the array is unconstrained (i.e., created using `None()`), then `nullopt` will be\n    //! returned for any `problem_index`. Otherwise, the number of constraints for the specified\n    //! problem will be returned.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. The array is constrained (i.e., *NOT* created using `None()`) and `problem_index` is\n    //!      out of bounds.\n    [[nodiscard]] std::optional<int> numConstraints(int problem_index) const;\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Task-space targets restrict the position and (optionally) orientation of the tool frame.\n  struct CUMO_EXPORT TaskSpaceTarget {\n    //! Create a task-space target.\n    explicit TaskSpaceTarget(const TranslationConstraint &translation_constraint,\n                             const OrientationConstraint &orientation_constraint =\n                                 OrientationConstraint::None());\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `TaskSpaceTarget` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of pose constraints are considered concurrently. Each pose\n  //! constraint in the goalset must have the same mode (e.g., \"position target and no orientation\n  //! constraints\") but may have different data for each constraint.\n  //!\n  //! DEPRECATED: This class is deprecated and will be removed in a future version. Use\n  //! `TaskSpaceTargetArray` instead.\n  struct CUMO_EXPORT TaskSpaceTargetGoalset {\n    //! Create a task-space target goalset.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. The number of `translation_constraints` does not match the number of\n    //!      `orientation_constraints`.\n    explicit TaskSpaceTargetGoalset(const TranslationConstraintGoalset &translation_constraints,\n                                    const OrientationConstraintGoalset &orientation_constraints =\n                                        OrientationConstraintGoalset::None());\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Jagged 2D array of `TaskSpaceTarget`s for solving IK problem(s).\n  //!\n  //! The outer dimension of the array represents the number of problems to solve, and the inner\n  //! dimension represents the number of targets for each problem, which may vary in size across\n  //! problems.  All task-space targets must have the same mode (e.g., \"position target and full\n  //! orientation constraint\") but may have different data for each target.\n  //!\n  //! NOTE: The current version supports either multiple targets for a single problem (goalset\n  //! planning) or a single target for multiple problems (single-target batch planning).\n  //! However, using multiple targets for multiple problems (goalset batch planning) is not\n  //! currently supported.\n  struct CUMO_EXPORT TaskSpaceTargetArray {\n    //! Create a task-space target array.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. The number of problems in `translation_constraints` does not match the number of\n    //!      problems in `orientation_constraints`, *OR*\n    //!   2. The number of constraints for any problem in `translation_constraints` does not\n    //!      match the number of constraints for that same problem in `orientation_constraints`.\n    //!\n    //! NOTE: Conditions 1 and 2 are not checked when `orientation_constraints` is unconstrained\n    //! (i.e., `OrientationConstraintArray::None()`). Unconstrained orientation constraints can be\n    //! paired with translation constraints of any size.\n    explicit TaskSpaceTargetArray(const TranslationConstraintArray &translation_constraints,\n                                  const OrientationConstraintArray &orientation_constraints =\n                                      OrientationConstraintArray::None());\n\n    //! Return the number of problems in the array.\n    [[nodiscard]] int numProblems() const;\n\n    //! Return the number of task-space targets for a specific problem in the array.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `problem_index` is out of bounds.\n    [[nodiscard]] int numTargets(int problem_index) const;\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Results from an inverse kinematics solve.\n  class CUMO_EXPORT Results {\n   public:\n    //! Indicate the success or failure of the inverse kinematics solve.\n    enum class Status {\n      //! One or more valid c-space positions found.\n      SUCCESS,\n\n      //! The inverse kinematics solver failed to find a valid solution.\n      INVERSE_KINEMATICS_FAILURE,\n    };\n\n    virtual ~Results() = default;\n\n    //! Return the `Status` from the inverse kinematics solve.\n    [[nodiscard]] virtual Status status() const = 0;\n\n    //! If `status()` returns `SUCCESS`, then `cSpacePositions()` returns unique IK solutions.\n    //!\n    //! A `SUCCESS` status indicates that at least one IK solution was found. Returned\n    //! solutions are ordered from the lowest cost to the highest cost.\n    //!\n    //! If `status()` returns `INVERSE_KINEMATICS_FAILURE`, then an empty vector will be returned.\n    [[nodiscard]] virtual std::vector<Eigen::VectorXd> cSpacePositions() const = 0;\n\n    //! Return the indices of the targets selected for each valid c-space position.\n    //!\n    //! For goalset planning (e.g., `solveGoalset()`), returned values represent indices into\n    //! the constraint vectors used to create the goalset (e.g., `TaskSpaceTargetGoalset`).\n    //!\n    //! For single target planning (e.g., `solve()`), zero will be returned for each valid\n    //! c-space position in `cSpacePositions()`.\n    //!\n    //! In all cases, the length of returned `targetIndices()` will be equal to the length of\n    //! returned `cSpacePositions()`.\n    [[nodiscard]] virtual std::vector<int> targetIndices() const = 0;\n  };\n\n  //! Results from an inverse kinematics solve for multiple problems.\n  class CUMO_EXPORT ResultsArray {\n   public:\n    virtual ~ResultsArray() = default;\n\n    //! Return the number of problems that were successfully solved.\n    //!\n    //! The number of problems with `Results::Status::SUCCESS`.\n    [[nodiscard]] virtual int numSuccesses() const = 0;\n\n    //! Return the number of problems processed by the solver.\n    [[nodiscard]] virtual int numProblems() const = 0;\n\n    //! Return the results of the IK solve for a specific problem.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `problem_index` is out of bounds.\n    [[nodiscard]] virtual std::shared_ptr<const Results> problem(int problem_index) const = 0;\n  };\n\n  //! Attempt to find c-space solutions that satisfy the constraints specified in\n  //! `task_space_target`.\n  //!\n  //! The `cspace_seeds` are optional inputs that can be used to \"warm start\" the IK optimization.\n  //! NOTE: It is *not* required that the `cspace_seeds` represent valid c-space positions (i.e., be\n  //!       within position limits, be collision-free, etc.).\n  //!\n  //! A fatal error will be logged if:\n  //!   1. Any c-space position in `cspace_seeds` does not have the same number of c-space\n  //!      coordinates as the `RobotDescription` used to configure the\n  //!      `CollisionFreeIkSolverConfig`.\n  [[nodiscard]] virtual std::unique_ptr<Results> solve(\n      const TaskSpaceTarget &task_space_target,\n      const std::vector<Eigen::VectorXd> &cspace_seeds = {}) const = 0;\n\n  //! Attempt to find c-space solutions that satisfy the constraints specified in\n  //! `task_space_target_goalset`.\n  //!\n  //! To be considered a valid solution, a c-space position must satisfy the constraints of any\n  //! *one* of the targets specified in `task_space_target_goalset`.\n  //!\n  //! The `cspace_seeds` are optional inputs that can be used to \"warm start\" the IK optimization.\n  //! NOTE: It is *not* required that the `cspace_seeds` represent valid c-space positions (i.e., be\n  //!       within position limits, be collision-free, etc.).\n  //!\n  //! DEPRECATED: This function is deprecated and will be removed in a future version. Use\n  //! `solveArray()` instead. To achieve the same goalset planning functionality with\n  //! `solveArray()`, create a `TaskSpaceTargetArray` with a single problem (i.e., a single element\n  //! in the outer constraint vectors), and place the goalset data as the inner constraint vectors.\n  //! Then call `solveArray()` and inspect the results for the first problem.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. Any c-space position in `cspace_seeds` does not have the same number of c-space\n  //!      coordinates as the `RobotDescription` used to configure the\n  //!      `CollisionFreeIkSolverConfig`.\n  [[nodiscard]] CUMO_DEPRECATED virtual std::unique_ptr<Results> solveGoalset(\n      const TaskSpaceTargetGoalset &task_space_target_goalset,\n      const std::vector<Eigen::VectorXd> &cspace_seeds = {}) const = 0;\n\n  //! Attempt to find c-space solutions for each problem in `task_space_target_array`.\n  //!\n  //! The `cspace_seeds` are optional inputs that can be used to \"warm start\" each IK optimization.\n  //! NOTE: It is *not* required that the `cspace_seeds` represent valid c-space positions (i.e., be\n  //!       within position limits, be collision-free, etc.).\n  //!\n  //! Non-fatal warnings will be logged if:\n  //!   1. The number of `cspace_seeds` is larger than `num_seeds` in the\n  //!      `CollisionFreeIkSolverConfig`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. Any c-space position in `cspace_seeds` does not have the same number of c-space\n  //!      coordinates as the `RobotDescription` used to configure the\n  //!      `CollisionFreeIkSolverConfig`.\n  [[nodiscard]] virtual std::unique_ptr<ResultsArray> solveArray(\n      const TaskSpaceTargetArray &task_space_target_array,\n      const std::vector<Eigen::VectorXd> &cspace_seeds = {}) const = 0;\n};\n\n//! Create a `CollisionFreeIkSolver` with the given `config`.\nCUMO_EXPORT std::unique_ptr<CollisionFreeIkSolver>\nCreateCollisionFreeIkSolver(const CollisionFreeIkSolverConfig &config);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/collision_sphere_generator.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! The `CollisionSphereGenerator` generates a set of spheres to approximate the volume enclosed by\n//! a triangular mesh.\nclass CUMO_EXPORT CollisionSphereGenerator {\n public:\n  virtual ~CollisionSphereGenerator() = default;\n\n  //! Simple representation of a sphere.\n  struct CUMO_EXPORT Sphere {\n    //! 3d coordinates for the center position of the sphere.\n    Eigen::Vector3d center;\n\n    //! Radius of the sphere.\n    double radius;\n  };\n\n  //! Generate a set of `num_spheres` that approximate the volume of the specified mesh.\n  //!\n  //! If `radius_offset` is set to zero, all of the generated spheres will be (approximately)\n  //! tangent to at least two triangles in the mesh. A positive `radius_offset` will increase the\n  //! radii of all spheres by adding `radius_offset` (e.g., a `radius_offset` of 0.05 will allow\n  //! spheres to extend beyond the mesh by up to 0.05). A negative value will shrink the radii of\n  //! the returned spheres (down to a minimum radius of `min_sphere_radius` as described in the\n  //! documentation for `setParam()`).\n  virtual std::vector<Sphere> generateSpheres(int num_spheres, double radius_offset) = 0;\n\n  //! Specify the value for a given parameter.\n  //!\n  //! The required `ParamValue` constructor for each parameter is detailed in the documentation for\n  //! `setParam()`.\n  struct CUMO_EXPORT ParamValue {\n    //! Create `ParamValue` from `int`.\n    ParamValue(int value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `double`.\n    ParamValue(double value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `bool`.\n    ParamValue(bool value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Set the value of the parameter.\n  //!\n  //! If the value has been successfully updated, `true` is returned. If an invalid value has been\n  //! specified for a parameter, a verbose warning is logged, `false` is returned and the parameter\n  //! value is *NOT* updated.\n  //!\n  //! Default parameters are expected to work well for most meshes commonly used to represent\n  //! robots (i.e., generally \"meter scale\"). If the meshes are particularly larger or smaller\n  //! (or represented in units other than meters), then `min_sphere_radius`,\n  //! `convergence_radius_tol`, 'surface_offset`, `min_triangle_area`, and 'num_voxels' can be\n  //! scaled appropriately.\n  //!\n  //! Likewise, the default parameters assume a conventional counter-clockwise winding order for\n  //! vertex indices. If this is *not* the case, `flip_normals` can be set to `true`.\n  //!\n  //! Finally, increasing `num_medial_sphere_samples` may increase the quality of the selected\n  //! spheres (at the expense of more processing time).\n  //!\n  //! The full set of parameters that can be set are:\n  //!\n  //! `num_medial_sphere_samples` [`int`]\n  //!   - The collision sphere generation begins by sampling medial spheres uniformly from the\n  //!     surface of the provided mesh. The medial axis of a mesh is the set of all points having\n  //!     more than one closest point to the mesh. The medial spheres are spheres centered on the\n  //!     medial axis with radius set to the minimum distance from the mesh. By definition, these\n  //!     medial spheres are tangent to at least two faces of the mesh.\n  //!   - These sampled spheres are the set from which the minimal set of spheres to approximate\n  //!     the mesh will be selected. A larger number of samples may result in a better selection of\n  //!     spheres, and more spheres may be valuable if the mesh is particularly complex.\n  //!   - `num_medial_sphere_samples` must be positive.\n  //!   - Default value is 500.\n  //!\n  //! `flip_normals` [`bool`]\n  //!   - The mesh input to the `CollisionSphereGenerator ` is not required to be strictly\n  //!     watertight, but is expected to generally represent a volume. Spheres will be generated by\n  //!     sampling points on the triangular mesh faces and \"growing\" the spheres inwards to find a\n  //!     sphere that is tangent to both the original triangle and some other triangle on the mesh.\n  //!   - It is expected that for any point on any triangular face, a sphere that is tangent to that\n  //!     point on the \"inside\" (negative normal direction) of the triangle with diameter set to the\n  //!     maximum extent on the mesh will intersect at least one other triangle.\n  //!   - The normal is by default computed assuming a counter-clockwise winding direction for\n  //!     vertex indices. If the mesh is input with clockwise winding direction for vertex indices,\n  //!     then `flip_normals` should be set to `true` to reverse winding order.\n  //!   - Default value is `false`.\n  //!\n  //! `min_sphere_radius` [`double`]\n  //!   - When generating medial spheres, any spheres that are found to have radius less than\n  //!     `min_sphere_radius` will be discarded.\n  //!   - NOTE: Discarded spheres still count towards `num_medial_sphere_samples`. E.g., if\n  //!     `generateSpheres()` is called with `num_medial_sphere_samples` = 500, and 20 spheres are\n  //!     discarded for being too small, then the actual number of spheres that will be used to\n  //!     select the final spheres is 480.\n  //!   - `min_sphere_radius` must be positive.\n  //!   - Default value is 1e-3.\n  //!\n  //! `seed` [`int`]\n  //!   - Random seed used for sampling surface points on mesh from which to sample spheres.\n  //!   - `seed` must be positive.\n  //!   - Default value is 12345.\n  //!\n  //! `max_iterations` [`int`]\n  //!   - Maximum number of iterations of binary search used to approximate the radius of each\n  //!     medial sphere.\n  //!   - NOTE: If `max_iterations` is reached, the sphere will be discarded. Discarded spheres\n  //!     still count towards `num_medial_sphere_samples`. E.g., if `generateSpheres()` is called\n  //!     with `num_medial_sphere_samples` = 500, and 20 spheres are discarded for not converging to\n  //!     the medial sphere radius tolerance within the `max_iterations`, then the actual number of\n  //!     spheres that will be used to select the final spheres is 480.\n  //!   - `max_iterations` must be positive.\n  //!   - Default value is 100.\n  //!\n  //! `convergence_radius_tol` [`double`]\n  //!   - Convergence criteria for binary search used to approximate the radius of each medial\n  //!     sphere. The search will end when the sphere radius is within `convergence_radius_tol` of\n  //!     the actual medial sphere radius.\n  //!   - `convergence_radius_tol` must be positive.\n  //!   - Default value is 1e-3.\n  //!\n  //! `surface_offset` [`double`]\n  //!   - When points are sampled from the mesh surface to generate medial spheres, they are offset\n  //!     by `surface_offset` towards the \"inside\" of the mesh in order to avoid collisions with\n  //!     faces very close to the sampling face. This is to help avoid very small spheres that would\n  //!     be culled due to the `min_sphere_radius`.\n  //!   - `surface_offset` must be positive.\n  //!   - Default value is 1e-4.\n  //!\n  //! `min_triangle_area` [`double`]\n  //!   - Triangles with area below `min_triangle_area` are discarded when creating the mesh for\n  //!     generating medial spheres. This includes degenerate triangles where surface area\n  //!     approaches zero as well as relatively small triangles that will increase computation cost\n  //!     without being likely to improve the set of sampled spheres.\n  //!   - `min_triangle_area` must be positive.\n  //!   - Default value is 1e-8.\n  //!\n  //!  `num_voxels` [`int`]\n  //!    - A voxel grid is used to estimate volume additions from each sphere when selecting spheres\n  //!      from the full sample of medial spheres. The `num_voxels` provides a trade-off between\n  //!      accurate volume approximation (with larger `num_voxels`) and faster selection (with\n  //!      smaller `num_voxels`).\n  //!    - `num_voxels` specifies the number of voxels along the longest dimension of an\n  //!      axis-aligned bounding box (AABB) encompassing the specified mesh. For example, if the\n  //!      AABB has dimensions {20, 50, 30} and `num_voxels is set to 100, then each voxel will\n  //!      have side length 50 / 100 = 0.5. Thus, the number of voxels along each axis of the grid\n  //!      will be [40, 100, 60].\n  //!    - `num_voxels` must be positive.\n  //!    - Default value is 50.\n  virtual bool setParam(const std::string &param_name, ParamValue value) = 0;\n\n  //! Return the number of validated triangles that have been included in the mesh used for sampling\n  //! spheres to approximate volume.\n  //! NOTE: The number of returned triangles may be less than the number of `triangles` passed to\n  //!       `CreateCollisionSphereGenerator()` if triangles are discarded for invalid indices or\n  //!       having an area smaller than `min_triangle_area` (see `setParam()` documentation for\n  //!       details).\n  virtual int numTriangles() = 0;\n\n  //! Return all of the medial axis spheres used to approximate the volume of the mesh. The spheres\n  //! returned by `generateSpheres()` are selected from this set.\n  //! NOTE: This function is intended primarily for debugging functionality and/or tuning parameters\n  //!       in `setParam()` and is not likely to be needed by most users.\n  virtual std::vector<Sphere> getSampledSpheres() = 0;\n};\n\n//! Create a `CollisionSphereGenerator` to generate spheres that approximate the volume of a mesh\n//! represented by `vertices` and `triangles`.\n//!\n//! Each vertex in `vertices` is a set of (x,y,z) coordinates that can be referenced by one or more\n//! triangles in `triangles`. Each triangle is a set of indices in the `vertices` vector.\n//!\n//! Vertex indices for a given triangle are considered valid if:\n//!   [a] The index for each vertex is in range [0, vertices.size()), and\n//!   [b] The same index is *NOT* included twice.\n//!\n//! Triangles with invalid indices will be discarded with verbose warnings.\n//!\n//! Additionally, each triangle in `triangles` is tested to ensure that its area is greater than or\n//! equal to `min_triangle_area` (see `CollisionSphereGenerator::setParam()` documentation for\n//! details). Triangles that are too small will be discarded.\n//!\n//! By default, the triangle normals will be computed assuming a counter-clockwise winding\n//! direction. This convention can be flipped by setting `flip_normals` (see\n//! `CollisionSphereGenerator::setParam()` documentation for details).\nCUMO_EXPORT std::unique_ptr<CollisionSphereGenerator> CreateCollisionSphereGenerator(\n    const std::vector<Eigen::Vector3d> &vertices,\n    const std::vector<Eigen::Vector3i> &triangles);\n\n//! Default input parameters for `GenerateCollisionSpheres()`.\nconstexpr double kCollisionSphereDefaultMaxOvershoot = 0.05;\nconstexpr int kCollisionSphereDefaultMaxNumSpheres = 3000;\nconstexpr double kCollisionSphereDefaultSurfacePointDensity = 30000.;\nconstexpr int kCollisionSphereDefaultSurfacePointSamplingSeed = 123;\nconstexpr bool kCollisionSphereDefaultFlipNormals = false;\nconstexpr double kCollisionSphereDefaultMinTriangleArea = 1e-8;\n\n//! Generate a selection of collision spheres from a grid of points filling a triangle mesh.\n//!\n//! NOTE: For most meshes, this `GenerateCollisionSpheres()` function is expected to perform better\n//!       than `CollisionSphereGenerator()` (which is likely to be deprecated).\n//!\n//! The mesh is represented by `triangles` and `vertices`, where each triangle in `triangles` is a\n//! set of indices referencing points in `vertices`. At least three `vertices` and at least one\n//! triangle must be provided.\n//!\n//! The `max_overshoot` serves as an upper bound for how far any collision sphere may extend beyond\n//! the surface of the mesh. The units of the `max_overshoot` are the same as the mesh, typically\n//! meters for robotics applications. The `max_overshoot` must be positive.\n//!\n//! Larger `max_overshoot` values will create conservative collision sphere representations with\n//! relatively few spheres (increasing performance for motion generation and collision checking).\n//! Smaller `max_overshoot` values will fit spheres more tightly to the mesh and will (in general)\n//! require more spheres to fit a given mesh (reducing performance for motion generation and\n//! collision checking).\n//!\n//! The `max_num_spheres` sets an upper bound on the number of candidate spheres from which\n//! collision spheres will be selected. Specifically, the `max_num_spheres` is used to generate a\n//! grid of points filling the axis-aligned bounding box enclosing the mesh. These points serve as\n//! potential sphere centers. The `max_num_spheres` may need to be increased from the default value\n//! for particularly large meshes or for particularly small `max_overshoot` values. Increasing\n//! `max_num_spheres` will (in general) improve the selection of collision spheres, but result in\n//! slower generation time. The `max_num_spheres` must be positive.\n//!\n//! The goal of sphere selection is to select spheres such that the entire mesh surface is\n//! enclosed by one or more spheres. As an approximation, points are distributed uniformly on the\n//! mesh surface and used to test whether the surface is fully enclosed. The `surface_point_density`\n//! determines how many test point are distributed. The units for `surface_point_density` are\n//! 1 / [mesh-length-unit]^2. For the typical robotics case (with the mesh represented in meters),\n//! the default value corresponds to 3 surface points per cm^2. Using higher values for\n//! `surface_point_density` will increase the likelihood of full surface enclosure (at the cost of\n//! slower generation time). The `surface_point_density` must be positive. Changing the parameter\n//! `surface_point_sampling_seed` will result in different surface points being selected and cause\n//! different candidate spheres to be selected. This seed may be used, e.g.,  to randomly generate\n//! multiple sphere sets with the same `max_overshoot`.\n//!\n//! The mesh input to `GenerateCollisionSpheres()` is not required to be strictly watertight\n//! but is expected to generally represent a volume. For each triangle, the normal is (by default)\n//! computed assuming a counter-clockwise winding direction for vertex indices. If the mesh is input\n//! with clockwise winding direction for vertex indices, then `flip_normals` should be set to\n//! `true` to reverse winding order. The correct surface normal orientation for `triangles` is\n//! required for accurately determining whether grid points are interior or exterior to the mesh.\n//!\n//! Very small and/or degenerate triangles are unlikely to aid in collision sphere generation. Thus,\n//! triangles with surface area less than `min_triangle_area` are culled prior to sphere generation\n//! and selection. The `min_triangle_area` must be positive.\nCUMO_EXPORT std::vector<CollisionSphereGenerator::Sphere> GenerateCollisionSpheres(\n    const std::vector<Eigen::Vector3d> &vertices,\n    const std::vector<Eigen::Vector3i> &triangles,\n    double max_overshoot = kCollisionSphereDefaultMaxOvershoot,\n    int max_num_spheres = kCollisionSphereDefaultMaxNumSpheres,\n    double surface_point_density = kCollisionSphereDefaultSurfacePointDensity,\n    int surface_point_sampling_seed = kCollisionSphereDefaultSurfacePointSamplingSeed,\n    bool flip_normals = kCollisionSphereDefaultFlipNormals,\n    double min_triangle_area = kCollisionSphereDefaultMinTriangleArea);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/composite_path_spec.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cspace_path_spec.h\"\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/task_space_path_spec.h\"\n\nnamespace cumotion {\n\n//! The `CompositePathSpec` is used to procedurally composite `CSpacePathSpec` and\n//! `TaskSpacePathSpec` segments into a single path specification.\nclass CUMO_EXPORT CompositePathSpec {\n public:\n  virtual ~CompositePathSpec() = default;\n\n  //! Return the number of configuration space coordinates for the path specification.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Return the number of path specifications contained in the `CompositePathSpec`.\n  [[nodiscard]] virtual int numPathSpecs() const = 0;\n\n  //! Indicate whether a path specification is a `TaskSpacePathSpec` (i.e., `TASK_SPACE`) or a\n  //! `CSpacePathSpec` (i.e., `CSPACE`).\n  enum class PathSpecType {\n    TASK_SPACE,\n    CSPACE\n  };\n\n  //! Given a `path_spec_index` in range [0, `numPathSpecs()`), return the type of the corresponding\n  //! path specification.\n  //!\n  //! A fatal error will be logged if `path_spec_index` is not in range [0, `numPathSpecs()`).\n  //!\n  //! The `path_spec_index` corresponds to the order in which path specifications are added to the\n  //! `CompositePathSpec`.\n  [[nodiscard]] virtual PathSpecType pathSpecType(int path_spec_index) const = 0;\n\n  //! Return a `TaskSpacePathSpec` at the given `path_spec_index`.\n  //!\n  //! If the `path_spec_index` is invalid (i.e., not in range `[0, `numPathSpecs()`)) *OR* the\n  //! `path_spec_index` does not correspond to a `TaskSpacePathSpec`, then `nullptr` will be\n  //! returned and an error will be logged.\n  [[nodiscard]]\n  virtual std::unique_ptr<TaskSpacePathSpec> taskSpacePathSpec(int path_spec_index) const = 0;\n\n  //! Return a `CSpacePathSpec` at the given `path_spec_index`.\n  //!\n  //! If the `path_spec_index` is invalid (i.e., not in range `[0, `numPathSpecs()`)) *OR* the\n  //! `path_spec_index` does not correspond to a `CSpacePathSpec`, then `nullptr` will be\n  //! returned and an error will be logged.\n  [[nodiscard]]\n  virtual std::unique_ptr<CSpacePathSpec> cSpacePathSpec(int path_spec_index) const = 0;\n\n  //! Specify the transition preceding a `TaskSpacePathSpec` or `CSpacePathSpec`.\n  enum class TransitionMode {\n    //! Skip either the initial task space pose of the `TaskSpacePathSpec` or the initial c-space\n    //! configuration of the `CSpacePathSpec`.\n    //!\n    //! For a `TaskSpacePathSpec`, the first task space path segment in the appended\n    //! `TaskSpacePathSpec` will instead originate from the current task space pose of the\n    //! `CompositePathSpec`.\n    //!\n    //! For a `CSpacePathSpec`, The first c-space waypoint of the added `CSpacePathSpec` will be\n    //! added directly after the current c-space configuration of the `CompositePathSpec`.\n    SKIP,\n\n    //! Add a path from the current pose of the `CompositePathSpec` to the initial task space pose\n    //! of the `TaskSpacePathSpec` or the initial c-space configuration of the `CSpacePathSpec`,\n    //! with no restrictions on the form of the path.\n    FREE,\n\n    //! Add a path that is linear in task space from the current task space pose of the\n    //! `CompositePathSpec` to the initial task space pose of the `TaskSpacePathSpec`.\n    //!\n    //! *WARNING* This mode is *ONLY* available for adding a `TaskSpacePathSpec`\n    //! (via `addTaskSpacePathSpec()`). Usage with `addCSpacePathSpec()` will result in an error\n    //! and the `CSpacePathSpec` will *NOT* be added.\n    LINEAR_TASK_SPACE\n  };\n\n  //! Given a `path_spec_index` in range [0, `numPathSpecs()`), return the corresponding transition\n  //! mode.\n  //!\n  //! A fatal error will be logged if `path_spec_index` is not in range [0, `numPathSpecs()`).\n  //!\n  //! The `path_spec_index` corresponds to the order in which path specifications are added to the\n  //! `CompositePathSpec`.\n  [[nodiscard]] virtual TransitionMode transitionMode(int path_spec_index) const = 0;\n\n  //! Add a task space `path_spec` to the `CompositePathSpec` with the specified `transition_mode`.\n  //!\n  //! Returns `true` if path specification is successfully added. Else, returns `false`.\n  virtual bool addTaskSpacePathSpec(const TaskSpacePathSpec &path_spec,\n                                    TransitionMode transition_mode) = 0;\n\n  //! Add a c-space `path_spec` to the `CompositePathSpec` with the specified `transition_mode`.\n  //!\n  //! `path_spec` will be discarded (with logged error) if it does not have the same number of\n  //! c-space coordinates as the `CompositePathSpec` (i.e., `numCSpaceCoords()`).\n  //!\n  //! `path_spec` will be discarded (with logged error) if the `transition_mode` is invalid\n  //! (i.e., `LINEAR_TASK_SPACE`).\n  //!\n  //! Returns `true` if path specification is successfully added. Else, returns `false`.\n  virtual bool addCSpacePathSpec(const CSpacePathSpec &path_spec,\n                                 TransitionMode transition_mode) = 0;\n};\n\n//! Create a `CompositePathSpec` with the specified `initial_cspace_position`.\nCUMO_EXPORT std::unique_ptr<CompositePathSpec> CreateCompositePathSpec(\n    const Eigen::VectorXd &initial_cspace_position);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/cspace_path.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Represent a path through configuration space (i.e., c-space or \"joint space\").\n//!\n//! This path is parameterized by independent variable `s` and is generally expected to be\n//! continuous, but not necessarily smooth.\nclass CUMO_EXPORT CSpacePath {\n public:\n  virtual ~CSpacePath() = default;\n\n  //! Return the number of configuration space coordinates for the path.\n  virtual int numCSpaceCoords() const = 0;\n\n  //! Indicates continuous range of independent values, `s`, for which the path is defined.\n  struct CUMO_EXPORT Domain {\n    //! Minimum value of `s` defining domain.\n    double lower;\n\n    //! Maximum value of `s` defining domain.\n    double upper;\n\n    //! Convenience function to return the span of `s` values included in domain.\n    [[nodiscard]] double span() const { return upper - lower; }\n  };\n\n  //! Return the domain for the path.\n  [[nodiscard]] virtual Domain domain() const = 0;\n\n  //! Evaluate the path at the given `s`.\n  //!\n  //! A fatal error is logged if `s` is outside of the path `domain()`.\n  [[nodiscard]] virtual Eigen::VectorXd eval(double s) const = 0;\n\n  //! Return the total distance accumulated along the path, where distance is computed using the\n  //! L2-norm.\n  //!\n  //! This length is not, in general, equal to the distance between the configurations at the lower\n  //! and upper bounds of the domain.\n  [[nodiscard]] virtual double pathLength() const = 0;\n\n  //! Return the minimum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These minimum position values are not, in general, synchronous. Instead, they represent\n  //!       the minimum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd minPosition() const = 0;\n\n  //! Return the maximum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These maximum position values are not, in general, synchronous. Instead, they represent\n  //!       the maximum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd maxPosition() const = 0;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/cspace_path_spec.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! The `CSpacePathSpec` is used to procedurally specify a `CSpacePath` from a series of\n//! configuration space points.\nclass CUMO_EXPORT CSpacePathSpec {\n public:\n  virtual ~CSpacePathSpec() = default;\n\n  //! Return the number of configuration space coordinates for the path specification.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Add a path to connect the previous configuration to the `waypoint`.\n  //!\n  //! The `waypoint` must have dimension equal to `numCSpaceCoords()` or it will be discarded.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addCSpaceWaypoint(const Eigen::VectorXd &waypoint) = 0;\n};\n\n//! Create a `CSpacePathSpec` with the specified `initial_cspace_position`.\nCUMO_EXPORT std::unique_ptr<CSpacePathSpec> CreateCSpacePathSpec(\n    const Eigen::VectorXd &initial_cspace_position);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/cumotion.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2019-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Shared interface functions for the cuMotion library.\n\n#pragma once\n\n#include <exception>\n#include <memory>\n#include <ostream>\n#include <string>\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/version.h\"\n\nnamespace cumotion {\n\n//! @brief Get cuMotion library version as a `std::string`.\n//!\n//! Also see the `CUMOTION_VERSION_STRING` macro, which serves a similar purpose.  The two should\n//! always agree.  If they don't, it indicates a mismatch between the installed cuMotion headers\n//! and installed cuMotion libraries.\nCUMO_EXPORT std::string VersionString();\n\n// On Windows, `wingdi.h` (included transitively via `windows.h` and certain CUDA headers)\n// contains a `#define ERROR 0` that collides with the `LogLevel::ERROR` enum value.\n// The following temporarily undefines the macro.\n#ifdef _WIN32\n#  pragma push_macro(\"ERROR\")\n#  undef ERROR\n#endif\n\n//! @brief Logging levels, ordered from least to most verbose.\nenum class CUMO_EXPORT LogLevel {\n  FATAL,    //!< Logging level for nonrecoverable errors (minimum level, so always enabled).\n  ERROR,    //!< Logging level for recoverable errors.\n  WARNING,  //!< Logging level for warnings, indicating possible cause for concern.\n  INFO,     //!< Logging level for informational messages.\n  VERBOSE   //!< Logging level for highly verbose informational messages.\n};\n\n#ifdef _WIN32\n#  pragma pop_macro(\"ERROR\")\n#endif\n\n//! @brief Base class for user-defined logger that allows custom handling of log messages, warnings,\n//! and errors.\n//!\n//! Such a custom logger may be installed via `SetLogger()`.\nclass CUMO_EXPORT Logger {\n public:\n  virtual ~Logger() = default;\n\n  //! Return an `ostream` suitable for logging messages at the given `log_level`.\n  virtual std::ostream &log(LogLevel log_level) = 0;\n\n  //! Perform any required post-processing of individual log messages.  This function is called\n  //! after the message has been logged to the `std::ostream` returned by `log()` but before\n  //! `handleFatalError()` is called (if applicable).\n  virtual void finalizeLogMessage([[maybe_unused]] LogLevel log_level) {\n    // Do nothing by default.\n  }\n\n  //! Handle fatal errors.  This callback will be called after any associated error message has\n  //! been written to the ostream returned by `log(FATAL)`.  Because fatal errors are\n  //! non-recoverable, this function must ensure that any active cuMotion objects are discarded.\n  virtual void handleFatalError() = 0;\n};\n\n//! @brief Exception thrown by the default logger for fatal errors.\n//!\n//! This is the only exception thrown by cuMotion, so clients may avoid exceptions completely by\n//! installing a custom logger.\nclass CUMO_EXPORT FatalException : public std::exception {\n public:\n  explicit FatalException(const std::string &message) : message_(message) {}\n\n  [[nodiscard]] const char *what() const noexcept override { return message_.c_str(); }\n\n private:\n  std::string message_;\n};\n\n//! @brief Suppress output for all log messages with associated verbosity higher than `log_level`.\n//!\n//! Messages suppressed in this way incur very low overhead, so it's best to take advantage of\n//! this facility even if a custom `Logger` is provided.\n//!\n//! Until `SetLogLevel()` is called, the default log level is `WARNING`.  The lowest supported\n//! `log_level` is `FATAL`, since it is not possible to supress fatal errors.\nCUMO_EXPORT void SetLogLevel(LogLevel log_level);\n\n//! @brief Install a custom logger, derived from the above `Logger` class.\n//!\n//! If `logger` is a null pointer, then the default logger is reenabled.  The default logger\n//! directs all output to stdout and throws a `FatalException` in the event of a fatal error.\nCUMO_EXPORT void SetLogger(std::shared_ptr<Logger> logger = nullptr);\n\n//! Set color/style used by the default logger for messages of a given `log_level`.  The `style`\n//! string may contain one or more ANSI control sequences (e.g., enabling fatal error messages to\n//! be rendered in bold with a red foreground color).\n//!\n//! For convenience, a selection of common control sequences is provided in\n//! include/cumotion/text_style.h\nCUMO_EXPORT void SetDefaultLoggerTextStyle(LogLevel log_level, const std::string &style);\n\n//! Set prefix used by the default logger for all messages. The `prefix` string is logged after\n//! the `style` string set for each `log_level` by `SetDefaultLoggerTextStyle()` and prior to the\n//! content of the logged message.\n//!\n//! The default prefix is an empty string.\nCUMO_EXPORT void SetDefaultLoggerPrefix(const std::string &prefix);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/cumotion_export.h",
    "content": "\n#ifndef CUMO_EXPORT_H\n#define CUMO_EXPORT_H\n\n#ifdef CUMO_STATIC_DEFINE\n#  define CUMO_EXPORT\n#  define CUMO_NO_EXPORT\n#else\n#  ifndef CUMO_EXPORT\n#    ifdef cumotion_cumotion_EXPORTS\n        /* We are building this library */\n#      define CUMO_EXPORT __attribute__((visibility(\"default\")))\n#    else\n        /* We are using this library */\n#      define CUMO_EXPORT __attribute__((visibility(\"default\")))\n#    endif\n#  endif\n\n#  ifndef CUMO_NO_EXPORT\n#    define CUMO_NO_EXPORT __attribute__((visibility(\"hidden\")))\n#  endif\n#endif\n\n#ifndef CUMO_DEPRECATED\n#  define CUMO_DEPRECATED __attribute__ ((__deprecated__))\n#endif\n\n#ifndef CUMO_DEPRECATED_EXPORT\n#  define CUMO_DEPRECATED_EXPORT CUMO_EXPORT CUMO_DEPRECATED\n#endif\n\n#ifndef CUMO_DEPRECATED_NO_EXPORT\n#  define CUMO_DEPRECATED_NO_EXPORT CUMO_NO_EXPORT CUMO_DEPRECATED\n#endif\n\n#if 0 /* DEFINE_NO_DEPRECATED */\n#  ifndef CUMO_NO_DEPRECATED\n#    define CUMO_NO_DEPRECATED\n#  endif\n#endif\n#ifdef _MSC_VER\n#  define CUMO_EXTERN_TEMPLATE_EXPORT\n#  define CUMO_TEMPLATE_EXPORT CUMO_EXPORT\n#else\n#  define CUMO_EXTERN_TEMPLATE_EXPORT CUMO_EXPORT\n#  define CUMO_TEMPLATE_EXPORT\n#endif\n\n#endif /* CUMO_EXPORT_H */\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/ik_solver.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for inverse kinematics solvers.\n\n#pragma once\n\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/kinematics.h\"\n\nnamespace cumotion {\n\n//! Configuration for solving inverse kinematics.\nstruct CUMO_EXPORT IkConfig {\n  //! Construct configuration with default values likely to be appropriate for most 6- and 7-dof\n  //! robotic arms.\n  IkConfig() = default;\n\n  //! The maximum number of c-space positions that will be used as seeds (i.e., initial positions)\n  //! for cyclic coordinate descent.\n  //!\n  //! The inverse kinematic solver will terminate when a valid c-space configuration is found OR\n  //! the number of attempted descents reaches `max_num_descents`.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 100\n  int max_num_descents = 100;\n\n  //! Optional parameter to set the initial c-space configurations for each descent.\n  //!\n  //! These `cspace_seeds` will be used to attempt cyclic coordinate descent in the order\n  //! provided, terminating before all `cspace_seeds` are tried if a valid `cspace_position` is\n  //! found.\n  //!\n  //! If the number of attempted descents is greater than the number of provided `cspace_seeds`,\n  //! random starting configurations will be generated (see `irwin_hall_sampling_order` for\n  //! details).\n  //!\n  //! It is valid to provide no `cspace_seeds`. In this case all initial configurations will be\n  //! randomly sampled.\n  std::vector<Eigen::VectorXd> cspace_seeds = {};\n\n  //! If the number of attempted descents exceeds the provided number of `cspace_seeds` (including\n  //! the case where no `cspace_seeds` are provided), then random c-space configurations will be\n  //! sampled for each descent.\n  //!\n  //! Each c-space coordinate value will be sampled from an Irwin-Hall distribution spanning the\n  //! joint limits. The distribution mean will always be set to the midpoint between upper and\n  //! lower joint limits.\n  //!\n  //! Setting `irwin_hall_sampling_order` to 1 corresponds to a uniform distribution, setting to\n  //! 2 corresponds to a triangular distribution, and higher values approximate a normal\n  //! distribution.\n  //!\n  //! See https://en.wikipedia.org/wiki/Irwin%E2%80%93Hall_distribution for reference, noting that\n  //! the internal distribution is scaled s.t. all sampled values will be within joint limits.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 2\n  int irwin_hall_sampling_order = 2;\n\n  //! Seed for random number generator used for Irwin-Hall sampling for initial c-space positions.\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 123456\n  int sampling_seed = 123456;\n\n  //! Maximum position error (L2-norm) of the task space pose for a successful IK solution.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 1e-3\n  double position_tolerance = 1e-3;\n\n  //! Maximum orientation error of the task space pose for a successful IK solution.\n  //!\n  //! For each axis in task space, error is defined as the L2-norm of the difference between the\n  //! axis direction and the target pose axis direction.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 0.01\n  double orientation_tolerance = 0.01;\n\n  //================================================================================================\n  // Parameters for cyclic coordinate descent (CCD) algorithm.\n\n  //! The number of iterations used for cyclic coordinate descent from each initial c-space\n  //! position (i.e., seed).\n  //!\n  //! Smaller values will decrease the amount of computation time per descent, but may require\n  //! more descents to converge to valid c-space configuration.\n  //!\n  //! CCD will be disabled for values less than 1.\n  //!\n  //! Default: 10\n  int ccd_max_iterations = 10;\n\n  //! Each descent can terminate early if the L2-norm of the change to c-space coordinates is less\n  //! than the `descent_termination_delta`.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 1e-1\n  double ccd_descent_termination_delta = 1e-1;\n\n  //! Weight for the relative importance of position error term when performing\n  //! cyclic coordinate descent.\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 1.0\n  double ccd_position_weight = 1.0;\n\n  //! Weight for the relative importance of orientation error term when performing\n  //! cyclic coordinate descent.\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 0.05\n  double ccd_orientation_weight = 0.05;\n\n  //! Number of samples used to identify valid three-point bracket for numerical optimization of\n  //! c-space updates.\n  //!\n  //! NOTE: This parameter is *ONLY* used when more than one active upstream joint is controlled by\n  //! a single c-space coordinate.\n  //!\n  //! For the \"many-to-one joint-to-cspace\" case, quadratic fit search (QFS) is used to approximate\n  //! an optimal c-space update during each iteration of cyclic coordinate descent. In order to\n  //! find a valid three-point bracket for QFS, N points are sampled to attempt to find a region\n  //! with minimum error. This sampling is incorporated into the numerical solver since, in\n  //! general, the error function will not be unimodal within the domain bounded by the c-space\n  //! position limits.\n  //!\n  //! Must be greater than one.\n  //!\n  //! Default: 10\n  int ccd_bracket_search_num_uniform_samples = 10;\n\n  //================================================================================================\n  // Parameters for Broyden-Fletcher-Goldfarb-Shanno (BFGS) solver.\n\n  //! The number of iterations used for Broyden-Fletcher-Goldfarb-Shanno (BFGS) descent from each\n  //! initial c-space position (i.e., seed).\n  //!\n  //! Smaller values will decrease the amount of computation time per descent, but may require\n  //! more descents to converge to valid c-space configuration.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 60\n  int bfgs_max_iterations = 60;\n\n  //! The BFGS solver will terminate if the L2-norm of the error function gradient at the current\n  //! best c-space position is less than `bfgs_gradient_norm_termination`.\n  //!\n  //! Low values for `bfgs_gradient_norm_termination` will increase solver accuracy, while high\n  //! values will decrease solve times for a given problem.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 1e-6\n  double bfgs_gradient_norm_termination = 1e-6;\n\n  //! The BFGS solver is implemented as a two-step process, with a coarse solve used to identify\n  //! whether convergence to a valid c-space position is likely.\n  //!\n  //! For the coarse solve, the `bfgs_gradient_norm_termination` is multiplied by the\n  //! `bfgs_gradient_norm_termination_coarse_scale_factor` such that the optimization will end\n  //! early.\n  //!\n  //! Must be greater than or equal to 1.\n  //!\n  //! Default: 1e7\n  double bfgs_gradient_norm_termination_coarse_scale_factor = 1e7;\n\n  //! The error function for BFGS descent optionally includes a cost-term for avoiding c-space\n  //! coordinate values near position limits.\n  //!\n  //! If set to `ENABLE`, this cost term will always be turned on. If set to `DISABLE`, this cost\n  //! term will always be turned off. Setting to `AUTO` will turn this cost term on for systems\n  //! with 7 or more degrees-of-freedom (DoF), while leaving it off for systems with 6 or fewer\n  //! DoF. NOTE: DoF is defined as the numer of c-space coordinates upstream from the target frame.\n  //!\n  //! Setting to `AUTO` is likely to provide desirable results for most common robots. Redundant\n  //! mechanisms with less than 7-DoF are a potential exception. E.g., a 4-Dof robot restricted to\n  //! planar motion may benefit from enabling a cost term to avoid position limits.\n  enum class CSpaceLimitBiasing {\n    AUTO,\n    ENABLE,\n    DISABLE,\n  };\n\n  //! See documentation for `CSpaceLimitBiasing`.\n  //!\n  //! Default: AUTO\n  CSpaceLimitBiasing bfgs_cspace_limit_biasing = CSpaceLimitBiasing::AUTO;\n\n  //! Define the region near c-space limits which will induce penalties when c-space limit biasing\n  //! is active.\n  //!\n  //! The region is defined as a fraction of the c-space span for each c-space coordinate.\n  //! For example, imagine a 2d system for which the limits of the first coordinate are [-5, 5] and\n  //! the limits of the second coordinate are [0, 2]. If the `cspace_limit_penalty_region` is set\n  //! to 0.1, then the size of each penalty region for first coordinate will be:\n  //!   0.1 * (5 - (-5)) = 1\n  //! Similarly, the size of each penalty region for the second coordinate will be:\n  //!   0.1 * (2 - 0) = 0.2\n  //! This means that for the first coordinate, c-space limit errors will be active from [-5, -4]\n  //! and [4, 5]. For the second coordinate, c-space limit errors will be active from [0, 0.2] and\n  //! [1.8, 2]\n  //!\n  //! Must be in the range [0, 0.5].\n  //!\n  //! Default: 0.01\n  double bfgs_cspace_limit_penalty_region = 0.01;\n\n  //! Relative weight applied to c-space limit error (if active).\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 1.0\n  double bfgs_cspace_limit_biasing_weight = 1.0;\n\n  //! Weight for the relative importance of position error term when performing BFGS descent.\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 1.0\n  double bfgs_position_weight = 1.0;\n\n  //! Weight for the relative importance of orientation error term when performing BFGS descent.\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 100.0\n  double bfgs_orientation_weight = 100.0;\n};\n\n//! Results from solving inverse kinematics.\nstruct CUMO_EXPORT IkResults {\n  //! Indicate whether a valid `cspace_position` within the tolerances specified in the\n  //! `IkConfig` has been found.\n  bool success;\n\n  //! If `success` is set to `true`, `cspace_position` will contain a valid joint configuration\n  //! that corresponds to the `target_pose` passed to the IK solver within the tolerances\n  //! specified in the `IkConfig`.\n  Eigen::VectorXd cspace_position;\n\n  //! Position error (L2-norm) of the task space pose corresponding to the returned\n  //! `cspace_position`\n  //!\n  //! If `success` is set to `true`, the `position_error` will be less than or equal to the\n  //! `position_tolerance` set in the `IkConfig`.\n  double position_error;\n\n  //! X-axis orientation error (L2-norm) of the task space pose corresponding to the returned\n  //! `cspace_position`\n  //!\n  //! If `success` is set to `true`, the `x_axis_orientation_error` will be less than or equal to\n  //! the `orientation_tolerance` set in the `IkConfig`.\n  double x_axis_orientation_error;\n\n  //! Y-axis orientation error (L2-norm) of the task space pose corresponding to the returned\n  //! `cspace_position`\n  //!\n  //! If `success` is set to `true`, the `y_axis_orientation_error` will be less than or equal to\n  //! the `orientation_tolerance` set in the `IkConfig`.\n  double y_axis_orientation_error;\n\n  //! Z-axis orientation error (L2-norm) of the task space pose corresponding to the returned\n  //! `cspace_position`\n  //!\n  //! If `success` is set to `true`, the `z_axis_orientation_error` will be less than or equal to\n  //! the `orientation_tolerance` set in the `IkConfig`.\n  double z_axis_orientation_error;\n\n  //! The number of unique c-space positions that were used for attempting cyclic coordinate\n  //! descent prior to the IK solver terminating.\n  //!\n  //! If `success` is set to `true`, this will be the number\n  //! of descents that were attempted in order to get a `cspace_position` within the provided\n  //! tolerance. If `success` is set to `false`, the number of descents will be set to the\n  //! `max_num_descents` set in the `IkConfig`.\n  int num_descents;\n};\n\n//! Attempt to solve inverse kinematics.\n//!\n//! A `target_pose` is provided for the task space `target_frame` and the solver attempts to find\n//! a corresponding set of c-space coordinates. A valid c-space configuration must not exceed\n//! joint limits set in the kinematic structure and must be the target pose tolerances specified\n//! within `config`.\n//!\n//! If the `target_frame` is selected such that some of the c-space coordinates do not affect the\n//! pose of the target frame, these \"downstream\" coordinates will be ignored by the IK solver. For\n//! the case where the successful descent was started from a seed provided in `config.cspace_seeds`,\n//! the \"downstream\" coordinates will not be altered from the provided seed. For the case where\n//! a random seed is used, the \"downstream\" coordinates will be returned with values between their\n//! respective c-space position limits. This behavior is intended to support cases such as branched\n//! kinematic structures (i.e., to solve for palm pose of a robotic system with an arm and multiple\n//! fingers) or a redundant serial linkage for which it is desired to set a pose of a frame not\n//! rigidly fixed to the end effector.\n//!\n//! This implementation of cyclic coordinate descent (CCD) is based on \"A Combined Optimization\n//! Method for Solving the Inverse Kinematics Problem of Mechanical Manipulators\" (1991) by\n//! Wang et al.\n//! Ref: http://web.cse.ohio-state.edu/~parent.1/classes/788/Sp06/ReferenceMaterial/IK/WC91.pdf\n//!\n//! As described by Wang et al., this \"combined\" method uses cyclic coordinate descent (CCD) as a\n//! quick method to find a feasible c-space position that is in the neighborhood of an exact\n//! solution to the inverse kinematics problem. These nearby c-space positions are then used to\n//! \"warm start\" the Broyden-Fletcher-Goldfarb-Shanno (BFGS) algorithm. Given a point sufficiently\n//! near an exact solution, the BFGS algorithm is able to leverage an estimate of curvature to\n//! rapidly converge to a very accurate approximation of the exact solution.\n//!\n//! Concise explanations of the CCD algorithm with interactive demonstrations are available at:\n//! [1] https://zalo.github.io/blog/inverse-kinematics/\n//! [2] http://rodolphe-vaillant.fr/?e=114\nCUMO_EXPORT IkResults SolveIk(const Kinematics &kinematics,\n                              const Pose3 &target_pose,\n                              const Kinematics::FrameHandle &target_frame,\n                              const IkConfig &config);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/kinematics.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2019-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for querying a kinematic structure.\n\n#pragma once\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/pose3.h\"\n\nnamespace cumotion {\n\n//! Class representing the mapping from configuration space to coordinate frames that are rigidly\n//! attached to the kinematic structure.\nclass CUMO_EXPORT Kinematics {\n public:\n  //! Opaque handle to a frame\n  struct CUMO_EXPORT FrameHandle {\n    class Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  virtual ~Kinematics() = default;\n\n  //! Return the number of configuration space coordinates for the kinematic structure.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Return the name of a given configuration space coordinate of the kinematic structure.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `coord_index` < 0, *OR*\n  //!   2. `coord_index` >= `numCSpaceCoords()`.\n  [[nodiscard]] virtual std::string cSpaceCoordName(int coord_index) const = 0;\n\n  //! Lower and upper limits for a configuration space coordinate.\n  struct CUMO_EXPORT Limits {\n    double lower;  //!< lower limit\n    double upper;  //!< upper limit\n  };\n\n  //! Return the limits of a given configuration space coordinate of the kinematic structure.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `coord_index` < 0, *OR*\n  //!   2. `coord_index` >= `numCSpaceCoords()`.\n  [[nodiscard]] virtual Limits cSpaceCoordLimits(int coord_index) const = 0;\n\n  //! Determine whether a specified configuration space position is within limits for each\n  //! coordinate.\n  //!\n  //! If `log_warnings` is set to `true` and `cspace_position` is outside limits, a warning will be\n  //! logged indicating which coordinates are outside limits.\n  virtual bool withinCSpaceLimits(const Eigen::VectorXd &cspace_position,\n                                  bool log_warnings) const = 0;\n\n  //! Return the velocity limit of a given configuration space coordinate of the kinematic\n  //! structure.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `coord_index` < 0, *OR*\n  //!   2. `coord_index` >= `numCSpaceCoords()`.\n  [[nodiscard]] virtual double cSpaceCoordVelocityLimit(int coord_index) const = 0;\n\n  //! Return the acceleration limit of a given configuration space coordinate of the kinematic\n  //! structure.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `coord_index` < 0, *OR*\n  //!   2. `coord_index` >= `numCSpaceCoords()`.\n  [[nodiscard]] virtual double cSpaceCoordAccelerationLimit(int coord_index) const = 0;\n\n  //! Return the jerk limit of a given configuration space coordinate of the kinematic structure.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `coord_index` < 0, *OR*\n  //!   2. `coord_index` >= `numCSpaceCoords()`.\n  [[nodiscard]] virtual double cSpaceCoordJerkLimit(int coord_index) const = 0;\n\n  //! Return all the frame names in the kinematic structure.\n  [[nodiscard]] virtual const std::vector<std::string> &frameNames() const = 0;\n\n  //! Return the name of the given `frame`.\n  [[nodiscard]] virtual const std::string &frameName(const FrameHandle &frame) const = 0;\n\n  //! Return a handle representing the frame with the given `frame_name`.\n  //!\n  //! A fatal error will be logged if `frame_name` is not a valid frame name.\n  [[nodiscard]] virtual FrameHandle frame(const std::string &frame_name) const = 0;\n\n  //! Return a handle representing the base frame of the kinematic structure.\n  [[nodiscard]] virtual FrameHandle baseFrame() const = 0;\n\n  //! Return the pose of the given `frame` with respect to `reference_frame` at the given\n  //! `cspace_position`.\n  [[nodiscard]] virtual Pose3 pose(const Eigen::VectorXd &cspace_position,\n                                   const FrameHandle &frame,\n                                   const FrameHandle &reference_frame) const = 0;\n\n  //! Return the pose of the given `frame` with respect to the base frame (i.e., `baseFrame()`) at\n  //! the given `cspace_position`.\n  [[nodiscard]] virtual Pose3 pose(const Eigen::VectorXd &cspace_position,\n                                   const FrameHandle &frame) const = 0;\n\n  //! Return the position of the origin of the given `frame` with respect to `reference_frame` at\n  //! the given `cspace_position`.\n  [[nodiscard]] virtual Eigen::Vector3d position(const Eigen::VectorXd &cspace_position,\n                                                 const FrameHandle &frame,\n                                                 const FrameHandle &reference_frame) const = 0;\n\n  //! Return the position of the origin of the given `frame` with respect to the base frame\n  //! (i.e., `baseFrame()`) at the given `cspace_position`.\n  [[nodiscard]] virtual Eigen::Vector3d position(const Eigen::VectorXd &cspace_position,\n                                                 const FrameHandle &frame) const = 0;\n\n  //! Return the orientation of the given `frame` with respect to `reference_frame` at the given\n  //! `cspace_position`.\n  [[nodiscard]] virtual Rotation3 orientation(const Eigen::VectorXd &cspace_position,\n                                              const FrameHandle &frame,\n                                              const FrameHandle &reference_frame) const = 0;\n\n  //! Return the orientation of the given `frame` with respect to the base frame\n  //! (i.e., `baseFrame()`) at the given `cspace_position`.\n  [[nodiscard]] virtual Rotation3 orientation(const Eigen::VectorXd &cspace_position,\n                                              const FrameHandle &frame) const = 0;\n\n  //! Return the Jacobian of the origin of the given `frame` with respect to the base frame\n  //! (i.e., `baseFrame()`) at the given `cspace_position`.\n  //!\n  //! The returned Jacobian is a 6 x N matrix where N is the `numCSpaceCoords`. Column `i` of the\n  //! returned Jacobian represents the perturbation contribution to origin of `frame` from the\n  //! `i`th c-space element in the coordinates of the base frame. For each column, the first three\n  //! elements represent the translational portion and the last three elements represent the\n  //! rotational portion.\n  [[nodiscard]] virtual Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian(\n      const Eigen::VectorXd &cspace_position,\n      const FrameHandle &frame) const = 0;\n\n  //! Return the Jacobian of the position of the origin of the given `frame` with respect to the\n  //! base frame (i.e., `baseFrame()`) at the given `cspace_position`.\n  //!\n  //! The returned Jacobian is a 3 x N matrix where N is the `numCSpaceCoords`. Column `i` of the\n  //! returned Jacobian represents the perturbation contribution to position of the origin of\n  //! `frame` from the `i`th c-space element in the coordinates of the base frame.\n  [[nodiscard]] virtual Eigen::Matrix<double, 3, Eigen::Dynamic> positionJacobian(\n      const Eigen::VectorXd &cspace_position,\n      const FrameHandle &frame) const = 0;\n\n  //! Return the Jacobian of the orientation of the given `frame` with respect to the base frame\n  //! (i.e., `baseFrame()`) at the given `cspace_position`.\n  //!\n  //! The returned Jacobian is a 3 x N matrix where N is the `numCSpaceCoords`. Column `i` of the\n  //! returned Jacobian represents the perturbation contribution to orientation of `frame` from the\n  //! `i`th c-space element in the coordinates of the base frame.\n  [[nodiscard]] virtual Eigen::Matrix<double, 3, Eigen::Dynamic> orientationJacobian(\n      const Eigen::VectorXd &cspace_position,\n      const FrameHandle &frame) const = 0;\n\n  //! Return the fixed transform between `frame` and its parent frame.\n  //!\n  //! Internally, the `Kinematics` structure is represented as a directed rooted tree. Each frame\n  //! has a single parent frame (other than the root which by definition has no parent). The edge\n  //! from parent frame to child frame represents both a fixed transform (i.e., a rigid linkage) AND\n  //! a \"joint\" transform (revolute, prismatic, or fixed). The root frame, while not having a\n  //! parent, maintains a fixed transform that defines its pose relative to global coordinates.\n  //!\n  //! This function returns the fixed transform that precedes the \"joint\" transform. The returned\n  //! `Pose3` is expressed relative to the parent frame. For example, if the `wrist_frame` is a\n  //! child of the `elbow_frame`, the returned pose would be the pose of the wrist joint expressed\n  //! in the local coordinates of the `elbow_frame`.\n  //!\n  //! NOTE: In general, the returned pose will NOT be the pose of the child frame expressed in the\n  //! local coordinates of the parent frame, as the \"joint\" transform is NOT included in the\n  //! returned transform, and the \"joint\" transform, in general, will not be identity. In order to\n  //! return the full transform from parent frame to child frame for a given c-space configuration,\n  //! the `pose()` function should be used with the child frame input for `frame` and the parent\n  //! frame input for `reference_frame`.\n  [[nodiscard]] virtual Pose3 getPrecedingFixedTransform(const FrameHandle &frame) const = 0;\n\n  //! Set the fixed transform between `frame` and its parent frame to `transform`.\n  //!\n  //! Internally, the `Kinematics` structure is represented as a directed rooted tree. Each frame\n  //! has a single parent frame (other than the root which by definition has no parent). The edge\n  //! from parent frame to child frame represents both a fixed transform (i.e., a rigid linkage) AND\n  //! a \"joint\" transform (revolute, prismatic, or fixed). The root frame, while not having a\n  //! parent, maintains a fixed transform that defines its pose relative to global coordinates; it\n  //! is valid to use this function to set that tranform to pose the base of the robot relative to\n  //! the global frame.\n  //!\n  //! This function sets the fixed transform that precedes the \"joint\" transform. The input\n  //! `transform` is expressed relative to the parent frame. For example, if the `wrist_frame` is a\n  //! child of the `elbow_frame`, the input `transform` would define the pose of the wrist joint\n  //! expressed in the local coordinates of the `elbow_frame`.\n  virtual void setPrecedingFixedTransform(const FrameHandle &frame, const Pose3 &transform) = 0;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/linear_cspace_path.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n#include <vector>\n\n#include \"cumotion/cspace_path.h\"\n#include \"cumotion/cspace_path_spec.h\"\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Represent a path linearly interpolated through configuration space (i.e., c-space) waypoints.\n//!\n//! This path is parameterized by independent variable `s` and will be continuous but not\n//! (in general) smooth.\nclass CUMO_EXPORT LinearCSpacePath : public CSpacePath {\n public:\n  virtual ~LinearCSpacePath() = default;\n\n  //! Return the number of configuration space coordinates for the path.\n  [[nodiscard]] int numCSpaceCoords() const override = 0;\n\n  //! Return the domain for the path.\n  [[nodiscard]] Domain domain() const override = 0;\n\n  //! Evaluate the path at the given `s`.\n  //!\n  //! A fatal error is logged if `s` is outside of the path `domain()`.\n  [[nodiscard]] Eigen::VectorXd eval(double s) const override = 0;\n\n  //! Return the total distance accumulated along the path, where distance is computed using the\n  //! L2-norm.\n  //!\n  //! This length is not, in general, equal to the distance between the configurations at the lower\n  //! and upper bounds of the domain.\n  [[nodiscard]] double pathLength() const override = 0;\n\n  //! Return the minimum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These minimum position values are not, in general, synchronous. Instead, they represent\n  //!       the minimum position independently achieved by each coordinate.\n  [[nodiscard]] Eigen::VectorXd minPosition() const override = 0;\n\n  //! Return the maximum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These maximum position values are not, in general, synchronous. Instead, they represent\n  //!       the maximum position independently achieved by each coordinate.\n  [[nodiscard]] Eigen::VectorXd maxPosition() const override = 0;\n\n  //! Return all of the waypoints through which the path is linearly interpolated (including the\n  //! initial and final c-space configurations).\n  [[nodiscard]] virtual std::vector<Eigen::VectorXd> waypoints() const = 0;\n};\n\n//! Create a `LinearCSpacePath` from a c-space path specification.\nCUMO_EXPORT std::unique_ptr<LinearCSpacePath> CreateLinearCSpacePath(\n    const CSpacePathSpec &cspace_path_spec);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/motion_planner.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface to cuMotion's global motion planning implementation.\n\n#pragma once\n\n#include <filesystem>\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Configuration parameters for a `MotionPlanner`.\nclass CUMO_EXPORT MotionPlannerConfig {\n public:\n  virtual ~MotionPlannerConfig() = default;\n\n  //! Indicate lower and upper limits for a coordinate.\n  struct CUMO_EXPORT Limit {\n    double lower;\n    double upper;\n  };\n\n  //! Specify the value for a given parameter.\n  //!\n  //! The required `ParamValue` constructor for each param is detailed in the\n  //! documentation for `setParam()`.\n  struct CUMO_EXPORT ParamValue {\n    //! Create `ParamValue` from `int`.\n    ParamValue(int value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `double`.\n    ParamValue(double value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `Eigen::Vector3d`.\n    ParamValue(const Eigen::Vector3d &value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `std::vector<double>`.\n    ParamValue(const std::vector<double> &value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `bool`.\n    ParamValue(bool value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `const char*`.\n    ParamValue(const char *value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `std::string`.\n    ParamValue(const std::string &value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `std::vector<Limit>`.\n    ParamValue(const std::vector<Limit> &value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Set the value of the parameter.\n  //!\n  //! `setParam` returns `true` if the param has been successfully updated and `false` if an error\n  //! has occurred (e.g., invalid parameter).\n  //!\n  //! The following parameters can be set for `MotionPlanner`:\n  //!\n  //! `seed` [`int`]\n  //!   - Used to initialize random sampling.\n  //!   - `seed` must be positive.\n  //!   - Default: 1234\n  //!\n  //! `step_size` [`double`]\n  //!   - Step size for tree extension and (optionally) for discretization of `interpolated_path`\n  //!     in `Results`\n  //!   - It is assumed that a straight path connecting two valid c-space configurations with\n  //!     separation distance <= `step_size` is a valid edge, where separation distance is defined\n  //!     as the L2-norm of the difference between the two configurations.\n  //!   - `step_size` must be positive.\n  //!   - Default: 0.05\n  //!\n  //! `max_iterations` [`int`]\n  //!   - Maximum number of iterations of tree extensions that will be attempted.\n  //!   - If `max_iterations` is reached without finding a valid path, the `Results` will\n  //!     indicate `path_found` is `false` and `path` will be an empty vector.\n  //!   - `max_iterations` must be positive.\n  //!   - Default: 10,000\n  //!\n  //! `max_sampling` [`int`]\n  //!   - Maximum number of configurations used for sampling in the environment setup.\n  //!   - `max_sampling` must be positive.\n  //!   - Default: 10'000\n  //!\n  //! `distance_metric_weights` [`std::vector<double>`]\n  //!   - When selecting a node for tree extension, the closest node is defined using a weighted,\n  //!     squared L2-norm:\n  //!       distance = (q0 - q1)^T * W * (q0 - q1)\n  //!       where q0 and q1 represent two configurations and W is a diagonal matrix formed from\n  //!       `distance_metric_weights`.\n  //!   - The length of the `distance_metric_weights` must be equal to the number of c-space\n  //!     coordinates for the robot and each weight must be positive.\n  //!   - NOTE: In general, it is recommended to use a vector of ones for `distance_metric_weights`\n  //!           (i.e., unweighted squared L2-norm). Doing so enables significant performance\n  //!           optimization for nearest neighbor searches during geometric planning. Non-unity\n  //!           weights should only be considered if the benefits of the weighted distance metric\n  //!           outweigh the cost of more expensive nearest neighbor searches.\n  //!   - Default: Vector of ones with length set by the number of c-space coordinates in\n  //!     `robot_description`.\n  //!\n  //! `tool_frame_name` [`std::string`]\n  //!   - Indicate the name (from URDF) of the frame to be used for task space planning.\n  //!   - With current implementation, setting a `tool_frame_name` that is not found in the\n  //!     kinematics will throw an exception rather than failing gracefully.\n  //!\n  //! `task_space_limits` [`std::vector<Limit>`]\n  //!   - Task space limits define a bounding box used for sampling task space when planning\n  //!     a path to a task space target.\n  //!   - The specified `task_space_limits` vector should be length 3, corresponding to the xyz\n  //!     dimensions of the bounding box.\n  //!   - Each upper limit must be >= the corresponding lower limit.\n  //!   - Default: Lower limits default to -1 and upper limits to 1.\n  //!\n  //! `enable_self_collision_checking` [`bool`]\n  //!  - Set to `true` to enable self-collision checking during motion planning.\n  //!  - When enabled, configurations that result in self-collision (collision between robot\n  //!    self-collision spheres) will be considered invalid during path planning.\n  //!  - Default: `true`\n  //!\n  //! `enable_cuda_tree` [`bool`]\n  //!   - Set to `true` to enable use of CUDA for storing explored configurations and performing\n  //!     nearest neighbor look-up, or `false` to disable usage of CUDA.\n  //!   - When `enable_cuda_tree` is set to `true`, CUDA will only be used when\n  //!     `distance_metric_weights` is not equal to `Eigen::VectorXd::Ones(cspace_dimension)`.\n  //!   - If set to `true` from the `false` state, default values for `cuda_tree_params` will be\n  //!     used.\n  //!   - If set to `false` from the `true` state, current values for `cuda_tree_params` will be\n  //!     discarded (i.e., if returned to `true`, default rather than previous values will be used).\n  //!   - NOTE: this parameter does NOT need to be set in the YAML configuration file. The presence\n  //!     of a `cuda_tree_params` with corresponding parameters in the configuration file indicates\n  //!     that `enable_cuda_tree` should be set to `true`.\n  //!   - Default: `true`\n  //!\n  //! `cuda_tree_params/max_num_nodes` [`int`]\n  //!   - DEPRECATED\n  //!   - The number of configurations that can be stored in CUDA-accelerated trees is no longer\n  //!     bound by a preset value. Growth is now limited by `max_iterations`, rather than the\n  //!     coupling of `max_iterations` and `cuda_tree_params/max_num_nodes`.\n  //!   - A deprecation warning is logged when `cuda_tree_params/max_num_nodes` is set via\n  //!     `setParam()` or when present in a configuration file. It is recommended to *not*\n  //!     provide a value for this parameter, as it will be ignored.\n  //!\n  //! `cuda_tree_params/max_buffer_size` [`int`]\n  //!   - Maximum number of valid configurations that are buffered on CPU prior to transferring to\n  //!     GPU.\n  //!   - A default value of 30 is recommended for most use-cases.\n  //!   - `cuda_tree_params/max_buffer_size` must be positive.\n  //!   - Default: 30\n  //!\n  //! `cuda_tree_params/num_nodes_cpu_gpu_crossover` [`int`]\n  //!   - Number of valid explored configurations added to tree prior to copying all configurations\n  //!     to GPU and using GPU for nearest neighbor lookup.\n  //!   - A default value of 3000 is recommended for most use-cases.\n  //!   - `cuda_tree_params/num_nodes_cpu_gpu_crossover` must be positive.\n  //!   - Default: 3000\n  //!\n  //!  NOTE: For all of the `cuda_tree_params` listed above, the best values for optimal performance\n  //!        will depend on the number of c-space coordinates, the system hardware (e.g., CPU, GPU,\n  //!        and memory configuration), and software versions (e.g., CUDA, NVIDIA GPU driver, and\n  //!        cuMotion versions). The provided default recommendations are chosen to be appropriate\n  //!        for most expected use-cases.\n  //!\n  //! `cspace_planning_params/exploration_fraction` [`double`]\n  //!   - The c-space planner uses RRT-Connect to try to find a path to a c-space target.\n  //!   - RRT-Connect attempts to iteratively extend two trees (one from the initial configuration\n  //!     and one from the target configuration) until the two trees can be connected. The\n  //!     configuration to which a tree is extended can be either a random sample\n  //!     (i.e., exploration) or a node on the tree to which connection is desired\n  //!     (i.e., exploitation). The `exploration_fraction` controls the fraction of steps that are\n  //!     exploration steps. It is generally recommended to set `exploration_fraction` in range\n  //!     [0.5, 1), where 1 corresponds to a single initial exploitation step followed by only\n  //!     exploration steps. Values of between [0, 0.5) correspond to more exploitation than\n  //!     exploration and are not recommended. If a value outside range [0, 1] is provided, a\n  //!     warning is logged and the value is clamped to range [0, 1].\n  //!   - A default value of 0.5 is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Default: 0.5\n  //!\n  //! `task_space_planning_params/translation_target_zone_tolerance` [`double`]\n  //!   - A configuration has reached the task space translation target when task space position has\n  //!     an L2 Norm within `translation_target_zone_tolerance` of the target.\n  //!   - It is assumed that a valid configuration within the target tolerance can be moved directly\n  //!     to the target configuration using an inverse kinematics solver and linearly stepping\n  //!     towards the solution.\n  //!   - In general, it is recommended that the size of the translation target zone be on the same\n  //!     order of magnitude as the translational distance in task-space corresponding to moving the\n  //!     robot in configuration space by one step with an L2 norm of `step_size`.\n  //!   - Must be > 0.\n  //!   - Default: 0.05\n  //!\n  //! `task_space_planning_params/orientation_target_zone_tolerance` [`double`]\n  //!   - A configuration has reached the task space pose target when task space orientation is\n  //!     within `orientation_target_zone_tolerance` radians and an L2 norm translation\n  //!     within `translation_target_zone_tolerance` of the target.\n  //!   - It is assumed that a valid configuration within the target tolerances can be moved\n  //!     directly to the target configuration using an inverse kinematics solver and linearly\n  //!     stepping towards the solution.\n  //!   - In general, it is recommended that the size of the orientation target zone be on the same\n  //!     order of magnitude as the rotational distance in task-space corresponding to moving the\n  //!     robot in configuration space by one step with an L2 norm of `step_size`.\n  //!   - Must be > 0.\n  //!   - Default: 0.09\n  //!\n  //! `task_space_planning_params/translation_target_final_tolerance` [`double`]\n  //!   - Once a path is found that terminates within `translation_target_zone_tolerance`, an IK\n  //!     solver is used to find a configuration space solution corresponding to the task space\n  //!     target. This solver terminates when the L2-norm of the corresponding task space position\n  //!     is within `translation_target_final_tolerance` of the target.\n  //!   - Note: This solver assumes that if a c-space configuration within\n  //!     `translation_target_zone_tolerance` is found then this c-space configuration can be\n  //!     moved linearly in c-space to the IK solution. If this assumption is NOT met, the returned\n  //!     path will not reach the task space target within the `translation_target_final_tolerance`\n  //!     and an error is logged.\n  //!   - The recommended default value is 1e-4, but in general this value should be set to a\n  //!     positive value that is considered \"good enough\" precision for the specific system.\n  //!   - Default: 1e-4\n  //!\n  //! `task_space_planning_params/orientation_target_final_tolerance` [`double`]\n  //!   - For pose targets, once a path is found that terminates within\n  //!     `orientation_target_zone_tolerance` and `translation_target_zone_tolerance` of the target,\n  //!     an IK solver is used to find a configuration space solution corresponding to the task\n  //!     space target. This solver terminates when the L2-norm of the corresponding task space\n  //!     position is within `orientation_target_final_tolerance` and\n  //!     `translation_target_final_tolerance` of the target.\n  //!   - Note: This solver assumes that if a c-space configuration within the target zone\n  //!     tolerances is found then this c-space configuration can be moved linearly in c-space to\n  //!     the IK solution. If this assumption is NOT met, the returned path will not reach the task\n  //!     space target within the final target tolerances and an error is logged.\n  //!   - The recommended default value is 1e-4, but in general this value should be set to a\n  //!     positive value that is considered \"good enough\" precision for the specific system.\n  //!   - Default: 0.005\n  //!\n  //! `task_space_planning_params/translation_gradient_weight` [`double`]\n  //!   - For pose targets, computed translation and orientation gradients are linearly weighted by\n  //!     `translation_gradient_weight` and `orientation_gradient_weight` to compute a combined\n  //!     gradient step when using the Jacobian Transpose method to guide tree expansion\n  //!     towards a task space target.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Must be > 0.\n  //!   - Default: 1.0\n  //!\n  //! `task_space_planning_params/orientation_gradient_weight` [`double`]\n  //!   - For pose targets, computed translation and orientation gradients are linearly weighted by\n  //!     `translation_gradient_weight` and `orientation_gradient_weight` to compute a combined\n  //!     gradient step when using the Jacobian Transpose method to guide tree expansion\n  //!     towards a task space target.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Must be > 0.\n  //!   - Default: 0.125\n  //!\n  //! `task_space_planning_params/nn_translation_distance_weight` [`double`]\n  //!   - For pose targets, nearest neighbor distances are computed by linearly weighting\n  //!     translation and orientation distance by `nn_translation_distance_weight` and\n  //!     `nn_orientation_distance_weight`.\n  //!   - Nearest neighbor search is used to select the node from which the tree of valid\n  //!     configurations will be expanded.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Must be > 0.\n  //!   - Default: 1.0\n  //!\n  //! `task_space_planning_params/nn_orientation_distance_weight` [`double`]\n  //!   - For pose targets, nearest neighbor distances are computed by linearly weighting\n  //!     translation and orientation distance by `nn_translation_distance_weight` and\n  //!     `nn_orientation_distance_weight`.\n  //!   - Nearest neighbor search is used to select the node from which the tree of valid\n  //!     configurations will be expanded.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Must be > 0.\n  //!   - Default: 0.125\n  //!\n  //! `task_space_planning_params/task_space_exploitation_fraction` [`double`]\n  //!   - Fraction of iterations for which tree is extended towards target position in task space.\n  //!   - Must be in range [0, 1]. Additionally, the sum of `task_space_exploitation_fraction` and\n  //!     `task_space_exploration_fraction` must be <= 1.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Default: 0.4\n  //!\n  //! `task_space_planning_params/task_space_exploration_fraction` [`double`]\n  //!   - Fraction of iterations for which tree is extended towards random position in task space.\n  //!   - Must be in range [0, 1]. Additionally, the sum of `task_space_exploitation_fraction` and\n  //!     `task_space_exploration_fraction` must be <= 1.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Default: 0.1\n  //!\n  //!  NOTE: The remaining fraction beyond `task_space_exploitation_fraction` and\n  //!        `task_space_exploration_fraction` is a `cspace_exploration_fraction` that is\n  //!        implicitly defined as:\n  //!          1 - (`task_space_exploitation_fraction` + `task_space_exploration_fraction`\n  //!        In general, easier path searches will take less time with higher exploitation fraction\n  //!        while more difficult searches will waste time if the exploitation fraction is too high\n  //!        and benefit from greater combined exploration fraction.\n  //!\n  //! `task_space_planning_params/max_extension_substeps_away_from_target` [`int`]\n  //!  - Maximum number of Jacobian transpose gradient descent substeps that may be taken\n  //!    while the end effector is away from the task-space target.\n  //!  - The threshold for nearness is determined by the\n  //!    `extension_substep_target_region_scale_factor` parameter.\n  //!  - The default value is recommended as a starting value for initial testing with a given\n  //!    system.\n  //!  - Must be >= 0.\n  //!  - Default: 6\n  //!\n  //! `task_space_planning_params/max_extension_substeps_near_target` [`int`]\n  //!  - Maximum number of Jacobian transpose gradient descent substeps that may be taken\n  //!    while the end effector is near the task-space target.\n  //!  - The threshold for nearness is determined by the\n  //!    `extension_substep_target_region_scale_factor` parameter.\n  //!  - The default value is recommended as a starting value for initial testing with a given\n  //!    system.\n  //!  - Must be >= 0.\n  //!  - Default: 50\n  //!\n  //! `task_space_planning_params/extension_substep_target_region_scale_factor` : [`double`]\n  //!  - A scale factor used to determine whether the end effector is close enough to the target\n  //!    to change the amount of gradient descent substeps allowed when adding a node in RRT.\n  //!  - The `max_extension_substeps_near_target` parameter is used when the distance\n  //!    (i.e., L2 norm) between the end effector and target position is less than\n  //!    `extension_substep_target_region_scale_factor` * `x_zone_target_tolerance`.\n  //!  - Must be greater than or equal to 1.0; a value of 1.0 effectively disables the\n  //!   `max_extension_substeps_near_target` parameter.\n  //!  - The default value is recommended as a starting value for initial testing with a given\n  //!    system.\n  //!  - Default: 2.0\n  //!\n  //! `task_space_planning_params/unexploited_nodes_culling_scalar` [`double`]\n  //!  - Scalar controlling culling of unexploited nodes during task-space planning.\n  //!  - Must be >= 0.0.\n  //!  - Default: 1.0\n  //!\n  //! `task_space_planning_params/gradient_substep_size` [`double`]\n  //!  - Size of each gradient-descent substep when following the Jacobian Transpose direction.\n  //!  - Must be > 0.0.\n  //!  - Default: 0.025\n  virtual bool setParam(const std::string &param_name, ParamValue value) = 0;\n};\n\n//! Load a set of `MotionPlanner` configuration parameters from file.\n//!\n//! These parameters are combined with `robot_description`, `tool_frame_name`, and `world_view` to\n//! create a configuration for motion planning.\n//!\n//! A fatal error will be logged if `motion_planner_config_file` is missing any required parameters.\n//! If any provided parameter values are invalid:\n//! - When recoverable, they will be clipped to their expected value range with a warning on calling\n//!   `CreateMotionPlanner()`.\n//! - When not recoverable, a fatal error will be logged.\n//!\n//! A configuration will *NOT* be created if:\n//!   1. `robot_description` is invalid, *OR*\n//!   2. `tool_frame_name` is not a valid frame in `robot_description`.\n//! In the case of failure, a non-fatal error will be logged and a `nullptr` will be returned.\nCUMO_EXPORT std::unique_ptr<MotionPlannerConfig> CreateMotionPlannerConfigFromFile(\n    const std::filesystem::path &motion_planner_config_file,\n    const RobotDescription &robot_description,\n    const std::string &tool_frame_name,\n    const WorldViewHandle &world_view);\n\n//! Use default parameters to create a configuration for motion planning.\n//!\n//! See `MotionPlannerConfig::setParam()` for the default parameter values.\n//!\n//! A configuration will *NOT* be created if:\n//!   1. `robot_description` is invalid, *OR*\n//!   2. `tool_frame_name` is not a valid frame in `robot_description`.\n//!\n//! In the case of failure, a non-fatal error will be logged and a `nullptr` will be returned.\nCUMO_EXPORT std::unique_ptr<MotionPlannerConfig> CreateDefaultMotionPlannerConfig(\n    const RobotDescription &robot_description,\n    const std::string &tool_frame_name,\n    const WorldViewHandle &world_view);\n\n//! Interface class for planning collision-free paths for robotic manipulators.\n//! Supports both configuration space (i.e., joint position) targets and task space (e.g., end\n//! effector position) targets.\nclass CUMO_EXPORT MotionPlanner {\n public:\n  virtual ~MotionPlanner() = default;\n\n  //! Results from a path search.\n  struct CUMO_EXPORT Results {\n    //! Indicate whether a collision-free path was found.\n    //! If `false`, `path` and `interpolated_path` will be empty.\n    bool path_found;\n\n    //! Minimal representation of collision-free path.\n    //! Each vector represents a knot in configuration space, where successive knots can be linearly\n    //! interpolated in configuration space to generate a collision-free path.\n    std::vector<Eigen::VectorXd> path;\n\n    //! Interpolation of `path` such that successive knots are (in general) one `step_size` apart\n    //! from each other (where distance is defined as L2-norm in c-space).\n    //! NOTE: Each straight segment from `path` above are interpolated individually, where the\n    //!       distance between the last two returned knots for each segment will be less than one\n    //!       `step_size` from each other.\n    //! The `interpolated_path` will only be populated if `generate_interpolated_path` is set to\n    //! `true` when generating a path plan.\n    std::vector<Eigen::VectorXd> interpolated_path;\n  };\n\n  //! Attempt to find a path from initial configuration space position (`q_initial`) to a\n  //! configuration space target (`q_target`).\n  //! `generate_interpolated_path` determines whether `interpolated_path` will be populated in\n  //! `Results`.\n  virtual Results planToCSpaceTarget(const Eigen::VectorXd &q_initial,\n                                     const Eigen::VectorXd &q_target,\n                                     bool generate_interpolated_path = false) = 0;\n\n  //! Attempt to find a path from initial configuration space position (`q_initial`) to a task\n  //! space translation target (`translation_target`).\n  //! `generate_interpolated_path` determines whether `interpolated_path` will be populated in\n  //! `Results`.\n  virtual Results planToTranslationTarget(const Eigen::VectorXd &q_initial,\n                                          const Eigen::Vector3d &translation_target,\n                                          bool generate_interpolated_path = false) = 0;\n\n  //! Attempt to find a path from initial configuration space position (`q_initial`) to a task\n  //! space pose target (`pose_target`).\n  //! `generate_interpolated_path` determines whether `interpolated_path` will be populated in\n  //! `Results`.\n  virtual Results planToPoseTarget(const Eigen::VectorXd &q_initial,\n                                   const Pose3 &pose_target,\n                                   bool generate_interpolated_path = false) = 0;\n\n  //! Reset all internal state so that subsequent planning calls produce the same results as they\n  //! did immediately after construction, given the same inputs.\n  virtual void reset() = 0;\n};\n\n//! Create a `MotionPlanner` with the given `config`.\nCUMO_EXPORT std::unique_ptr<MotionPlanner> CreateMotionPlanner(const MotionPlannerConfig &config);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/obstacle.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Obstacles represent 3d geometries that can be added to `cumotion::World`.\n//!\n//! See `cumotion/world.h` for usage.\nclass CUMO_EXPORT Obstacle {\n public:\n  //! Indicates what geometric primitive the obstacle represents.\n  //!\n  //! Each `Obstacle::Type` has one or more attributes that can be set via `setAttribute()`,\n  //! detailed in the documentation for the corresponding enum value.\n  enum class Type {\n    //! A cylinder with rounded ends.\n    //!\n    //! The capsule is oriented along the z-axis.\n    //!\n    //! Origin: Geometric center, with the radius in the x-y plane, and spheres placed equidistant\n    //!         along the z-axis.\n    //!\n    //! Attributes:\n    //! - RADIUS: Radius of the capsule.\n    //!           Default: 0.5\n    //! - HEIGHT: Height of the capsule, defined to be the distance between the centers of the two\n    //!           spheres defining the capsule.\n    //!           Default: 1.0\n    CAPSULE,\n\n    //! An axis-aligned cuboid (i.e., rectangular prism).\n    //!\n    //! The cuboid is constructed to be aligned with the axes of its local coordinate frame.\n    //!\n    //! Origin: Geometric center.\n    //!\n    //! Attributes:\n    //! - SIDE_LENGTHS: Dimensions along each axis (in the local obstacle frame). Each value\n    //!                 represents the full extent (i.e., \"length\") of the cuboid, not the\n    //!                 half-length.\n    //!                 Default: {1.0, 1.0, 1.0}\n    CUBOID,\n\n    //! A sphere.\n    //!\n    //! Origin: The center of the sphere.\n    //!\n    //! Attributes:\n    //! - RADIUS: The radius of the sphere.\n    //!           Default: 0.5\n    SPHERE,\n\n    //! An axis-aligned uniform grid of signed distances.\n    //!\n    //! Origin: Center of the grid.\n    //!\n    //! Attributes:\n    //! - GRID: Defines the dimensions and resolution of the SDF.\n    //!         A grid precision of `Grid::Precision::DOUBLE` is unsupported and will result in a\n    //!         fatal error if specified.\n    SDF\n  };\n\n  //! `Attribute`s are used to modify obstacles from their default geometry.\n  //!\n  //! Each `Attribute` may be applicable to one or more `Obstacle::Type`s. Details about which\n  //! `Attribute` can be used with which `Obstacle::Type` are included in documentation for\n  //! `Obstacle::Type`.\n  //!\n  //! Each `Attribute` has a required type for the `AttributeValue` used in conjunction with\n  //! `setAttribute()`, detailed in the documentation for each enum value below.\n  enum class Attribute {\n    //! The height of an obstacle (typically aligned with the z-axis).\n    //!\n    //! Data Type: `double`\n    HEIGHT,\n\n    //! The radius of an obstacle.\n    //!\n    //! Data Type: `double`\n    RADIUS,\n\n    //! The dimensions of the obstacle along the cardinal axes.\n    //!\n    //! Data Type: `Eigen::Vector3d`\n    SIDE_LENGTHS,\n\n    //! An axis-aligned regular grid of points.\n    //!\n    //! Data Type: `Obstacle::Grid`\n    GRID\n  };\n\n  //! Used to specify the defining attributes for a grid of voxels that covers an axis-aligned\n  //! rectangular region of the workspace.\n  //!\n  //! `Grid` fully describes how the grid values passed to `World::setGridValuesFromHost()` and\n  //! `World::setGridValuesFromDevice()` will be interpreted.  An `Attribute::GRID` specifies a grid\n  //! of voxels that each contain a scalar value.  Each voxel has a fixed workspace position\n  //! associated with its value that is implicitly defined by the parameters in `Grid`.\n  struct CUMO_EXPORT Grid {\n    //! Floating-point precision of grid data.\n    //!\n    //! Underlying integer values correspond to the size of each floating-point type in bytes.\n    enum class Precision {\n      HALF = 2,\n      FLOAT = 4,\n      DOUBLE = 8\n    };\n\n    //! A `Grid` covers a rectangular region in the workspace whose minimal coordinate is\n    //! the origin and whose voxel positions are determined by `num_voxels_x`, `num_voxels_y`,\n    //! `num_voxels_z`, and `voxel_size`.\n    //!\n    //! The values associated with a `Grid` correspond to the center of each voxel.  The minimal\n    //! corner of the minimal voxel in a `Grid` rests at the origin.  I.e., the origin is not at\n    //! a voxel center; it is at a voxel corner.\n    //!\n    //! Voxels in a `Grid` have length `voxel_size` along each axis.\n    //!\n    //! Grid contains `num_voxels_x` voxels along the X dimension, `num_voxels_y` voxels along the Y\n    //! dimension, and `num_voxels_z` voxels along the Z dimension.  This implies that the maximal\n    //! position in the region of the workspace that contains a `Grid` is\n    //! `voxel_size * [num_voxels_x, num_voxels_y, num_voxels_z]`.\n    //!\n    //! When grid values are set for an obstacle, up to two copies of the data will be maintained\n    //! at separately specified levels of precision.  For example, it is possible to pass a set of\n    //! grid values of type `double` and to specify that a set of grid values of type `double`\n    //! should be kept in host (CPU) memory for use in distance queries made on the host, along with\n    //! a set of grid values of type `float` kept resident in device (GPU) memory for use in\n    //! distance queries made on the device.\n    Grid(int num_voxels_x,\n         int num_voxels_y,\n         int num_voxels_z,\n         double voxel_size,\n         Precision host_precision,\n         Precision device_precision)\n        : num_voxels_x(num_voxels_x),\n          num_voxels_y(num_voxels_y),\n          num_voxels_z(num_voxels_z),\n          voxel_size(voxel_size),\n          host_precision(host_precision),\n          device_precision(device_precision) {}\n\n    //! The number of voxels along the X dimension of `Grid`.\n    int num_voxels_x;\n\n    //! The number of voxels along the Y dimension of `Grid`.\n    int num_voxels_y;\n\n    //! The number of voxels along the Z dimension of `Grid`.\n    int num_voxels_z;\n\n    //! The size of the voxels along each dimension.\n    double voxel_size;\n\n    //! The type of data for storing a set of grid values on CPU.\n    Precision host_precision;\n\n    //! The type of data for storing a set of grid values on GPU.\n    Precision device_precision;\n  };\n\n  //! Used to specify the value for a given `Attribute`.\n  //!\n  //! The required `AttributeValue` constructor for each `Attribute` is detailed in the\n  //! documentation for `Attribute`.\n  struct CUMO_EXPORT AttributeValue {\n    //! Create `AttributeValue` from `double`.\n    AttributeValue(double value);  // NOLINT Allow implicit conversion\n\n    //! Create `AttributeValue` from `Eigen::Vector3d`.\n    AttributeValue(const Eigen::Vector3d &value);  // NOLINT Allow implicit conversion\n\n    //! Create `AttributeValue` from `Grid`.\n    AttributeValue(const Grid &value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  virtual ~Obstacle() = default;\n\n  //! Set attribute for obstacle.\n  //!\n  //! Available attributes are based on the `Obstacle::Type` and detailed in documentation for\n  //! `Obstacle::Type`.\n  //!\n  //! Example usage:\n  //!     my_sphere.setAttribute(Obstacle::Attribute::RADIUS, 3.0);\n  //!     my_cuboid.setAttribute(Obstacle::Attribute::SIDE_LENGTHS, Eigen::Vector3d(2.0, 3.0, 1.0));\n  virtual void setAttribute(Attribute attribute, const AttributeValue &value) = 0;\n\n  //! Return the `Obstacle::Type` of this obstacle.\n  [[nodiscard]] virtual Type type() const = 0;\n};\n\n//! Create an obstacle with the given `type`.\n//!\n//! Available attributes and default attribute values for the given `type` are included in the\n//! documentation for `Obstacle::Type`.\nCUMO_EXPORT std::unique_ptr<Obstacle> CreateObstacle(Obstacle::Type type);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/path_conversion.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n\n#include \"cumotion/composite_path_spec.h\"\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/ik_solver.h\"\n#include \"cumotion/kinematics.h\"\n#include \"cumotion/linear_cspace_path.h\"\n\nnamespace cumotion {\n\n//! Configuration parameters for converting a `TaskSpacePathSpec` into a series of c-space\n//! configurations.\nstruct CUMO_EXPORT TaskSpacePathConversionConfig {\n  //! Create default set of configuration parameters for converting a `TaskSpacePathSpec` into a\n  //! series of c-space configurations.\n  //!\n  //! Default parameters are expected to work well for converting most task space paths to c-space\n  //! paths.\n  //!\n  //! If a tighter tolerance for adherence to the task space path is desired, then the\n  //! `min_position_deviation` and/or `max_position_deviation` can be decreased (at the expense of\n  //! more computational cost and more c-space waypoints which will likely lead to trajectories with\n  //! longer time-spans). Conversely, tolerances can be loosened for faster path conversion\n  //! and/or (likely) faster trajectories.\n  //!\n  //! Beyond (optionally) adjusting position deviations, it is unlikely that other parameters will\n  //! need to be modified from the provided default values.\n  TaskSpacePathConversionConfig() = default;\n\n  //! For each c-space waypoint that is generated, the position deviation between the desired task\n  //! space path and a task space mapping of a straight-line interpolation in c-space is\n  //! approximated. The minimum position deviation places a lower bound of deviation required to add\n  //! a c-space waypoint.\n  //!\n  //! While it is somewhat unintuitive that the deviation could be *too* small, this\n  //! minimum deviation is used to control the minimum spacing between c-space configurations along\n  //! the task space path domain. A (relatively) sparse series of c-space waypoints is desirable for\n  //! trajectory generation; if the minimum deviation is arbitrarily small then the c-space points\n  //! will be (in general) too close together to generate a time-optimal trajectory. Generation of\n  //! excessive c-space waypoints will also be computationally expensive and is, in general, best\n  //! avoided.\n  //!\n  //! `min_position_deviation` must be positive and less than `max_position_deviation`.\n  //!\n  //! Default value is 0.001.\n  double min_position_deviation = 0.001;\n\n  //! For each c-space waypoint that is generated, the position deviation between the desired task\n  //! space path and a task space mapping of a straight-line interpolation in c-space is\n  //! approximated. The maximum position deviation places an upper bound of deviation allowed to add\n  //! a c-space waypoint.\n  //!\n  //! `max_position_deviation` must be positive and greater than `min_position_deviation`.\n  //!\n  //! Default value is 0.003.\n  double max_position_deviation = 0.003;\n\n  //! Initial step size in the domain value 's' used to sample poses from the task space path to be\n  //! converted to c-space waypoints.\n  //!\n  //! The 's' step size will be adaptively updated throughout the path conversion and the default\n  //! value for initialization is generally recommended.\n  //!\n  //! `initial_s_step_size` must be positive.\n  //!\n  //! Default value is 0.05.\n  double initial_s_step_size = 0.05;\n\n  //! Initial step size \"delta\" that is used to adaptively adjust the 's' step size; 's' is the\n  //! domain value 's' used to sample poses from the task space path to be converted to c-space\n  //! waypoints.\n  //!\n  //! The 's' step size \"delta\" will be adaptively updated throughout the path conversion and the\n  //! default value for initialization is generally recommended.\n  //!\n  //! `initial_s_step_size_delta` must be positive.\n  //!\n  //! Default value is 0.005.\n  double initial_s_step_size_delta = 0.005;\n\n  //! Minimum allowable interval in domain value 's' that can separate poses from the task space\n  //! path to be converted to c-space waypoints.\n  //!\n  //! The minimum 's' step size serves to limit the number of c-space configurations that can be\n  //! returned in the converted path. Specifically, the upper bound for the number of returned\n  //! c-space configurations is (\"span of the task space path domain\" / min_s_step_size) + 1.\n  //!\n  //! `min_s_step_size` must be positive.\n  //!\n  //! Default value is 1e-5.\n  double min_s_step_size = 1e-5;\n\n  //! Minimum allowable 's' step size \"delta\" used to adaptively update the 's' step size.\n  //!\n  //! The `min_s_step_size_delta` serves to limit wasted iterations when (minimal) progress is being\n  //! made towards path conversion. If `min_s_step_size_delta` is reached during the search for any\n  //! c-space waypoint, then path conversion will fail.\n  //!\n  //! The default value is generally recommended.\n  //!\n  //! `min_s_step_size_delta` must be positive.\n  //!\n  //! Default value is 1e-5.\n  double min_s_step_size_delta = 1e-5;\n\n  //! Maximum number of iterations to search for each c-space waypoint.\n  //!\n  //! If `max_iterations` is reached for any c-space waypoint, then path conversion will fail.\n  //!\n  //! `max_iterations` must be positive.\n  //!\n  //! Default value is 50.\n  int max_iterations = 50;\n\n  //! \"alpha\" is used to exponentially scale the 's' step size \"delta\" to speed convergence when the\n  //! 's' step size is being successively increased or successively decreased. When an increase is\n  //! followed by a decrease, or vice versa, \"alpha\" is used to decrease the 's' step size \"delta\"\n  //! to reduce overshoot.\n  //!\n  //! The default value is generally recommended.\n  //!\n  //! `alpha` must be greater than 1.\n  //!\n  //! Default value is 1.4.\n  double alpha = 1.4;\n};\n\n//! Convert a `composite_path_spec` into a linear c-space path.\n//!\n//! The mapping from c-space to task space is defined by `kinematics` for the given `control_frame`.\n//!\n//! If non-default configuration parameters for the path conversion process are desired, then\n//! `task_space_path_conversion_config` can (optionally) be specified.\n//!\n//! If non-default configuration parameters for the inverse kinematics (IK) solver are desired,\n//! then `ik_config` can optionally be specified.\nCUMO_EXPORT std::unique_ptr<LinearCSpacePath> ConvertCompositePathSpecToCSpace(\n    const CompositePathSpec &composite_path_spec,\n    const Kinematics &kinematics,\n    const Kinematics::FrameHandle &control_frame,\n    const TaskSpacePathConversionConfig &task_space_path_conversion_config =\n        TaskSpacePathConversionConfig(),\n    const IkConfig &ik_config = IkConfig());\n\n//! Convert a `task_space_path_spec` into a linear c-space path.\n//!\n//! Inverse kinematics will be used to convert the initial task space pose of `task_space_path_spec`\n//! to a c-space position. If a particular c-space solution is desired, this can be set in\n//! `ik_config.cspace_seeds`. If the specified c-space does *not* correspond to the initial task\n//! space pose, it will simply be used as a warm start and the IK solver will proceed to search for\n//! an appropriate c-space position.\n//!\n//! The mapping from c-space to task space is defined by `kinematics` for the given `control_frame`.\n//!\n//! If non-default configuration parameters for the path conversion process are desired, then\n//! `task_space_path_conversion_config` can (optionally) be specified.\n//!\n//! If non-default configuration parameters for the inverse kinematics (IK) solver are desired,\n//! then `ik_config` can optionally be specified.\nCUMO_EXPORT std::unique_ptr<LinearCSpacePath> ConvertTaskSpacePathSpecToCSpace(\n    const TaskSpacePathSpec &task_space_path_spec,\n    const Kinematics &kinematics,\n    const Kinematics::FrameHandle &control_frame,\n    const TaskSpacePathConversionConfig &task_space_path_conversion_config =\n        TaskSpacePathConversionConfig(),\n    const IkConfig &ik_config = IkConfig());\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/path_spec_yaml.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <filesystem>\n#include <memory>\n#include <string>\n\n#include \"cumotion/composite_path_spec.h\"\n#include \"cumotion/cspace_path_spec.h\"\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/task_space_path_spec.h\"\n\nnamespace cumotion {\n\n//! Load a `TaskSpacePathSpec` from file with absolute path `task_space_path_spec_file`.\n//!\n//! The `task_space_spec_file` is expected to correspond to a YAML file specifying a task space\n//! path. This path specification must include:\n//!   [1] an initial task space pose, and\n//!   [2] a series of path segments emanating from this initial pose.\n//!\n//! The initial pose uses the key \"initial pose\" and is expected to have the following format:\n//!\n//!   initial pose:\n//!     position: [#, #, #]\n//!     orientation: {w: #, xyz: [#, #, #]}\n//!\n//! where each \"#\" represents a floating point value. Position must be a 3d-vector and orientation\n//! is represented by a quaternion.\n//!\n//! NOTE: Standard YAML formatting is allowed, wherein:\n//!   * mapped keys can be included in any order (e.g., \"position\" and \"orientation\" can be\n//!     included in either order),\n//!   * mapped keys can be included on a single comma-separated line with curly braces, or\n//!     expanded to multiple lines. E.g., \"orientation\" can be formatted as:\n//!        orientation: {w: #, xyz: [#, #, #]}\n//!     or equivalently as:\n//!        orientation:\n//!          w: #\n//!          xyz: [#, #, #]\n//!   * sequences can be included in a single comma-separated line with square brackets, or\n//!     expanded into multiple lines with preceding dashes. E.g., \"position\" can be formatted as:\n//!        position: [#, #, #]\n//!     or equivalently as:\n//!        position:\n//!          - #\n//!          - #\n//!          - #\n//!\n//! The path segments are specified as a sequence under the key \"path specs\". Each path\n//! specification must specify both a \"position mode\" and a \"orientation mode\".\n//!\n//! The \"position mode\" must be:\n//!   \"linear\",\n//!   \"constant\",\n//!   \"three_point_arc\", or\n//!   \"tangent_arc\".\n//! The \"orientation mode\" must be:\n//!   \"slerp\",\n//!   \"constant\", or\n//!   \"tangent\".\n//!\n//! WARNING: Not all combinations of \"position mode\" and \"orientation mode\" are compatible.\n//! Specifically, a \"linear\" position mode is *NOT* compatible with a \"tangent\" orientation mode.\n//! A \"constant\" position mode is *ONLY* compatible with a \"slerp\" orientation mode. The two arc\n//! position modes (\"three_point_arc\" and \"tangent_arc\") are compatible with all orientation modes.\n//!\n//! Each (valid) combination of position and orientation mode requires specific input data:\n//!\n//! [1] Linear path:\n//!     - \"linear\" position mode with \"slerp\" orientation mode.\n//!     - Requires \"target pose\" and optional \"blend radius\".\n//!     - See `TaskSpacePathSpec::addLinearPath()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: linear\n//!             orientation mode: slerp\n//!             target pose:\n//!               position: [#, #, #]\n//!               orientation: {w: #, xyz: [#, #, #]}\n//!             blend radius: #\n//! [2] Translation path:\n//!     - \"linear\" position mode with \"constant\" orientation mode.\n//!     - Requires \"target position\" and optional \"blend radius\".\n//!     - See `TaskSpacePathSpec::addTranslation()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: linear\n//!             orientation mode: constant\n//!             target position: [#, #, #]\n//!             blend radius: #\n//! [3] Rotation path:\n//!     - \"constant\" position mode with \"slerp\" orientation mode.\n//!     - Requires \"target orientation\" .\n//!     - See `TaskSpacePathSpec::addRotation()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: constant\n//!             orientation mode: slerp\n//!             target orientation: {w: #, xyz: [#, #, #]}\n//! [4] Three-point arc with constant orientation:\n//!     - \"three_point_arc\" position mode with \"constant\" orientation mode.\n//!     - Requires \"target position\" and \"intermediate position\".\n//!     - See `TaskSpacePathSpec::addThreePointArc()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: three_point_arc\n//!             orientation mode: constant\n//!             target position: [#, #, #]\n//!             intermediate position: [#, #, #]\n//! [5] Three-point arc with tangent orientation:\n//!     - \"three_point_arc\" position mode with \"tangent\" orientation mode.\n//!     - Requires \"target position\" and \"intermediate position\".\n//!     - See `TaskSpacePathSpec::addThreePointArc()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: three_point_arc\n//!             orientation mode: tangent\n//!             target position: [#, #, #]\n//!             intermediate position: [#, #, #]\n//! [6] Three-point arc with orientation target:\n//!     - \"three_point_arc\" position mode with \"slerp\" orientation mode.\n//!     - Requires \"target pose\" and \"intermediate position\".\n//!     - See `TaskSpacePathSpec::addThreePointArcWithOrientationTarget()` in\n//!       `task_space_path_spec.h` for more documentation.\n//!     - Format:\n//!           - position mode: three_point_arc\n//!             orientation mode: slerp\n//!             target pose:\n//!               position: [#, #, #]\n//!               orientation: {w: #, xyz: [#, #, #]}\n//!             intermediate position: [#, #, #]\n//! [7] Tangent arc with constant orientation:\n//!     - \"tangent_arc\" position mode with \"constant\" orientation mode.\n//!     - Requires \"target position\".\n//!     - See `TaskSpacePathSpec::addTangentArc()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: tangent_arc\n//!             orientation mode: constant\n//!             target position: [#, #, #]\n//! [8] Tangent arc with tangent orientation:\n//!     - \"tangent_arc\" position mode with \"tangent\" orientation mode.\n//!     - Requires \"target position\".\n//!     - See `TaskSpacePathSpec::addTangentArc()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: tangent_arc\n//!             orientation mode: tangent\n//!             target position: [#, #, #]\n//! [9] Tangent arc with orientation target:\n//!     - \"tangent_arc\" position mode with \"slerp\" orientation mode.\n//!     - Requires \"target pose\".\n//!     - See `TaskSpacePathSpec::addTangentArcWithOrientationTarget()` in\n//!       `task_space_path_spec.h` for more documentation.\n//!     - Format:\n//!           - position mode: tangent_arc\n//!             orientation mode: slerp\n//!             target pose:\n//!               position: [#, #, #]\n//!               orientation: {w: #, xyz: [#, #, #]}\n//!\n//! If \"initial pose\" or \"path specs\" are unable to be parsed, `nullptr` will be returned. If any\n//! \"path specs\" fail to be parsed, they will be discarded and a warning will be logged, but any\n//! other valid \"path specs\" will continue to be added to the returned `TaskSpacePathSpec`.\nCUMO_EXPORT std::unique_ptr<TaskSpacePathSpec> LoadTaskSpacePathSpecFromFile(\n    const std::filesystem::path &task_space_path_spec_file);\n\n//! Load a `TaskSpacePathSpec` from the contents of a YAML file (`task_space_path_spec_yaml`).\n//!\n//! See `LoadTaskSpacePathSpecFromFile()` documentation for detailed description of required YAML\n//! format.\nCUMO_EXPORT std::unique_ptr<TaskSpacePathSpec> LoadTaskSpacePathSpecFromMemory(\n    const std::string &task_space_path_spec_yaml);\n\n//! Export `task_space_path_spec` as a string.\n//!\n//! The returned string will be in YAML format as documented in `LoadTaskSpacePathSpecFromFile()`.\nCUMO_EXPORT std::string ExportTaskSpacePathSpecToMemory(\n    const TaskSpacePathSpec &task_space_path_spec);\n\n//! Load a `CSpacePathSpec` from file with absolute path `cspace_path_spec_file`.\n//!\n//! The `cspace_spec_file` is expected to correspond to a YAML file specifying a c-space path.\n//! This path specification must include:\n//!   [1] an initial c-space position, and\n//!   [2] a series of c-space waypoints following this initial c-space position.\n//!\n//! The initial c-space position uses the key \"initial c-space position\" and is expected to have the\n//! following format:\n//!\n//!   initial c-space position: [#, #, ... , #]\n//!\n//! where each \"#\" represents a floating point value. The number of elements in this vector sets the\n//! expected number of c-space coordinates for the `CSpacePathSpec`.\n//!\n//! The c-space waypoints are specified as a sequence under the key \"waypoints\", with the following\n//! format:\n//!\n//!   waypoints:\n//!     - [#, #, ... , #]\n//!     - [#, #, ... , #]\n//!     - [#, #, ... , #]\n//!\n//! where the number of waypoints is variable. Each waypoint must have the same number of\n//! c-space coordinates as the \"initial c-space position\", or it will be discarded and a warning\n//! will be logged.\n//!\n//! If \"initial c-space position\" or \"waypoints\" are unable to be parsed, `nullptr` will be\n//! returned. If any \"waypoints\" fail to be parsed, they will be discarded and a warning will be\n//! logged, but any other valid \"waypoints\" will continue to be added to the returned\n//! `CSpacePathSpec`.\nCUMO_EXPORT std::unique_ptr<CSpacePathSpec> LoadCSpacePathSpecFromFile(\n    const std::filesystem::path &cspace_path_spec_file);\n\n//! Load a `CSpacePathSpec` from the contents of a YAML file (`cspace_path_spec_yaml`).\n//!\n//! See `LoadCSpacePathSpecFromFile()` documentation for detailed description of required YAML\n//! format.\nCUMO_EXPORT std::unique_ptr<CSpacePathSpec> LoadCSpacePathSpecFromMemory(\n    const std::string &cspace_path_spec_yaml);\n\n//! Export `cspace_path_spec` as a string.\n//!\n//! The returned string will be in YAML format as documented in `LoadCSpacePathSpecFromFile()`.\nCUMO_EXPORT std::string ExportCSpacePathSpecToMemory(const CSpacePathSpec &cspace_path_spec);\n\n//! Load a `CompositePathSpec` from file with absolute path `composite_path_spec_file`.\n//!\n//! The `composite_path_spec_file` is expected to correspond to a YAML file specifying a path\n//! composed of task space and/or c-space segments. This path specification must include:\n//!   [1] an initial c-space position, and\n//!   [2] a series of task-space and/or c-space path specifications following this initial\n//!       c-space position.\n//!\n//! The initial c-space position uses the key \"initial c-space position\" and is expected to have the\n//! following format:\n//!\n//!   initial c-space position: [#, #, ... , #]\n//!\n//! where each \"#\" represents a floating point value. The number of elements in this vector sets the\n//! expected number of c-space coordinates for the `CSpacePathSpec`.\n//!\n//! The series of path specifications uses the key \"path specs\" and each spec is required to be\n//! labelled as either \"c-space\" or \"task-space\". In addition to including the full\n//! specification for the task space or c-space path (see `LoadTaskSpacePathSpecFromFile()` and\n//! `LoadCSpacePathSpecFromFile()` for details), each path specification must specify a\n//! \"transition mode\" which must be \"skip\", \"free\", or \"linear_task_space\". These modes correspond\n//! to the `CompositePathSpec::TransitionMode` defined in `composite_path_spec.h`.\n//!\n//! An example specification may look like:\n//!\n//! initial c-space position: [#, #, ... , #]\n//! path specs:\n//!   - c-space:\n//!       transition mode: skip\n//!       initial c-space position: [#, #, ... , #]\n//!       waypoints:\n//!         - [#, #, ... , #]\n//!         - [#, #, ... , #]\n//!         - ...\n//!         - [#, #, ... , #]\n//!   - task-space:\n//!       transition mode: linear_task_space\n//!       initial pose:\n//!         position: [#, #, #]\n//!         orientation: { w: #, xyz: [#, #, #] }\n//!       path specs:\n//!         - position mode: linear\n//!           orientation mode: constant\n//!           target position: [#, #, #]\n//!           blend radius: 0.0\n//!         - ...\n//!   - ...\n//!\n//! where any combination of task space and c-space path specifications may be included. The order\n//! of the path specifications in the YAML file dictates the order in which they are added to the\n//! `CompositePathSpec`.\n//!\n//! If \"initial c-space position\" or \"path specs\" are unable to be parsed, `nullptr` will be\n//! returned. If any \"path specs\" fail to be parsed, they will be discarded and a warning will be\n//! logged, but any other valid \"path specs\" will continue to be added to the returned\n//! `CompositePathSpec`.\nCUMO_EXPORT std::unique_ptr<CompositePathSpec> LoadCompositePathSpecFromFile(\n    const std::filesystem::path &composite_path_spec_file);\n\n//! Load a `CompositePathSpec` from the contents of a YAML file (`composite_path_spec_yaml`).\n//!\n//! See `LoadCompositePathSpecFromFile()` documentation for detailed description of required YAML\n//! format.\nCUMO_EXPORT std::unique_ptr<CompositePathSpec> LoadCompositePathSpecFromMemory(\n    const std::string &composite_path_spec_yaml);\n\n//! Export `composite_path_spec` as a string.\n//!\n//! The returned string will be in YAML format as documented in `LoadCompositePathSpecFromFile()`.\nCUMO_EXPORT std::string ExportCompositePathSpecToMemory(\n    const CompositePathSpec &composite_path_spec);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/pose3.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2020-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for representing a pose in 3d.\n\n#pragma once\n\n#include <vector>\n\n#include \"Eigen/Core\"\n#include \"Eigen/Geometry\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/rotation3.h\"\n\nnamespace cumotion {\n\n//! Class representing a 3d pose.\nstruct CUMO_EXPORT Pose3 {\n public:\n  //! Default constructor (set `rotation` identity and `translation` to zero).\n  Pose3();\n\n  //! Construct pose from a `rotation` and `translation`.\n  Pose3(const Rotation3 &rotation, const Eigen::Vector3d &translation);\n\n  //! Construct pose from a 4 x 4 homogeneous transform matrix.\n  //!\n  //! @note The bottom row of the input matrix is assumed to be `[0, 0, 0, 1]`, but is not\n  //! explicitly checked.\n  explicit Pose3(const Eigen::Matrix<double, 4, 4> &matrix);\n\n  //! Create a pure rotational pose.\n  static Pose3 FromRotation(const Rotation3 &rotation);\n\n  //! Create a pure translational pose.\n  static Pose3 FromTranslation(const Eigen::Vector3d &translation);\n\n  //! Create identity pose.\n  static Pose3 Identity();\n\n  //! Return matrix representation of the pose.\n  [[nodiscard]] Eigen::Matrix4d matrix() const;\n\n  //! Returns the inverse pose.\n  [[nodiscard]] Pose3 inverse() const;\n\n  //! Transforms a collection of 3d `vectors` by this pose.\n  [[nodiscard]]\n  std::vector<Eigen::Vector3d> transformVectors(const std::vector<Eigen::Vector3d> &vectors) const;\n\n  //! Transforms a collection of 3d `vectors` by this pose.\n  //!\n  //! Each column of `vectors` represents a 3d vector to be transformed.\n  [[nodiscard]] Eigen::Matrix<double, 3, Eigen::Dynamic> transformVectors(\n      const Eigen::Matrix<double, 3, Eigen::Dynamic> &vectors) const;\n\n  //! Compose poses via * operator.\n  friend CUMO_EXPORT Pose3 operator*(const Pose3 &lhs, const Pose3 &rhs);\n\n  //! Transforms a vector by the given pose.\n  friend CUMO_EXPORT Eigen::Vector3d operator*(const Pose3 &pose, const Eigen::Vector3d &vector);\n\n  //! Rotation component of pose.\n  Rotation3 rotation;\n\n  //! Translation component of pose.\n  Eigen::Vector3d translation;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/rmpflow.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2019-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface to cuMotion's RMPflow implementation\n\n#pragma once\n\n#include <filesystem>\n#include <memory>\n#include <optional>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/rotation3.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Interface class for loading and manipulating RMPflow parameters.\n//! WARNING: This interface may change in a future release.\nclass CUMO_EXPORT RmpFlowConfig {\n public:\n  virtual ~RmpFlowConfig() = default;\n\n  //! Get the value of a parameter, given a \"param_name\" string of the form\n  //! \"<rmp_name>/<parameter_name>\"\n  [[nodiscard]] virtual double getParam(const std::string &param_name) const = 0;\n\n  //! Set the value of the parameter.\n  virtual void setParam(const std::string &param_name, double value) = 0;\n\n  //! Get the names and values of all parameters.  The two vectors will be overwritten if not empty.\n  virtual void getAllParams(std::vector<std::string> &names, std::vector<double> &values) const = 0;\n\n  //! Set all parameters at once.  The vectors \"names\" and \"values\" must have the same size.\n  //! The parameter corresponding to names[i] will be set to the value given by values[i].\n  virtual void setAllParams(const std::vector<std::string> &names,\n                            const std::vector<double> &values) = 0;\n\n  //! Set the world view that will be used for obstacle avoidance. All enabled obstacles in\n  //! `world_view` will be avoided by the RMPflow policy.\n  virtual void setWorldView(const WorldViewHandle &world_view) = 0;\n};\n\n//! Load a set of RMPflow parameters from file, and combine with a robot description to create a\n//! configuration object for consumption by CreateRmpFlow().  The \"end_effector_frame\" should\n//! correspond to a link name as specified in the original URDF or added as a frame in the XRDF used\n//! to create the robot description. All enabled obstacles in `world_view` will be avoided by the\n//! RMPflow policy.\n//!\n//! DEPRECATED: This function is deprecated and will be removed in a future version.\n//!             Use the variant of `CreateRmpFlowConfigFromFile()` that does *not* include\n//!             `end_effector_frame` instead. The end effector frame can then be added using\n//!             `RmpFlow::addTargetFrame()`.\nCUMO_DEPRECATED_EXPORT std::unique_ptr<RmpFlowConfig> CreateRmpFlowConfigFromFile(\n    const std::filesystem::path &rmpflow_config_file,\n    const RobotDescription &robot_description,\n    const std::string &end_effector_frame,\n    const WorldViewHandle &world_view);\n\n//! Load a set of RMPflow parameters from file, and combine with a robot description to create a\n//! configuration object for consumption by CreateRmpFlow(). All enabled obstacles in `world_view`\n//! will be avoided by the RMPflow policy.\nCUMO_EXPORT std::unique_ptr<RmpFlowConfig> CreateRmpFlowConfigFromFile(\n    const std::filesystem::path &rmpflow_config_file,\n    const RobotDescription &robot_description,\n    const WorldViewHandle &world_view);\n\n//! Load a set of RMPflow parameters from string, and combine with a robot description to create a\n//! configuration object for consumption by CreateRmpFlow().  The \"end_effector_frame\" should\n//! correspond to a link name as specified in the original URDF or added as a frame in the XRDF used\n//! to create the robot description. All enabled obstacles in `world_view` will be avoided by the\n//! RMPflow policy.\n//!\n//! DEPRECATED: This function is deprecated and will be removed in a future version.\n//!             Use the variant of `CreateRmpFlowConfigFromMemory()` that does *not* include\n//!             `end_effector_frame` instead. The end effector frame can then be added using\n//!             `RmpFlow::addTargetFrame()`.\nCUMO_DEPRECATED_EXPORT std::unique_ptr<RmpFlowConfig> CreateRmpFlowConfigFromMemory(\n    const std::string &rmpflow_config,\n    const RobotDescription &robot_description,\n    const std::string &end_effector_frame,\n    const WorldViewHandle &world_view);\n\n//! Load a set of RMPflow parameters from string, and combine with a robot description to create a\n//! configuration object for consumption by CreateRmpFlow(). All enabled obstacles in `world_view`\n//! will be avoided by the RMPflow policy.\nCUMO_EXPORT std::unique_ptr<RmpFlowConfig> CreateRmpFlowConfigFromMemory(\n    const std::string &rmpflow_config,\n    const RobotDescription &robot_description,\n    const WorldViewHandle &world_view);\n\n//! Interface class for building and evaluating a motion policy in the RMPflow framework\nclass CUMO_EXPORT RmpFlow {\n public:\n  virtual ~RmpFlow() = default;\n\n  //! Parameters to configure task-space RMPs.\n  //!\n  //! For detailed description of all parameters, see RMPflow documentation\n  //! (https://nvidia-isaac.github.io/cumotion/concepts/rmpflow.html) and associated tuning guide\n  //! (https://nvidia-isaac.github.io/cumotion/concepts/rmpflow_tuning_guide.html). This\n  //! guide provides a mathematical framework for the target RMP and defines variables used\n  //! to document each of the parameters in `TargetRmpConfig`.\n  struct CUMO_EXPORT TargetRmpConfig {\n    //! Create a `TargetRmpConfig` using default parameters.\n    TargetRmpConfig();\n\n    //! Create a `TargetRmpConfig` using parameters from `rmpflow_config`.\n    explicit TargetRmpConfig(const RmpFlowConfig &rmpflow_config);\n\n    //! Parameters to configure a task-space position RMP.\n    struct CUMO_EXPORT PositionConfig {\n      //! Position gain.\n      //!\n      //! Units: m/s^2\n      //! Must be positive.\n      double accel_p_gain{80.0};\n\n      //! Damping gain.\n      //!\n      //! Units: s^-1\n      //! Must be positive.\n      double accel_d_gain{120.0};\n\n      //! Length scale controlling transition between constant acceleration region far from target\n      //! and linear region near target.\n      //!\n      //! Units: m\n      //! Must be positive.\n      double accel_norm_eps{0.075};\n\n      //! Length scale of the Gaussian controlling blending between `S` and `I`.\n      //!\n      //! Units: m\n      //! Must be positive.\n      double metric_alpha_length_scale{0.05};\n\n      //! Controls the minimum contribution of the isotropic `M_near` term to the metric (inertia\n      //! matrix).\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double min_metric_alpha{0.01};\n\n      //! Metric scalar for the isotropic `M_near` contribution to the metric (inertia matrix).\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double max_metric_scalar{10000.0};\n\n      //! Metric scalar for the directional `M_far` contribution to the metric (inertia matrix).\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double min_metric_scalar{2500.0};\n\n      //! Scale factor controlling the strength of boosting near the target.\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double proximity_metric_boost_scalar{20.0};\n\n      //! Length scale of the Gaussian controlling boosting near the target.\n      //!\n      //! Units: m\n      //! Must be positive.\n      double proximity_metric_boost_length_scale{0.02};\n    };\n\n    //! Parameters to configure a task-space orientation RMP.\n    struct CUMO_EXPORT OrientationConfig {\n      //! Position gain.\n      //!\n      //! Units: s^-2\n      //! Must be positive.\n      double accel_p_gain{200.0};\n\n      //! Damping gain.\n      //!\n      //! Units: s^-1\n      //! Must be positive.\n      double accel_d_gain{40.0};\n\n      //! Priority weight relative to other RMPs.\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double metric_scalar{10.0};\n\n      //! Scale factor controlling the strength of boosting near the position target.\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double proximity_metric_boost_scalar{3000.0};\n\n      //! Length scale of the Gaussian controlling boosting near the position target.\n      //!\n      //! Units: m\n      //! Must be positive.\n      double proximity_metric_boost_length_scale{0.05};\n    };\n\n    //! Parameters to configure damping for task-space RMPs.\n    struct CUMO_EXPORT DampingConfig {\n      //! Nonlinear damping gain.\n      //!\n      //! Units: m^-1\n      //! Must be positive.\n      double accel_d_gain{30.0};\n\n      //! Priority weight relative to other RMPs.\n      //!\n      //! Units: (m/s)^-1\n      //! Must be positive.\n      double metric_scalar{50.0};\n\n      //! Additional inertia.\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double inertia{100.0};\n    };\n\n    //! Parameters to configure a task-space position RMP.\n    PositionConfig position_config;\n\n    //! Parameters to configure a task-space orientation RMP.\n    OrientationConfig orientation_config;\n\n    //! Parameters to configure damping for task-space RMPs.\n    DampingConfig damping_config;\n  };\n\n  //! Return the number of task-space target frames.\n  //!\n  //! This count includes both \"active\" target frames (i.e., frames with position and/or orientation\n  //! targets set) as well as \"inactive\" target frames (i.e., frames for which no position or\n  //! orientation targets have been set). This count does *not* include any target frames that have\n  //! been removed.\n  [[nodiscard]] virtual int numTargetFrames() const = 0;\n\n  //! Return the names of all frames with task-space targets.\n  //!\n  //! These names include both \"active\" target frames (i.e., frames with position and/or orientation\n  //! targets set) as well as \"inactive\" target frames (i.e., frames for which no position or\n  //! orientation targets have been set). These names do *not* include any target frames that have\n  //! been removed.\n  [[nodiscard]] virtual std::vector<std::string> targetFrameNames() const = 0;\n\n  //! Enable a task-space target for the frame corresponding to `frame_name`.\n  //!\n  //! The target may be configured using the optional `config`. If `config` is not provided, then\n  //! configuration parameters are taken from the `RmpFlowConfig` originally used to create the\n  //! `RmpFlow` object.\n  //!\n  //! NOTE: The task-space target will not be active until a position and/or orientation target\n  //!       is set.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` is not a valid frame in the `RobotDescription` used to construct `RmpFlow`,\n  //!   2. `frame_name` is already in use as a target frame (i.e., this frame has been added and\n  //!      not removed). This includes the `end_effector_frame` (optionally) used to construct the\n  //!      `RmpflowConfig` used create this `RmpFlow` instance, *OR*\n  //!   3. Any parameter in `config` is invalid.\n  virtual void addTargetFrame(const std::string &frame_name,\n                              std::optional<TargetRmpConfig> config = std::nullopt) = 0;\n\n  //! Remove the task-space target for the frame corresponding to `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void removeTargetFrame(const std::string &frame_name) = 0;\n\n  //! Set both a position and orientation target for `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void setPoseTarget(const std::string &frame_name, const Pose3 &pose) = 0;\n\n  //! Clear both the position and orientation target for `frame_name`.\n  //!\n  //! This function will be a no-op if neither a position nor an orientation target is active for\n  //! `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void clearPoseTarget(const std::string &frame_name) = 0;\n\n  //! Set a position target for `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void setPositionTarget(const std::string &frame_name,\n                                 const Eigen::Vector3d &position) = 0;\n\n  //! Clear the position target for `frame_name`.\n  //!\n  //! This function will be a no-op if a position target is not active for `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void clearPositionTarget(const std::string &frame_name) = 0;\n\n  //! Set an orientation target for `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void setOrientationTarget(const std::string &frame_name,\n                                    const Rotation3 &orientation) = 0;\n\n  //! Clear the orientation target for `frame_name`.\n  //!\n  //! This function will be a no-op if an orientation target is not active for `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void clearOrientationTarget(const std::string &frame_name) = 0;\n\n  //! Set an end-effector position attractor.\n  //!\n  //! The origin of the end effector frame will be driven towards the specified position.\n  //!\n  //! The \"end-effector\" is defined as the first task-space target (i.e., the first frame name\n  //! returned from `targetFrameNames()`). A fatal error will be logged if there are no target\n  //! frames (i.e., `numTargetFrames()` == 0).\n  //!\n  //! DEPRECATED: This function is deprecated and will be removed in a future release.\n  //!             Use `setPositionTarget()` instead.\n  CUMO_DEPRECATED virtual void setEndEffectorPositionAttractor(const Eigen::Vector3d &position) = 0;\n\n  //! Clear end-effector position attractor.\n  //!\n  //! The RMP driving the origin of the end effector frame towards a particular position will be\n  //! deactivated.\n  //!\n  //! The \"end-effector\" is defined as the first task-space target (i.e., the first frame name\n  //! returned from `targetFrameNames()`). A fatal error will be logged if there are no target\n  //! frames (i.e., `numTargetFrames()` == 0).\n  //!\n  //! DEPRECATED: This function is deprecated and will be removed in a future release.\n  //!             Use `clearPositionTarget()` instead.\n  CUMO_DEPRECATED virtual void clearEndEffectorPositionAttractor() = 0;\n\n  //! Set an end-effector orientation attractor.\n  //!\n  //! The orientation of the end effector frame will be driven towards the specified orientation.\n  //!\n  //! The \"end-effector\" is defined as the first task-space target (i.e., the first frame name\n  //! returned from `targetFrameNames()`). A fatal error will be logged if there are no target\n  //! frames (i.e., `numTargetFrames()` == 0).\n  //!\n  //! DEPRECATED: This function is deprecated and will be removed in a future release.\n  //!             Use `setOrientationTarget()` instead.\n  CUMO_DEPRECATED virtual void setEndEffectorOrientationAttractor(const Rotation3 &orientation) = 0;\n\n  //! Clear end-effector orientation attractor.\n  //!\n  //! The RMPs driving the orientation of the end effector frame towards a particular orientation\n  //! will be deactivated.\n  //!\n  //! The \"end-effector\" is defined as the first task-space target (i.e., the first frame name\n  //! returned from `targetFrameNames()`). A fatal error will be logged if there are no target\n  //! frames (i.e., `numTargetFrames()` == 0).\n  //!\n  //! DEPRECATED: This function is deprecated and will be removed in a future release.\n  //!             Use `clearOrientationTarget()` instead.\n  CUMO_DEPRECATED virtual void clearEndEffectorOrientationAttractor() = 0;\n\n  //! Set an attractor in generalized coordinates (configuration space).\n  //!\n  //! The c-space coordinates will be biased towards the specified configuration.\n  //!\n  //! NOTE:  Unlike the end effector attractors, there is always an active c-space attractor (either\n  //!        set using `setCSpaceAttractor()` or using the default value loaded from the robot\n  //!        description).\n  virtual void setCSpaceAttractor(const Eigen::VectorXd &cspace_position) = 0;\n\n  //! Compute configuration-space acceleration from motion policy, given input state. This takes\n  //! into account the current c-space and/or task-space targets, as well as any\n  //! currently-enabled obstacles.\n  virtual void evalAccel(const Eigen::VectorXd &cspace_position,\n                         const Eigen::VectorXd &cspace_velocity,\n                         Eigen::Ref<Eigen::VectorXd> cspace_accel) const = 0;\n\n  //! Compute configuration-space force and metric from motion policy, given input state. This takes\n  //! into account the current c-space and/or task-space targets, as well as any\n  //! currently-enabled obstacles.\n  virtual void evalForceAndMetric(const Eigen::VectorXd &cspace_position,\n                                  const Eigen::VectorXd &cspace_velocity,\n                                  Eigen::Ref<Eigen::VectorXd> cspace_force,\n                                  Eigen::Ref<Eigen::MatrixXd> cspace_metric) const = 0;\n};\n\n//! Create an instance of the RmpFlow interface from an RMPflow configuration.\nCUMO_EXPORT std::unique_ptr<RmpFlow> CreateRmpFlow(const RmpFlowConfig &config);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/robot_description.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2020-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for loading and accessing a robot description\n\n#pragma once\n\n#include <filesystem>\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Forward declaration of Kinematics from `cumotion/kinematics.h`.\nclass Kinematics;\n\n//! Class encapsulating all semantic and kinematic properties of a robot\nclass CUMO_EXPORT RobotDescription {\n public:\n  virtual ~RobotDescription() = default;\n\n  //! Return the number of actuated joints for the robot.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Return the name of a given joint of the robot.\n  [[nodiscard]] virtual std::string cSpaceCoordName(int coord_index) const = 0;\n\n  //! Return default joint positions for the robot.\n  //!\n  //! Returned vector will have length equal to `numCSpaceCoords()`.\n  [[nodiscard]] virtual Eigen::VectorXd defaultCSpaceConfiguration() const = 0;\n\n  //! Return a copy of robot kinematics.\n  [[nodiscard]] virtual std::unique_ptr<Kinematics> kinematics() const = 0;\n\n  //! Return the names of all tool frames (if any) specified in the robot description.\n  [[nodiscard]] virtual std::vector<std::string> toolFrameNames() const = 0;\n};\n\n//! Load a robot description from an XRDF (`robot_xrdf`) and a URDF (`robot_urdf`).\n//!\n//! It is recommended that `robot_xrdf` and `robot_urdf` be specified as absolute filepaths.\n//! Relative paths will be resolved using the same rules as the `std::filesystem::path` type.\n//!\n//! The \"Extended Robot Description Format\" (XRDF) is documented at:\n//! https://nvidia-isaac-ros.github.io/v/release-3.2/concepts/manipulation/xrdf.html\n//!\n//! A fatal error will be logged if:\n//! - `robot_xrdf` is not a valid file path,\n//! - `robot_urdf` is not a valid file path,\n//! - `robot_xrdf` cannot be successfully parsed, *OR*\n//! - `robot_urdf` cannot be successfully parsed.\n[[nodiscard]] CUMO_EXPORT std::unique_ptr<RobotDescription> LoadRobotFromFile(\n    const std::filesystem::path &robot_xrdf,\n    const std::filesystem::path &robot_urdf);\n\n//! Load a robot description from the contents of an XRDF (\"robot_xrdf\") and the contents\n//! of a URDF (\"robot_urdf\").\n//!\n//! The \"Extended Robot Description Format\" (XRDF) is documented at:\n//! https://nvidia-isaac-ros.github.io/v/release-3.2/concepts/manipulation/xrdf.html\n[[nodiscard]] CUMO_EXPORT std::unique_ptr<RobotDescription> LoadRobotFromMemory(\n    const std::string &robot_xrdf,\n    const std::string &robot_urdf);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/robot_segmenter.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2024-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for segmenting out the contribution of a robot from a depth image\n//!\n//! @note This interface is experimental and may evolve in a future release.  It also lacks\n//!       a corresponding Python API.\n\n#pragma once\n\n#include <limits>\n#include <memory>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/pose3.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/vision.h\"\n\nnamespace cumotion {\n\n//! Interface class for segmenting (masking) out the contribution of a robot from a depth image.\nclass CUMO_EXPORT RobotSegmenter {\n public:\n  enum class RobotGeometryKind {\n    SELF_COLLISION_SPHERES,   //!< Use self-collision spheres for robot segmentation.\n    WORLD_COLLISION_SPHERES,  //!< Use world-collision spheres for robot segmentation.\n  };\n\n  virtual ~RobotSegmenter() {}\n\n  //! For the given `camera_pose_in_robot_frame`, `cspace_position` of the robot, and\n  //! `input_depth_image`, compute a filtered `output_depth_image` and/or `output_depth_mask`.\n  //!\n  //! Both `output_depth_image` and `output_mask` are optional, and passing in `nullptr` for either\n  //! will skip the corresponding computation.  A warning will be logged if `nullptr` is passed in\n  //! for both.\n  //!\n  //! If not null, `output_depth_image` will be populated with the same depth values as\n  //! `input_depth_image` for all pixels where the corresponding point (in three dimensions) is not\n  //! contained within the robot geometry, taking into account the `additional_buffer_distance`\n  //! specified during construction of the `RobotSegmenter`.  Depth values for pixels corresponding\n  //! to points contained in the robot geometry will be set to zero.\n  //!\n  //! If not null, `output_mask` will contain a zero for all pixels where the corresponding point\n  //! (in three dimensions) is contained within the robot geometry and a value larger than zero for\n  //! all other pixels.  The mask is pixel-aligned with `input_depth_image`.  The particular\n  //! nonzero value used depends on the scalar type of the `DepthImage`:\n  //!\n  //! * For a scalar type of `float`, the nonzero value is 1.0.  Note that `meters_per_unit` for\n  //!   the `DepthImage` has no effect; it's the \"raw\" value that's set to 1.0.\n  //! * For a scalar type of `uint16_t`, the nonzero value is 65,535 (the maximum representable\n  //!   integer).  Again, `meters_per_unit` for the `DepthImage` has no effect.\n  //!\n  //! `min_depth` and `max_depth` are given in meters and influence the interpretation of depth\n  //! values read from the `input_depth_image`.  A depth value outside the range\n  //! (`min_depth`, `max_depth`) is assumed to correspond to a point that is not contained within\n  //! the robot geometry, and such a depth value will be written to the `output_depth_image`\n  //! unchanged.\n  //!\n  //! The current implementation requires that all depth images have `HOST` or `MANAGED` residency.\n  //!\n  //! A fatal error will be logged if:\n  //!\n  //! 1. `output_depth_image` is not null, and its width or height differs from the width or height\n  //!    (respectively) of `input_depth_image`.\n  //! 2. `output_mask` is not null, and its width or height differs from the width or height\n  //!    (respectively) of `input_depth_image`.\n  //! 3. `output_depth_image` or `output_mask` is provided as a `DepthImage` with a `const` scalar\n  //!    type.\n  //! 4. `input_depth_image`, `output_depth_image`, or `output_mask` have `DEVICE` residency.\n  virtual void segmentDepthImage(const Pose3 &camera_pose_in_robot_frame,\n                                 const Eigen::VectorXd &cspace_position,\n                                 const DepthImageBase &input_depth_image,\n                                 DepthImageBase *output_depth_image,\n                                 DepthImageBase *output_mask,\n                                 double min_depth = 0.01,\n                                 double max_depth = std::numeric_limits<double>::max()) = 0;\n};\n\n//! Create a `RobotSegmenter` from a `robot_description` and `camera_intrinsics`.\n//!\n//! `camera_intrinsics` must be in the four-parameter \"pinhole\" format with zero skew.\n//!\n//! If provided, `additional_buffer_distance` will inflate the effective size of the robot geometry\n//! (e.g., collision spheres) such that a point within that distance is considered to be contained\n//! within the geometry.  This `additional_buffer_distance` augments rather than replaces any\n//! per-link buffer distance specified in the XRDF for the robot.\n//!\n//! `additional_buffer_distance` may be negative, shrinking the effective size of the robot\n//! geometry.  A given part of the geometry (e.g., a collision sphere) will be ignored altogether\n//! once `additional_buffer_distance` is sufficiently negative (e.g., negative with a magnitude\n//! that exceeds the radius, for the case of a sphere).\n//!\n//! The currently-supported values for `robot_geometry_kind` are `SELF_COLLISION_SPHERES` and\n//! `WORLD_COLLISION_SPHERES`, mapping to the corresponding collision geometry types in XRDF.\n//!\n//! A fatal error will be logged if the `camera_intrinsics` are not in the four-parameter \"pinhole\"\n//! format (with zero skew).\nCUMO_EXPORT std::unique_ptr<RobotSegmenter>\nCreateRobotSegmenter(const RobotDescription &robot_description,\n                     const CameraIntrinsics &camera_intrinsics,\n                     double additional_buffer_distance = 0.0,\n                     RobotSegmenter::RobotGeometryKind robot_geometry_kind =\n                         RobotSegmenter::RobotGeometryKind::WORLD_COLLISION_SPHERES);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/robot_world_inspector.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2023-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for querying spatial relationships between a robot and a world.\n\n#pragma once\n\n#include <memory>\n#include <optional>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Interface for querying spatial relationships between a robot and a world.\nclass CUMO_EXPORT RobotWorldInspector {\n public:\n  virtual ~RobotWorldInspector() = default;\n\n  //! Clear the existing world view from `RobotWorldInspector`.\n  //!\n  //! All `ObstacleHandle`s will be invalid from the perspective of `RobotWorldInspector`.\n  //!\n  //! `inCollisionWithObstacle()` will always return `false`.\n  virtual void clearWorldView() = 0;\n\n  //! Set the internal world view used by `RobotWorldInspector`.\n  virtual void setWorldView(const WorldViewHandle &world_view) = 0;\n\n  //! Return the number of world-collision spheres in the robot representation.\n  [[nodiscard]] virtual int numWorldCollisionSpheres() const = 0;\n\n  //! Return the number of self-collision spheres in the robot representation.\n  [[nodiscard]] virtual int numSelfCollisionSpheres() const = 0;\n\n  //! Return the name of the frame associated with the world-collision sphere corresponding to\n  //! `world_collision_sphere_index`.\n  //!\n  //! A fatal error is logged if:\n  //!  1. `world_collision_sphere_index` is greater than or equal to the number of world-collision\n  //!     spheres attached to the robot (i.e., `numWorldCollisionSpheres()`), *OR*\n  //!  2. `world_collision_sphere_index` is negative.\n  [[nodiscard]]\n  virtual std::string worldCollisionSphereFrameName(int world_collision_sphere_index) const = 0;\n\n  //! Return the name of the frame associated with the self-collision sphere corresponding to\n  //! `self_collision_sphere_index`.\n  //!\n  //! A fatal error is logged if:\n  //!  1. `self_collision_sphere_index` is greater than or equal to the number of self-collision\n  //!     spheres attached to the robot (i.e., `numSelfCollisionSpheres()`), *OR*\n  //!  2. `self_collision_sphere_index` is negative.\n  [[nodiscard]]\n  virtual std::string selfCollisionSphereFrameName(int self_collision_sphere_index) const = 0;\n\n  //! Return the radii of all world-collision spheres on the robot.\n  //!\n  //! Each radius is the \"effective radius\", representing the sum of the \"base radius\" of the sphere\n  //! and any specified world-collision buffer distance.\n  //!\n  //! The order of `sphere_radii` corresponds to the order of `sphere_positions` returned by\n  //! `worldCollisionSpherePositions()`.\n  //!\n  //! The length of `sphere_radii` will be set to the number of world-collision spheres specified in\n  //! the `RobotDescription`.\n  virtual void worldCollisionSphereRadii(std::vector<double> &sphere_radii) const = 0;\n\n  //! Return the radii of all self-collision spheres on the robot.\n  //!\n  //! Each radius is the \"effective radius\", representing the sum of the \"base radius\" of the sphere\n  //! and any specified self-collision buffer distance.\n  //!\n  //! The order of `sphere_radii` corresponds to the order of `sphere_positions` returned by\n  //! `selfCollisionSpherePositions()`.\n  //!\n  //! The length of `sphere_radii` will be set to the number of self-collision spheres specified in\n  //! the `RobotDescription`.\n  virtual void selfCollisionSphereRadii(std::vector<double> &sphere_radii) const = 0;\n\n  //! Compute positions of all world-collision spheres on the robot at the specified\n  //! `cspace_position`.\n  //!\n  //! The order of `sphere_positions` corresponds to order of `sphere_radii` returned by\n  //! `worldCollisionSphereRadii()`.\n  //!\n  //! If the length of `sphere_positions` is less than the number of world-collision spheres\n  //! specified in the `RobotDescription`, `sphere_positions` will be resized to the number of\n  //! world-collision spheres. If the length of `sphere_positions` is larger than the number of\n  //! world-collision spheres, the vector will NOT be resized. Only the first n elements will be\n  //! written to, where n is the number of world-collision spheres.\n  virtual void worldCollisionSpherePositions(\n      const Eigen::VectorXd &cspace_position,\n      std::vector<Eigen::Vector3d> &sphere_positions) const = 0;\n\n  //! Compute positions of all self-collision spheres on the robot at the specified\n  //! `cspace_position`.\n  //!\n  //! The order of `sphere_positions` corresponds to order of `sphere_radii` returned by\n  //! `selfCollisionSphereRadii()`.\n  //!\n  //! If the length of `sphere_positions` is less than the number of self-collision spheres\n  //! specified in the `RobotDescription`, `sphere_positions` will be resized to the number of\n  //! self-collision spheres. If the length of `sphere_positions` is larger than the number of\n  //! self-collision spheres, the vector will NOT be resized. Only the first n elements will be\n  //! written to, where n is the number of self-collision spheres.\n  virtual void selfCollisionSpherePositions(\n      const Eigen::VectorXd &cspace_position,\n      std::vector<Eigen::Vector3d> &sphere_positions) const = 0;\n\n  //! Determine whether a specified `cspace_position` puts the robot into collision\n  //! with an obstacle.\n  //!\n  //! If no world view is specified, `false` is returned for any `cspace_position`.\n  virtual bool inCollisionWithObstacle(const Eigen::VectorXd &cspace_position) const = 0;\n\n  //! Compute the minimum pair-wise signed distance between the set of robot spheres and the set of\n  //! obstacles for the provided position in configuration space.  A positive distance implies that\n  //! the obstacle and the robot are *NOT* in collision.\n  //!\n  //! A fatal error is logged if the `RobotWorldInspector` does not have a world view set.\n  //!\n  //! NOTE: A distance of zero implies that the closest robot sphere to any obstacle intersects the\n  //!       obstacle at a point.\n  virtual double minDistanceToObstacle(const Eigen::VectorXd &cspace_position) const = 0;\n\n  //! Compute the signed distance between a given obstacle and collision sphere for the provided\n  //! position in configuration space.  A positive distance implies that the obstacle and the sphere\n  //! are *NOT* in collision.\n  //!\n  //! A fatal error is logged if:\n  //!  - `world_collision_sphere_index` is greater than or equal to the number of world-collision\n  //!    spheres attached to the robot (i.e., `numWorldCollisionSpheres()`), or it is negative.\n  //!  - The `RobotWorldInspector` does not have a world view set.\n  //!\n  //! NOTE: If the world-collision sphere intersects the `obstacle` a distance of zero is returned.\n  virtual double distanceToObstacle(const World::ObstacleHandle &obstacle,\n                                    int world_collision_sphere_index,\n                                    const Eigen::VectorXd &cspace_position) const = 0;\n\n  //! Indicate whether the robot is in self-collision at the given `cspace_position`.\n  //!\n  //! The robot is considered to be in self-collision if any collision sphere on any frame\n  //! intersects a collision sphere from another frame for which collisions have not been masked.\n  virtual bool inSelfCollision(const Eigen::VectorXd &cspace_position) const = 0;\n\n  //! Return pairs of frames for which self-collision occurs at the given `cspace_position`.\n  //!\n  //! Each pair of self-collision frames will be returned once in arbitrary order (e.g., the pair\n  //! {\"linkA\", \"linkB\"} may be returned *OR* the pair {\"linkB\", \"linkA\"} may be returned, but\n  //! not both).\n  virtual std::vector<std::pair<std::string, std::string>> framesInSelfCollision(\n      const Eigen::VectorXd &cspace_position) const = 0;\n};\n\n//! Create a `RobotWorldInspector` for a given `robot_description` and `world_view`.\n//!\n//! The `world_view` is optional. If not provided, queries related to obstacle collisions\n//! (e.g., `distanceToObstacle()`) will not be functional unless a world view is set by\n//! `setWorldView()` after construction.\nCUMO_EXPORT std::unique_ptr<RobotWorldInspector> CreateRobotWorldInspector(\n    const RobotDescription &robot_description,\n    std::optional<WorldViewHandle> world_view);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/rotation3.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2020-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for representing a rotation in 3d.\n\n#pragma once\n\n#include \"Eigen/Core\"\n#include \"Eigen/Geometry\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Class representing a 3d rotation.\nclass CUMO_EXPORT Rotation3 {\n public:\n  //! Construct rotation from a quaternion.\n  //!\n  //! By default, the `quaternion` will be normalized, but the argument `skip_normalization` can be\n  //! set to `true` to skip this normalization. This should only be done if the `quaternion` is\n  //! known to be normalized; otherwise, `Rotation3` operations will not work as expected.\n  explicit Rotation3(const Eigen::Quaterniond &quaternion, bool skip_normalization = false);\n  Rotation3(double w, double x, double y, double z);\n\n  //! Return a rotation that rotates by an angle (in radians) around a given axis.\n  //!\n  //! By default, the `axis` will be normalized (and if the axis norm is near zero, an identity\n  //! rotation will be constructed). The argument `skip_normalization` can set to `true` to skip\n  //! this normalization. This should only be done if the `axis` is known to be normalized;\n  //! otherwise, `Rotation3` may represent an invalid rotation.\n  static Rotation3 FromAxisAngle(const Eigen::Vector3d &axis,\n                                 double angle,\n                                 bool skip_normalization = false);\n\n  //! Return a rotation converted from a scaled axis.\n  //!\n  //! The magnitude of `scaled_axis` specifies the rotation angle in radians and the direction\n  //! of `scaled_axis` specifies the axis of rotation.\n  static Rotation3 FromScaledAxis(const Eigen::Vector3d &scaled_axis);\n\n  //! Return a rotation converted from a 3 x 3 rotation matrix.\n  //!\n  //! Internally, the `rotation_matrix` will be converted to a `Eigen::Quaterniond`. By default,\n  //! this quaternion will be normalized, but the argument `skip_normalization` can be\n  //! set to `true` to skip this normalization. This should only be done if the `rotation_matrix` is\n  //! known to be SO3; otherwise, `Rotation3` operations will not work as expected.\n  static Rotation3 FromMatrix(const Eigen::Matrix3d &rotation_matrix,\n                              bool skip_normalization = false);\n\n  //! Create identity rotation.\n  static Rotation3 Identity();\n\n  //! Return quaternion representation of the rotation. Returned quaternion is always normalized.\n  inline Eigen::Quaterniond quaternion() const { return quaternion_; }\n\n  //! Return `w` component of the quaternion representation of the rotation.\n  [[nodiscard]] double w() const { return quaternion_.w(); }\n\n  //! Return `x` component of the quaternion representation of the rotation.\n  [[nodiscard]] double x() const { return quaternion_.x(); }\n\n  //! Return `y` component of the quaternion representation of the rotation.\n  [[nodiscard]] double y() const { return quaternion_.y(); }\n\n  //! Return `z` component of the quaternion representation of the rotation.\n  [[nodiscard]] double z() const { return quaternion_.z(); }\n\n  //! Return matrix representation of the rotation.\n  [[nodiscard]] Eigen::Matrix3d matrix() const;\n\n  //! Return scaled axis representation of the rotation where magnitude of the returned vector\n  //! represents the rotation angle in radians.\n  [[nodiscard]] Eigen::Vector3d scaledAxis() const;\n\n  //! Returns the inverse rotation.\n  [[nodiscard]] Rotation3 inverse() const;\n\n  //! Compute the minimum angular distance (in radians) between two rotations.\n  static double Distance(const Rotation3 &rotation0, const Rotation3 &rotation1);\n\n  //! Smoothly interpolate between two rotations using spherical linear interpolation (\"slerp\").\n  //!\n  //! Intermediate rotations will follow a geodesic between `rotation0` and `rotation1` when\n  //! represented as quaternions on a unit sphere.\n  //!\n  //! The parameter `t` must be in range [0, 1], with 0 corresponding to `rotation0` and 1\n  //! corresponding to `rotation1`.\n  static Rotation3 Slerp(const Rotation3 &rotation0, const Rotation3 &rotation1, double t);\n\n  //! Compose rotations via * operator.\n  friend CUMO_EXPORT Rotation3 operator*(const Rotation3 &lhs, const Rotation3 &rhs);\n\n  //! Rotates a vector by the given rotation.\n  friend CUMO_EXPORT Eigen::Vector3d operator*(const Rotation3 &rotation,\n                                               const Eigen::Vector3d &vector);\n\n  //! See https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html\n  EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n private:\n  Eigen::Quaterniond quaternion_;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/task_space_path.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/pose3.h\"\n\nnamespace cumotion {\n\n//! Represent a path through task space (i.e., SE(3) group representing 6-dof poses).\n//!\n//! This path is parameterized by independent variable `s` and is generally expected to be\n//! continuous, but not necessarily smooth.\nclass CUMO_EXPORT TaskSpacePath {\n public:\n  virtual ~TaskSpacePath() = default;\n\n  //! Indicates continuous range of independent values, `s`, for which the path is defined.\n  struct CUMO_EXPORT Domain {\n    //! Minimum value of `s` defining domain.\n    double lower;\n\n    //! Maximum value of `s` defining domain.\n    double upper;\n\n    //! Convenience function to return the span of `s` values included in domain.\n    [[nodiscard]] double span() const { return upper - lower; }\n  };\n\n  //! Return the domain for the path.\n  [[nodiscard]] virtual Domain domain() const = 0;\n\n  //! Evaluate the path at the given `s`.\n  //!\n  //! A fatal error is logged if `s` is outside of the path `domain()`.\n  [[nodiscard]] virtual Pose3 eval(double s) const = 0;\n\n  //! Return the total translation distance accumulated along the path.\n  //!\n  //! This length is not, in general, equal to the translation distance between the poses at the\n  //! lower and upper bounds of the domain.\n  //!\n  //! E.g., if a path moved linearly from (0,0,0) to (0,1,0) to (1,1,0) to (1,0,0), then the path\n  //! length would be 3 while the net translation between the positions at the lower (0,0,0) and\n  //! upper (1,0,0) bounds of the domain would be 1.\n  [[nodiscard]] virtual double pathLength() const = 0;\n\n  //! Return the total angular distance (in radians) accumulated throughout the path.\n  //!\n  //! Similar to `pathLength()`, this value is not, in general, equal to the minimum angle between\n  //! the rotations at the lower and upper bounds of the domain.\n  //!\n  //! E.g., if a path rotates 2 pi around a single axis followed by pi around another axis, then the\n  //! accumulated rotation would be 3 pi, while the net rotation between the rotations at the lower\n  //! and upper bounds of the domain would be pi.\n  [[nodiscard]] virtual double accumulatedRotation() const = 0;\n\n  //! Return the minimum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These minimum position values are not, in general, synchronous. Instead, they represent\n  //!       the minimum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::Vector3d minPosition() const = 0;\n\n  //! Return the maximum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These maximum position values are not, in general, synchronous. Instead, they represent\n  //!       the maximum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::Vector3d maxPosition() const = 0;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/task_space_path_spec.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/task_space_path.h\"\n\nnamespace cumotion {\n\n//! The `TaskSpacePathSpec` is used to procedurally specify a `TaskSpacePath` from a series of\n//! continuous task space segments.\n//!\n//! Each segment can have position that is:\n//!   [1] constant,\n//!   [2] linearly interpolated,\n//!   [3] or defined by a circular arc.\n//! For the case where position follows an arc, the arc can be defined either by a series of three\n//! points, or by two endpoints and a constraint that the arc be tangent to the previous segment.\n//!\n//! Each segment may have orientation that is:\n//!   [1] constant (w.r.t. to the fixed global frame),\n//!   [2] smoothly blended via spherical linear interpolation (i.e., \"slerp\"), or\n//!   [3] constant w.r.t. the tangent direction of the position path (i.e., \"tangent orientation\").\n//! The \"tangent orientation\" specification is only relevant for path segments for which position is\n//! defined by an arc (since for linear position paths, the \"tangent orientation\" specification\n//! would reduce to a constant orientation w.r.t. the fixed global frame).\nclass CUMO_EXPORT TaskSpacePathSpec {\n public:\n  virtual ~TaskSpacePathSpec() = default;\n\n  //! Add a path to linearly connect the previous pose to the `target_pose`.\n  //!\n  //! Position will use linear interpolation and orientation will use \"slerp\".\n  //!\n  //! An optional `blend_radius` can be used to add arc segments between successive segments.\n  //! This arc segment will be added *only* if the successive segments both represent a path for\n  //! which position is linearly interpolated. Moreover, the `blend_radius` will be capped such that\n  //! no more than half of either linear segment is replaced by the intermediate arc segment. If\n  //! `blend_radius` is <= 0, then no blending will be performed.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addLinearPath(const Pose3 &target_pose, double blend_radius = 0.0) = 0;\n\n  //! Add a translation-only path to linearly connect the previous pose to the `target_position`.\n  //!\n  //! Position will use linear interpolation, and orientation will be constant.\n  //!\n  //! An optional `blend_radius` can be used to add arc segments between successive segments.\n  //! This arc segment will be added *only* if the successive segments both represent a path for\n  //! which position is linearly interpolated. Moreover, the `blend_radius` will be capped such that\n  //! no more than half of either linear segment is replaced by the intermediate arc segment. If\n  //! `blend_radius` is <= 0, then no blending will be performed.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addTranslation(const Eigen::Vector3d &target_position,\n                              double blend_radius = 0.0) = 0;\n\n  //! Add a rotation-only path connecting the previous pose to the `target_rotation`.\n  //!\n  //! Orientation will use \"slerp\" for interpolation, and position will be constant.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addRotation(const Rotation3 &target_rotation) = 0;\n\n  //! Add a path to connect the previous pose to the `target_position` along a circular arc that\n  //! passes through `intermediate_position`.\n  //!\n  //! Position will follow a \"three-point arc\" where the previous position and `target_position` are\n  //! endpoints and `intermediate_position` is an intermediate point on the arc.\n  //!\n  //! If `constant_orientation` is set to `true`, the orientation will be constant throughout the\n  //! arc.\n  //!\n  //! If `constant_orientation` is set to `false`, the orientation will remain fixed relative to the\n  //! tangent direction of the arc (i.e., \"tangent orientation\"; this tangent orientation is defined\n  //! such that if the angular distance of the arc is N radians, then the change in orientation\n  //! throughout the arc will be N radians about the normal axis of the arc).\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addThreePointArc(const Eigen::Vector3d &target_position,\n                                const Eigen::Vector3d &intermediate_position,\n                                bool constant_orientation = true) = 0;\n\n  //! Add a path to connect the previous pose to the `target_pose` along a circular arc that\n  //! passes through `intermediate_position`.\n  //!\n  //! Position will follow a \"three-point arc\" where the previous position and\n  //! `target_pose.translation` are endpoints and `intermediate_position` is an intermediate point\n  //! on the arc.\n  //!\n  //! Orientation will use \"slerp\" for interpolation between the previous orientation and\n  //! `target_pose.rotation`.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addThreePointArcWithOrientationTarget(\n      const Pose3 &target_pose,\n      const Eigen::Vector3d &intermediate_position) = 0;\n\n  //! Add a path to connect the previous pose to the `target_position` along a circular arc that\n  //! is tangent to the previous segment\n  //!\n  //! Position will follow a \"tangent arc\" where the previous position and `target_position` are\n  //! endpoints and the arc is tangent to the previous segment.\n  //!\n  //! It is required that at least one previous segment on the path defines a change in position for\n  //! a \"tangent arc\" to be able to be added. If no segments have been added or only rotation\n  //! segments have been added, then an error will be logged, no \"tangent arc\" segment will be\n  //! added, and `false` will be returned.\n  //!\n  //! If `constant_orientation` is set to `true`, the orientation will be constant throughout the\n  //! arc.\n  //!\n  //! If `constant_orientation` is set to `false`, the orientation will remain fixed relative to the\n  //! tangent direction of the arc (i.e., \"tangent orientation\"; this tangent orientation is defined\n  //! such that if the angular distance of the arc is N radians, then the change in orientation\n  //! throughout the arc will be N radians about the normal axis of the arc).\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addTangentArc(const Eigen::Vector3d &target_position,\n                             bool constant_orientation = true) = 0;\n\n  //! Add a path to connect the previous pose to the `target_pose` along a circular arc that\n  //! is tangent to the previous segment.\n  //!\n  //! Position will follow a \"tangent arc\" where the previous position and `target_pose.translation`\n  //! are endpoints and the arc is tangent to the previous segment.\n  //!\n  //! It is required that at least one previous segment on the path defines a change in position for\n  //! a \"tangent arc\" to be able to be added. If no segments have been added or only rotation\n  //! segments have been added, then an error will be logged, no \"tangent arc\" segment will be\n  //! added, and `false` will be returned.\n  //!\n  //! Orientation will use \"slerp\" for interpolation between the previous orientation and\n  //! `target_pose.rotation`.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addTangentArcWithOrientationTarget(const Pose3 &target_pose) = 0;\n\n  //! Generate a continuous path between all of the procedurally added task space path segments.\n  //!\n  //! The lower bound of the domain of the generated path will be zero and the upper bound will be\n  //! determined by task space distance.\n  [[nodiscard]] virtual std::unique_ptr<TaskSpacePath> generatePath() const = 0;\n};\n\n//! Create a `TaskSpacePathSpec` with the specified `initial_pose`.\nCUMO_EXPORT std::unique_ptr<TaskSpacePathSpec> CreateTaskSpacePathSpec(const Pose3 &initial_pose);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/text_style.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief ANSI escape sequence definitions for formatting console text\n//!\n//! See, for example,\n//! https://en.wikipedia.org/wiki/ANSI_escape_code#SGR_(Select_Graphic_Rendition)_parameters\n\n#pragma once\n\nnamespace cumotion {\nnamespace text_style {\n\n// Reset all attributes.\nstatic constexpr char RESET[] = \"\\x1B[0m\";\n\n// Formatting\nstatic constexpr char BOLD[] = \"\\x1B[1m\";\nstatic constexpr char DIM[] = \"\\x1B[2m\";\nstatic constexpr char UNDERLINE[] = \"\\x1B[4m\";\nstatic constexpr char BLINK[] = \"\\x1B[5m\";\nstatic constexpr char REVERSE[] = \"\\x1B[7m\";\n\n// Foreground colors\nstatic constexpr char DEFAULT[] = \"\\x1B[39m\";\nstatic constexpr char BLACK[] = \"\\x1B[30m\";\nstatic constexpr char RED[] = \"\\x1B[31m\";\nstatic constexpr char GREEN[] = \"\\x1B[32m\";\nstatic constexpr char YELLOW[] = \"\\x1B[33m\";\nstatic constexpr char BLUE[] = \"\\x1B[34m\";\nstatic constexpr char MAGENTA[] = \"\\x1B[35m\";\nstatic constexpr char CYAN[] = \"\\x1B[36m\";\nstatic constexpr char LIGHT_GRAY[] = \"\\x1B[37m\";\nstatic constexpr char DARK_GRAY[] = \"\\x1B[90m\";\nstatic constexpr char BRIGHT_RED[] = \"\\x1B[91m\";\nstatic constexpr char BRIGHT_GREEN[] = \"\\x1B[92m\";\nstatic constexpr char BRIGHT_YELLOW[] = \"\\x1B[93m\";\nstatic constexpr char BRIGHT_BLUE[] = \"\\x1B[94m\";\nstatic constexpr char BRIGHT_MAGENTA[] = \"\\x1B[95m\";\nstatic constexpr char BRIGHT_CYAN[] = \"\\x1B[96m\";\nstatic constexpr char WHITE[] = \"\\x1B[97m\";\n\n// Background colors\nstatic constexpr char BG_DEFAULT[] = \"\\x1B[49m\";\nstatic constexpr char BG_BLACK[] = \"\\x1B[40m\";\nstatic constexpr char BG_RED[] = \"\\x1B[41m\";\nstatic constexpr char BG_GREEN[] = \"\\x1B[42m\";\nstatic constexpr char BG_YELLOW[] = \"\\x1B[43m\";\nstatic constexpr char BG_BLUE[] = \"\\x1B[44m\";\nstatic constexpr char BG_MAGENTA[] = \"\\x1B[45m\";\nstatic constexpr char BG_CYAN[] = \"\\x1B[46m\";\nstatic constexpr char BG_LIGHT_GRAY[] = \"\\x1B[47m\";\nstatic constexpr char BG_DARK_GRAY[] = \"\\x1B[100m\";\nstatic constexpr char BG_BRIGHT_RED[] = \"\\x1B[101m\";\nstatic constexpr char BG_BRIGHT_GREEN[] = \"\\x1B[102m\";\nstatic constexpr char BG_BRIGHT_YELLOW[] = \"\\x1B[103m\";\nstatic constexpr char BG_BRIGHT_BLUE[] = \"\\x1B[104m\";\nstatic constexpr char BG_BRIGHT_MAGENTA[] = \"\\x1B[105m\";\nstatic constexpr char BG_BRIGHT_CYAN[] = \"\\x1B[106m\";\nstatic constexpr char BG_WHITE[] = \"\\x1B[107m\";\n\n}  // namespace text_style\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/trajectory.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Represent a path through configuration space (i.e., \"c-space\") parameterized by time.\nclass CUMO_EXPORT Trajectory {\n public:\n  virtual ~Trajectory() = default;\n\n  //! Return the number of configuration space coordinates for the kinematic structure.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Indicates the continuous range of time values, `t`, for which the trajectory and its\n  //! derivatives are defined.\n  struct CUMO_EXPORT Domain {\n    //! Minimum value of time defining domain.\n    double lower;\n\n    //! Maximum value of time defining domain.\n    double upper;\n\n    //! Convenience function to return the span of time values included in domain.\n    [[nodiscard]] double span() const { return upper - lower; }\n  };\n\n  //! Return the domain for the trajectory.\n  [[nodiscard]] virtual Domain domain() const = 0;\n\n  //! Evaluate the trajectory at the given `time`.\n  virtual void eval(double time,\n                    Eigen::VectorXd *cspace_position,\n                    Eigen::VectorXd *cspace_velocity = nullptr,\n                    Eigen::VectorXd *cspace_acceleration = nullptr,\n                    Eigen::VectorXd *cspace_jerk = nullptr) const = 0;\n\n  //! Evaluate specified trajectory derivative at the given `time` and return value.\n  //!\n  //! The default `derivative_order` is the \"zeroth derivative\" (which is simply the c-space\n  //! position of the trajectory). Setting `derivative_order` to 1 will output the c-space velocity,\n  //! with higher `derivative_order`s corresponding to higher derivatives.\n  //!\n  //! Trajectories are expected to support at least the third derivative (i.e., \"jerk\").\n  //!\n  //! The default implementation internally calls the above `eval()` function and will log a fatal\n  //! error if the `derivative_order` is not in range [0, 3].\n  [[nodiscard]] virtual Eigen::VectorXd eval(double time, int derivative_order = 0) const;\n\n  //! Return the minimum position for each c-space coordinate within the defined `domain()`.\n  //!\n  //! NOTE: These minimum position values are not, in general, synchronous. Instead, they represent\n  //!       the minimum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd minPosition() const = 0;\n\n  //! Return the maximum position for each c-space coordinate within the defined `domain()`.\n  //!\n  //! NOTE: These maximum position values are not, in general, synchronous. Instead, they represent\n  //!       the maximum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd maxPosition() const = 0;\n\n  //! Return the maximum magnitude of velocity for each c-space coordinate within the defined\n  //! `domain()`.\n  //!\n  //! NOTE: These maximum magnitude values are not, in general, synchronous. Instead, they represent\n  //!       the maximum magnitudes independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd maxVelocityMagnitude() const = 0;\n\n  //! Return the maximum magnitude of acceleration for each c-space coordinate within the defined\n  //! `domain()`.\n  //!\n  //! NOTE: These maximum magnitude values are not, in general, synchronous. Instead, they represent\n  //!       the maximum magnitudes independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd maxAccelerationMagnitude() const = 0;\n\n  //! Return the maximum magnitude of jerk for each c-space coordinate within the defined\n  //! `domain()`.\n  //!\n  //! NOTE: These maximum magnitude values are not, in general, synchronous. Instead, they represent\n  //!       the maximum magnitudes independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd maxJerkMagnitude() const = 0;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/trajectory_generator.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/kinematics.h\"\n#include \"cumotion/trajectory.h\"\n\nnamespace cumotion {\n\n//! Configure a trajectory generator that can compute smooth* trajectories. A trajectory generation\n//! problem is specified by:\n//!\n//!   1. Bound constraints (position, velocity, and acceleration),\n//!   2. Intermediate position waypoints, and\n//!   3. Limits on some combination of position, velocity, acceleration and jerk.\n//!\n//! Times for domain bounds and intermediate waypoints are *not* specified. It is assumed that a\n//! time-optimal trajectory that respects the constraints (with the definition of time-optimality\n//! being implementation-specific) will be generated.\n//!\n//! * The definition of \"smooth\" is implementation-specific. Currently, `CSpaceTrajectoryGenerator`\n//!   uses a single implementation that interpolates through waypoints using a series of cubic\n//!   splines. For this implementation, \"smooth\" is defined as smooth velocity, continuous\n//!   acceleration, and bounded jerk.\nclass CUMO_EXPORT CSpaceTrajectoryGenerator {\n public:\n  virtual ~CSpaceTrajectoryGenerator() = default;\n\n  //! Return the number of configuration space coordinates for the trajectory generator.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Set position limits.\n  //!\n  //! A fatal error is logged if either `min_position` or `max_position` does not have a length\n  //! matching the expected number of c-space coordinates, or if any coordinate of\n  //! `max_position` is not greater than the corresponding coordinate of `min_position`.\n  virtual void setPositionLimits(const Eigen::VectorXd &min_position,\n                                 const Eigen::VectorXd &max_position) = 0;\n\n  //! Set velocity magnitude limits.\n  //!\n  //! A fatal error is logged if `max_velocity` does not have a length matching the expected number\n  //! of c-space coordinates, or if any coordinate of `max_velocity` is negative.\n  //!\n  //! WARNING: The current implementation using a series of cubic splines requires velocity limits\n  //!          to be specified. This restriction may be lifted in future versions of the\n  //!          `CSpaceTrajectoryGenerator`.\n  virtual void setVelocityLimits(const Eigen::VectorXd &max_velocity) = 0;\n\n  //! Set acceleration magnitude limits.\n  //!\n  //! A fatal error is logged if `max_acceleration` does not have a length matching the expected\n  //! number of c-space coordinates, or if any coordinate of `max_acceleration` is negative.\n  virtual void setAccelerationLimits(const Eigen::VectorXd &max_acceleration) = 0;\n\n  //! Set jerk magnitude limits.\n  //!\n  //! A fatal error is logged if `max_jerk` does not have a length matching the expected\n  //! number of c-space coordinates, or if any coordinate of `max_jerk` is negative.\n  virtual void setJerkLimits(const Eigen::VectorXd &max_jerk) = 0;\n\n  //! Attempt to generate a time-optimal trajectory passing through the specified `waypoints` with\n  //! the specified constraints.\n  //!\n  //! If a trajectory cannot be generated, `nullptr` is returned.\n  [[nodiscard]] virtual std::unique_ptr<Trajectory> generateTrajectory(\n      const std::vector<Eigen::VectorXd> &waypoints) const = 0;\n\n  //! Interpolation modes used by `interpolateTrajectory()`.\n  enum class InterpolationMode {\n    //! Linear interpolation between c-space positions.\n    //!\n    //! The resulting trajectory will have continuous (but not smooth) position and discontinuous\n    //! segments of constant velocity. Acceleration and jerk will be returned as zero (including at\n    //! velocity discontinuities where values are technically undefined).\n    LINEAR,\n    //! Piecewise solution with a cubic spline between adjacent c-space positions.\n    //!\n    //! The resulting trajectory will have smooth position and velocity, continuous (but not smooth)\n    //! acceleration, and bounded (but discontinuous) jerk.\n    CUBIC_SPLINE\n  };\n\n  //! Attempt to interpolate a trajectory passing through the specified `waypoints` at the specified\n  //! `times`.\n  //!\n  //! Interpolation will fail if:\n  //! 1. The number of specified `positions` does not match the number of specified `times`;\n  //! 2. There is not at least one position specified for `LINEAR` interpolation mode or at least\n  //!    two positions specified for `CUBIC_SPLINE` interpolation mode;\n  //! 3. Not all `positions` have the same number of c-space coordinates;\n  //! 4. Not all `times` are strictly increasing; *OR*\n  //! 5. The interpolated trajectory does not satisfy specified limits on c-space position,\n  //!    velocity, acceleration, or jerk.\n  //!\n  //! If a trajectory cannot be generated due to invalid input (items 1-4 above), a fatal error is\n  //! logged. If failure is due to c-space limits (item 5 above), then `nullptr` is returned.\n  [[nodiscard]] virtual std::unique_ptr<Trajectory> generateTimeStampedTrajectory(\n      const std::vector<Eigen::VectorXd> &waypoints,\n      const std::vector<double> &times,\n      InterpolationMode interpolation_mode = InterpolationMode::CUBIC_SPLINE) const = 0;\n\n  //================================================================================================\n  // Interface for setting implementation-specific solver parameters.\n  // NOTE: Default values are likely to be sufficient for most use-cases.\n\n  //! Specify the value for a given parameter.\n  //!\n  //! The required `SolverParamValue` constructor for each parameter is detailed in the\n  //! documentation for `setSolverParam()`.\n  struct CUMO_EXPORT SolverParamValue {\n    //! Create `SolverParamValue` from `int`.\n    SolverParamValue(int value);  // NOLINT Allow implicit conversion\n    //! Create `SolverValue` from `double`.\n    SolverParamValue(double value);  // NOLINT Allow implicit conversion\n    //! Create `SolverParamValue` from `const char*`.\n    SolverParamValue(const char *value);  // NOLINT Allow implicit conversion\n    //! Create `SolverParamValue` from `std::string`.\n    SolverParamValue(const std::string &value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Set the value of the solver parameter.\n  //!\n  //! Currently, `CSpaceTrajectoryGenerator` uses a single solver implementation based on computing\n  //! a series of cubic splines. The following parameters can be set for this piecewise cubic spline\n  //! solver:\n  //!\n  //! `max_segment_iterations` [`int`]\n  //!   - The first step towards finding a time-optimal trajectory using a series of cubic\n  //!     splines is to iteratively compute an optimal span for each spline segment.\n  //!   - Setting a relatively high `max_segment_iterations` will, in general, result in shorter\n  //!     trajectory time spans, but will tend to take longer to converge.\n  //!   - This step can be skipped completely by setting `max_segment_iterations` to zero.\n  //!   - `max_segment_iterations` must be non-negative. Additionally at least one of\n  //!     `max_segment_iterations` and `max_aggregate_iterations` must be non-zero.\n  //!   - Default value is 5.\n  //!\n  //! `max_aggregate_iterations` [`int`]\n  //!   - The second step towards finding a time-optimal trajectory using a series of cubic splines\n  //!     is to iteratively compute an optimal span for the entire trajectory.\n  //!   - Relying more heavily on this second step (i.e., setting `max_segment_iterations` to a\n  //!     a relatively low value) will, in general, require less iterations (and thus less time) to\n  //!     converge, but the generated trajectories will tend to have longer time spans.\n  //!   - This step can be skipped completely by setting `max_aggregate_iterations` to zero.\n  //!   - `max_aggregate_iterations` must be non-negative. Additionally at least one of\n  //!     `max_segment_iterations` and `max_aggregate_iterations` must be non-zero.\n  //!   - Default value is 50.\n  //!\n  //! `convergence_dt` [`double`]\n  //!   - The search for optimal time values will terminate if the maximum change to any time value\n  //!     during a given iteration is less than the `convergence_dt`.\n  //!   - `convergence_dt` must be positive.\n  //!   - Default value is 0.001.\n  //!\n  //! `max_dilation_iterations` [`int`]\n  //!   - After the segment-wise and/or aggregate time-optimal search has converged or reached\n  //!     maximum iterations, the resulting set of splines will be tested to see if any derivative\n  //!     limits are exceeded.\n  //!   - If any derivative limits are exceeded, the splines will be iteratively scaled in time to\n  //!     reduce the maximum achieved derivative. This process will repeat until no derivative\n  //!     limits are exceeded (success) or `max_dilation_iterations` are reached (failure).\n  //!   - `max_dilation_iterations` must be non-negative.\n  //!   - Default value is 100.\n  //!\n  //! `dilation_dt` [`double`]\n  //!   - For the iterative dilation step described in `max_dilation_iterations` documentation, the\n  //!     `dilation_dt` is the \"epsilon\" value added to the span of the trajectory that exceeds\n  //!     derivative limits.\n  //!   - `dilation_dt` must be positive.\n  //!   - Default value is 0.001.\n  //!\n  //! `min_time_span` [`double`]\n  //!   - Specify the minimum allowable time span between adjacent waypoints/endpoints.\n  //!   - `min_time_span` must be positive.\n  //!   - Default value is 0.01.\n  //!\n  //! `time_split_method` [`string`]\n  //!   - Specify the `TimeSplitMethod` for the initial distribution of time values that will be\n  //!     used to iteratively search for time-optimal values.\n  //!   - Valid settings are `uniform`, `chord_length` and `centripetal`.\n  [[nodiscard]]\n  virtual bool setSolverParam(const std::string &param_name, SolverParamValue value) = 0;\n};\n\n//! Create a `CSpaceTrajectoryGenerator` with the specified number of configuration space\n//! coordinates.\n//!\n//! Position and derivative limits, bound constraints, and intermediate waypoints can be added\n//! after construction.\nCUMO_EXPORT std::unique_ptr<CSpaceTrajectoryGenerator> CreateCSpaceTrajectoryGenerator(\n    int num_cspace_coords);\n\n//! Create a `CSpaceTrajectoryGenerator` with the specified `kinematics`.\n//!\n//! The `kinematics` will be used to specify the number of configuration space coordinates, position\n//! limits, and any available derivative limits.\n//!\n//! Position and derivative limits may be added or overwritten after construction. Bound\n//! constraints and intermediate waypoints can be added after construction.\nCUMO_EXPORT std::unique_ptr<CSpaceTrajectoryGenerator> CreateCSpaceTrajectoryGenerator(\n    const Kinematics &kinematics);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/trajectory_optimizer.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <filesystem>\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/rotation3.h\"\n#include \"cumotion/trajectory.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Configuration parameters for a `TrajectoryOptimizer`.\nclass CUMO_EXPORT TrajectoryOptimizerConfig {\n public:\n  virtual ~TrajectoryOptimizerConfig() = default;\n\n  //! Specify the value for a given parameter.\n  //!\n  //! The required `ParamValue` constructor for each param is detailed in the\n  //! documentation for `setParam()`.\n  struct CUMO_EXPORT ParamValue {\n    //! Create `ParamValue` from `int`.\n    ParamValue(int value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `double`.\n    ParamValue(double value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `bool`.\n    ParamValue(bool value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `std::vector<double>`.\n    ParamValue(const std::vector<double> &value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Set the value of the parameter.\n  //!\n  //! `setParam()` returns `true` if the parameter has been successfully updated and `false` if an\n  //! error has occurred (e.g., invalid parameter).\n  //!\n  //! The following parameters can be set for `TrajectoryOptimizer`:\n  //!\n  //! `enable_self_collision` [`bool`]\n  //!   - Enable or disable self-collision avoidance between robot spheres during optimization.\n  //!   - When `true`, both inverse kinematics (IK) and trajectory optimization will include\n  //!     collision costs to prevent robot spheres from colliding with each other.\n  //!   - When `false`, self-collision costs are disabled, which may improve solve time but allows\n  //!     self-intersecting trajectories.\n  //!   - Default value is `true` (self-collision avoidance enabled).\n  //!\n  //! `enable_world_collision` [`bool`]\n  //!   - Enable or disable collision avoidance with world obstacles during optimization.\n  //!   - When `true`, both IK and trajectory optimization will include collision costs to avoid\n  //!     obstacles in the environment.\n  //!   - When `false`, world collision costs are disabled, which may improve performance but allows\n  //!     trajectories that intersect with obstacles.\n  //!   - Default value is `true` (world collision avoidance enabled).\n  //!\n  //! `task_space_terminal_position_tolerance` [`double`]\n  //!   - Maximum allowable violation of the user-specified constraint on the position of the tool\n  //!     frame.\n  //!   - Used both for IK and trajectory optimization problems.\n  //!     It determines when a solution is considered to have satisfied position targets.\n  //!     Smaller values require more precise solutions but may be harder to achieve.\n  //!   - Units are in meters.\n  //!   - A default value of 1e-3 (1mm) is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `task_space_terminal_orientation_tolerance` [`double`]\n  //!   - Maximum allowable violation of the user-specified constraint on the orientation of the\n  //!     tool frame.\n  //!   - Used both for IK and trajectory optimization problems.\n  //!     It determines when a solution is considered to have satisfied orientation targets.\n  //!     Smaller values require more precise solutions but may be harder to achieve.\n  //!   - Units are in radians.\n  //!   - A default value of 5e-3 (approximately 0.29 degrees) is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `ik/num_seeds` [`int`]\n  //!   - Number of seeds used to solve the inverse kinematics (IK) problem.\n  //!   - The trajectory optimizer generates multiple pseudo-random c-space configurations and\n  //!     optimizes them to find diverse collision-free configurations for the desired tool pose.\n  //!     Higher values increase the likelihood of finding valid solutions but increase\n  //!     computational cost.\n  //!   - A default value of 32 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `ik/max_reattempts` [`int`]\n  //!   - Maximum number of times to restart the IK problem with different random seeds, in case of\n  //!     failure, before giving up.\n  //!   - Higher values increase the likelihood of finding valid IK solutions but increase\n  //!     memory usage and the maximum possible time to find a solution. A value of 0 means no\n  //!     retries (i.e. only perform the initial attempt).\n  //!   - A default value of 10 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/desired_cspace_position_weight` [`double`]\n  //!   - Weight used on the distance to a desired c-space configuration when ranking IK solutions.\n  //!   - When this weight is zero, the valid IK solutions are ranked purely by their tool\n  //!     constraint violations.\n  //!   - A default value of 1.0 provides balanced behavior for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `ik/pbo/cost/tool_frame_position_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space position error violations at tool-frame constraints\n  //!     during PBO IK optimization.\n  //!   - Higher values more strongly enforce exact position matching at goal targets.\n  //!   - A default value of `10000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/tool_frame_position_error_penalty/activation_distance` [`double`]\n  //!   - Distance from the deviation limit at which task-space position error penalties activate.\n  //!     This value is subtracted from the deviation limit specified when creating the position\n  //!     constraint (e.g., `TranslationConstraint::Target()`) during PBO IK\n  //!     optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero position errors will be penalized, encouraging solutions that have as little\n  //!     error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final error of `deviation_limit - activation_distance`, instead of an error\n  //!     near zero.\n  //!   - Units are in meters.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/tool_frame_orientation_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space orientation error violations at tool-frame\n  //!     constraints during PBO IK optimization.\n  //!   - Higher values more strongly enforce exact orientation matching at goal targets.\n  //!   - A default value of `5000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/tool_frame_orientation_error_penalty/activation_distance` [`double`]\n  //!   - Angular distance from the deviation limit at which task-space orientation error penalties\n  //!     activate. This value is subtracted from the deviation limit specified when creating the\n  //!     orientation constraint (e.g., `OrientationConstraint::TerminalTarget()`) during\n  //!     PBO IK optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero orientation errors will be penalized, encouraging solutions that have as little\n  //!     angular error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final angular error of `deviation_limit - activation_distance`, instead of an\n  //!     error near zero.\n  //!   - Units are in radians.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/cspace_position_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied when IK solutions violate c-space position limits, minus the\n  //!     activation distance, during Particle-Based Optimizer (PBO) IK optimization.\n  //!   - Higher values more strongly discourage solutions near joint limits.\n  //!   - A default value of 5000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/cspace_position_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from c-space position limits at which the position limit penalty\n  //!     activates, during PBO IK optimization.\n  //!   - Units correspond to rad for revolute joints, m for prismatic joints.\n  //!   - A default value of 0.05 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/enable_cspace_position_limit` [`bool`]\n  //!   - Enable or disable c-space position limit penalties during PBO IK optimization.\n  //!   - When `true`, PBO IK will penalize solutions that violate or approach c-space\n  //!     position limits.\n  //!   - When `false`, position limit costs are ignored (not recommended).\n  //!   - Default value is `true` (position limit penalties enabled).\n  //!\n  //! `ik/pbo/cost/self_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in self-colliding robot configurations during PBO IK optimization.\n  //!   - Higher values more strongly discourage self-colliding configurations.\n  //!   - This parameter is only used when `enable_self_collision` is `true`.\n  //!   - A default value of 500.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/self_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the self-collision penalty activates during PBO IK\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     each other.\n  //!   - Units are in meters.\n  //!   - A default value of 0.01 m (1 cm) is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/world_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in world-colliding robot configurations during PBO IK optimization.\n  //!   - Higher values more strongly discourage configurations that collide with obstacles.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 500.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/world_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the world collision penalty activates during PBO IK\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     obstacles.\n  //!   - Units are in meters.\n  //!   - A default value of 0.035 m is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/tool_frame_position_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space position error violations at tool-frame constraints\n  //!     during L-BFGS IK optimization.\n  //!   - Higher values more strongly enforce exact position matching at goal targets.\n  //!   - A default value of `10000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/tool_frame_position_error_penalty/activation_distance` [`double`]\n  //!   - Distance from the deviation limit at which task-space position error penalties activate.\n  //!     This value is subtracted from the deviation limit specified when creating the position\n  //!     constraint (e.g., `TranslationConstraint::Target()`) during L-BFGS IK\n  //!     optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero position errors will be penalized, encouraging solutions that have as little\n  //!     error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final error of `deviation_limit - activation_distance`, instead of an error\n  //!     near zero.\n  //!   - Units are in meters.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/tool_frame_orientation_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space orientation error violations at tool-frame\n  //!     constraints during L-BFGS IK optimization.\n  //!   - Higher values more strongly enforce exact orientation matching at goal targets.\n  //!   - A default value of `5000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/tool_frame_orientation_error_penalty/activation_distance` [`double`]\n  //!   - Angular distance from the deviation limit at which task-space orientation error penalties\n  //!     activate. This value is subtracted from the deviation limit specified when creating the\n  //!     orientation constraint (e.g., ``OrientationConstraint::TerminalTarget()`) during\n  //!     L-BFGS IK optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero orientation errors will be penalized, encouraging solutions that have as little\n  //!     angular error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final angular error of `deviation_limit - activation_distance`, instead of an\n  //!     error near zero.\n  //!   - Units are in radians.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/cspace_position_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied when IK solutions violate c-space position limits, minus the\n  //!     activation distance, during L-BFGS IK optimization.\n  //!   - Higher values more strongly discourage solutions near joint limits.\n  //!   - A default value of 100.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/cspace_position_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from c-space position limits at which the position limit penalty\n  //!     activates, during L-BFGS IK optimization.\n  //!   - Units correspond to rad for revolute joints, m for prismatic joints.\n  //!   - A default value equivalent to 5 degrees is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/enable_cspace_position_limit` [`bool`]\n  //!   - Enable or disable c-space position limit penalties during L-BFGS IK optimization.\n  //!   - When `true`, L-BFGS IK will penalize solutions that violate or approach c-space\n  //!     position limits.\n  //!   - When `false`, position limit costs are ignored (not recommended).\n  //!   - Default value is `true` (position limit penalties enabled).\n  //!\n  //! `ik/lbfgs/cost/self_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in self-colliding robot configurations during L-BFGS IK\n  //!     optimization.\n  //!   - Higher values more strongly discourage self-colliding configurations.\n  //!   - This parameter is only used when `enable_self_collision` is `true`.\n  //!   - A default value of 1000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/self_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the self-collision penalty activates during L-BFGS IK\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     each other.\n  //!   - Units are in meters.\n  //!   - A default value of 0.01 m (1 cm) is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/world_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in world-colliding robot configurations during L-BFGS IK\n  //!     optimization.\n  //!   - Higher values more strongly discourage configurations that collide with obstacles.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 1000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/world_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the world collision penalty activates during L-BFGS IK\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     obstacles.\n  //!   - Units are in meters.\n  //!   - A default value of 0.001 m (1 mm) is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/max_iterations` [`int`]\n  //!   - Maximum number of L-BFGS iterations for the inverse kinematics solver.\n  //!   - Higher values allow the IK solver to converge more precisely but increase computational\n  //!     cost per IK attempt.\n  //!   - A default value of 100 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `trajopt/num_seeds` [`int`]\n  //!   - Number of seeds used to solve the trajectory optimization problem.\n  //!   - The trajectory optimizer generates interpolated trajectories between the initial\n  //!     configuration and each of the IK solutions, and then optimizes each trajectory.\n  //!     Higher values provide more trajectory candidates but increase computational cost.\n  //!   - A default value of 4 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `trajopt/num_knots_per_trajectory` [`int`]\n  //!   - Number of knot points per trajectory used for trajectory optimization.\n  //!   - This parameter controls the temporal resolution of the optimized trajectory, where higher\n  //!     values provide finer temporal resolution but increase computational cost.\n  //!   - A default value of 32 is recommended for most use-cases.\n  //!   - Must be greater than or equal to 4.\n  //!\n  //! `trajopt/pbo/enabled` [`bool`]\n  //!   - Enable or disable Particle-Based Optimizer (PBO) for the trajectory optimization problem.\n  //!   - PBO is an algorithm that uses a set of \"particles\" to explore a non-linear design space.\n  //!     If enabled, PBO is used to find collision-free trajectories before starting the\n  //!     gradient-based L-BFGS optimization algorithm.  PBO is helpful for complex scenarios where\n  //!     a linearly-interpolated path between the initial configuration and the IK solutions is not\n  //!     sufficient to find a collision-free trajectory.\n  //!   - When set to `false`, PBO is disabled, and L-BFGS is initialized with a\n  //!     linearly-interpolated path.\n  //!   - When set to `true`, PBO is enabled and the `trajopt/pbo` parameters control its behavior.\n  //!   - Default value is `true` (PBO enabled).\n  //!\n  //! `trajopt/pbo/num_particles_per_problem` [`int`]\n  //!   - Number of particles used by PBO in the trajectory optimization problem, when enabled.\n  //!   - This parameter controls the population size of particles in the PBO algorithm. Higher\n  //!     values increase the likelihood of finding better solutions but increase solve time and\n  //!     memory usage.\n  //!   - This parameter is only used when `trajopt/pbo/enabled` is set to `true`.\n  //!   - A default value of 25 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `trajopt/pbo/num_iterations` [`int`]\n  //!   - Number of iterations to run PBO in the trajectory optimization problem, when enabled.\n  //!   - This parameter controls how many optimization iterations PBO performs.\n  //!     Higher values allow for more thorough optimization but increase computational cost.\n  //!   - This parameter is only used when `trajopt/pbo/enabled` is set to `true`.\n  //!   - A default value of 5 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `trajopt/pbo/cost/path_position_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space position error violations at tool-frame path\n  //!     constraints during PBO trajectory optimization for non-terminal knot points.\n  //!   - Higher values more strongly enforce exact position matching at goal targets.\n  //!   - A default value of `600.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/path_position_error_penalty/activation_distance` [`double`]\n  //!   - Distance from the deviation limit at which task-space path position error penalties\n  //!     activate.  This value is subtracted from the deviation limit specified when creating the\n  //!     path position constraint (e.g., `TranslationConstraint::LinearPathConstraint()`) during\n  //!     PBO trajectory optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero position errors will be penalized, encouraging solutions that have as little\n  //!     error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final error of `deviation_limit - activation_distance`, instead of an error\n  //!     near zero.\n  //!   - Units are in meters.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/path_orientation_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space path orientation error violations at tool-frame path\n  //!     constraints during PBO trajectory optimization.\n  //!   - Higher values more strongly enforce exact orientation matching at goal targets.\n  //!   - A default value of `100.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/path_orientation_error_penalty/activation_distance` [`double`]\n  //!   - Angular distance from the deviation limit at which task-space path orientation error\n  //!     penalties activate. This value is subtracted from the deviation limit specified when\n  //!     creating the path orientation constraint (e.g.,\n  //!     `OrientationConstraint::TerminalAndPathTarget()`) during PBO trajectory optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero orientation errors will be penalized, encouraging solutions that have as little\n  //!     angular error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final angular error of `deviation_limit - activation_distance`, instead of an\n  //!     error near zero.\n  //!   - Units are in radians.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_position_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space position limit violations during PBO trajectory\n  //!     optimization.\n  //!   - It discourages trajectories that violate joint limits.\n  //!   - A default value of 10.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_position_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from c-space position limits for PBO trajectory optimization penalties.\n  //!   - Units correspond to rad/s for revolute joints, m/s for prismatic joints.\n  //!   - A default value of 0.0 (no activation distance) is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_velocity_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space velocity limit violations during PBO trajectory\n  //!     optimization.\n  //!   - It discourages trajectories with velocities that exceed joint limits.\n  //!   - A default value of 0.0 (disabled) is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_velocity_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from velocity limits for PBO trajectory optimization penalties.\n  //!   - Units correspond to rad/s for revolute joints, m/s for prismatic joints.\n  //!   - A default value of 0.0 is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_acceleration_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space acceleration limit violations during PBO trajectory\n  //!     optimization.\n  //!   - It discourages trajectories with accelerations that exceed joint limits.\n  //!   - A default value of 0.0 (disabled) is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_acceleration_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from acceleration limits for PBO trajectory optimization penalties.\n  //!   - Units correspond to rad/s^2 for revolute joints, m/s^2 for prismatic joints.\n  //!   - A default value of 0.0 is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_jerk_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space jerk limit violations during PBO trajectory\n  //!     optimization.\n  //!   - It discourages trajectories with jerks that exceed joint limits.\n  //!   - A default value of 0.0 (disabled) is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_jerk_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from jerk limits for PBO trajectory optimization penalties.\n  //!   - Units correspond to rad/s^3 for revolute joints, m/s^3 for prismatic joints.\n  //!   - A default value of 0.0 is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_acceleration_smoothing_cost` [`double`]\n  //!   - Weight used to encourage smooth trajectories during PBO trajectory optimization by\n  //!     penalizing c-space acceleration.\n  //!   - Higher values encourage smoother trajectories by penalizing non-zero accelerations.\n  //!   - A default value of 10.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_jerk_smoothing_cost` [`double`]\n  //!   - Weight applied to minimize c-space jerk (smoothness regularization) during PBO trajectory\n  //!     optimization.\n  //!   - Higher values encourage smoother trajectories by penalizing non-zero jerk values.\n  //!   - A default value of 0.0 (disabled) is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/self_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in self-colliding robot configurations during PBO trajectory\n  //!     optimization.\n  //!   - This parameter is only used when `enable_self_collision` is `true`.\n  //!   - A default value of 100000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/self_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the self-collision penalty activates during PBO trajectory\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     each other.\n  //!   - Units are in meters.\n  //!   - A default value of 0.01 m (1 cm) is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/world_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in world-colliding robot configurations during PBO trajectory\n  //!     optimization.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 5000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/world_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the world collision penalty activates during PBO trajectory\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     obstacles.\n  //!   - Units are in meters.\n  //!   - A default value of 0.025 m is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/world_collision_max_sweep_steps` [`int`]\n  //!   - Maximum number of discrete sampling steps when \"sweeping\" collision spheres between\n  //!     trajectory knot points during PBO optimization. \"Sweeping\" samples sphere positions at\n  //!     intervals along the path between knot points to detect collisions that might be missed\n  //!     if only checking at the knot points themselves.\n  //!   - Higher values provide more accurate collision detection but increase computational cost.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 5 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/path_position_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space position error violations at tool-frame path\n  //!     constraints during L-BFGS trajectory optimization for non-terminal knot points.\n  //!   - Higher values more strongly enforce exact position matching at goal targets.\n  //!   - A default value of `50000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/path_position_error_penalty/activation_distance` [`double`]\n  //!   - Distance from the deviation limit at which task-space path position error penalties\n  //!     activate.  This value is subtracted from the deviation limit specified when creating the\n  //!     path position constraint (e.g., `TranslationConstraint::LinearPathConstraint()`) during\n  //!     L-BFGS trajectory optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero position errors will be penalized, encouraging solutions that have as little\n  //!     error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final error of `deviation_limit - activation_distance`, instead of an error\n  //!     near zero.\n  //!   - Units are in meters.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/path_orientation_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space path orientation error violations at tool-frame path\n  //!     constraints during L-BFGS trajectory optimization.\n  //!   - Higher values more strongly enforce exact orientation matching at goal targets.\n  //!   - A default value of `10000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/path_orientation_error_penalty/activation_distance` [`double`]\n  //!   - Angular distance from the deviation limit at which task-space path orientation error\n  //!     penalties activate. This value is subtracted from the deviation limit specified when\n  //!     creating the path orientation constraint (e.g.,\n  //!     `OrientationConstraint::TerminalAndPathTarget()`) during L-BFGS trajectory optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero orientation errors will be penalized, encouraging solutions that have as little\n  //!     angular error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final angular error of `deviation_limit - activation_distance`, instead of an\n  //!     error near zero.\n  //!   - Units are in radians.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_position_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space position limit violations during L-BFGS trajectory\n  //!     optimization.\n  //!   - Higher values more strongly discourage trajectories that violate joint limits.\n  //!   - A default value of 50000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_position_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from c-space position limits for L-BFGS trajectory optimization\n  //!     penalties.\n  //!   - Units correspond to rad/s for revolute joints, m/s for prismatic joints.\n  //!   - A default value equivalent to 5 degrees is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_velocity_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space velocity limit violations during L-BFGS trajectory\n  //!     optimization.\n  //!   - Higher values more strongly discourage trajectories with excessive joint velocities.\n  //!   - A default value of 1000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_velocity_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from velocity limits for L-BFGS trajectory optimization penalties.\n  //!   - Units correspond to rad/s for revolute joints and m/s for prismatic joints.\n  //!   - A default value equivalent to 5 degrees/s is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_acceleration_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space acceleration limit violations during L-BFGS trajectory\n  //!     optimization.\n  //!   - Higher values more strongly discourage trajectories with excessive joint accelerations.\n  //!   - A default value of 1000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_acceleration_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from acceleration limits for L-BFGS trajectory optimization penalties.\n  //!   - Units correspond to rad/s^2 for revolute joints and m/s^2 for prismatic joints.\n  //!   - A default value equivalent to 5 degrees/s^2 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_jerk_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space jerk limit violations during L-BFGS trajectory\n  //!     optimization.\n  //!   - Higher values more strongly discourage trajectories with excessive joint jerk.\n  //!   - A default value of 10.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_jerk_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from jerk limits for L-BFGS trajectory optimization penalties.\n  //!   - Units correspond to rad/s^3 for revolute joints and m/s^3 for prismatic joints.\n  //!   - A default value equivalent to 5 degrees/s^3 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_acceleration_smoothing_cost` [`double`]\n  //!   - Weight applied to minimize c-space acceleration (smoothness regularization) during L-BFGS\n  //!     trajectory optimization.\n  //!   - Higher values encourage smoother trajectories by penalizing large accelerations.\n  //!   - A default value of 10.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_jerk_smoothing_cost` [`double`]\n  //!   - Weight applied to minimize c-space jerk (smoothness regularization) during L-BFGS\n  //!     trajectory optimization.\n  //!   - Higher values encourage smoother trajectories by penalizing large jerk values.\n  //!   - A default value of 0.01 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/self_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in self-colliding robot configurations during L-BFGS trajectory\n  //!     optimization.\n  //!   - This parameter is only used when `enable_self_collision` is `true`.\n  //!   - A default value of 100000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/self_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the self-collision penalty activates during L-BFGS trajectory\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     each other.\n  //!   - Units are in meters.\n  //!   - A default value of 0.01 m (1 cm) is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/world_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in world-colliding robot configurations during L-BFGS trajectory\n  //!     optimization.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 1000000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/world_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the world-collision penalty activates during L-BFGS trajectory\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     obstacles.\n  //!   - Units are in meters.\n  //!   - A default value of 0.015 m is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/world_collision_max_sweep_steps` [`int`]\n  //!   - Maximum number of discrete sampling steps when \"sweeping\" collision spheres between\n  //!     trajectory knot points during L-BFGS optimization. \"Sweeping\" samples sphere positions at\n  //!     intervals along the path between knot points to detect collisions that might be missed\n  //!     if only checking at the knot points themselves.\n  //!   - Higher values provide more accurate collision detection but increase computational cost.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 5 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/history_length` [`int`]\n  //!   - Number of past iterations stored by the L-BFGS algorithm to approximate the inverse\n  //!     Hessian during trajectory optimization.\n  //!   - Larger values may improve convergence quality but increase memory usage and per-iteration\n  //!     computational cost.\n  //!   - A default value of 5 is recommended for most use-cases.\n  //!   - Must be less than or equal to the number of optimization variables in the trajectory\n  //!     optimization problem, calculated as\n  //!     `RobotDescription::numCSpaceCoords() * ('trajopt/num_knots_per_trajectory' - 2)`.\n  //!     This value is automatically clamped to not exceed the number of optimization variables, if\n  //!     setting any other parameter (e.g., 'trajopt/num_knots_per_trajectory') reduces the upper\n  //!     bound.\n  //!   - Must be positive.\n  //!\n  //! `trajopt/lbfgs/initial_iterations` [`int`]\n  //! - Number of L-BFGS iterations used to solve the initial trajectory optimization problem.\n  //! - A smaller number of iterations will result in a more coarse solution used when retiming\n  //!   the trajectory.\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - A value of 0 will result in the initial retiming attempt using the output of the\n  //!   particle-based optimizer.\n  //! - Default value: 300\n  //! - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/retiming_initial_iterations` [`int`]\n  //! - Number of L-BFGS iterations used to solve the 2nd trajectory optimization problem, after\n  //!   the initial solutions are retimed using `trajopt/timestep_scaling_factor` to minimize the\n  //!   trajectory time by saturating the kinematic limits.\n  //! - A smaller number of iterations will result in a more coarse solution used when retiming\n  //!   solution used when retiming the trajectory.\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - Default value: 400\n  //! - Must be positive.\n  //!\n  //! `trajopt/lbfgs/retiming_iterations` [`int`]\n  //! - Number of L-BFGS iterations used to solve the trajectory optimization problem after the\n  //!   initial solve and first retiming attempt, after the previous solution is scaled by\n  //!   `trajopt/timestep_scaling_factor` to minimize trajectory time by saturating the\n  //!   kinematic limits.\n  //! - A smaller number of iterations will result in a more coarse final solution, including the\n  //!   possibility of failing to find a valid solution where one would have been found with a\n  //!   greater number of iterations.\n  //! - A larger number of iterations will require more compute time, but may provide a\n  //!   higher-quality final trajectory (typically meaning smoother kinematic derivatives).\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - Default value: 100\n  //! - Must be positive.\n  //!\n  //! `trajopt/num_geometric_planning_attempts` [`int`]\n  //!  - Number of attempts that will be made to use a geometric planner to generate a feasible\n  //!    path to the target as a fallback when standard trajectory optimization fails.  Geometric\n  //!    planning is, in general, computationally expensive relative to trajectory optimization, but\n  //!    it can be necessary in complex environments.\n  //!  - A default value of 4 is used.\n  //!  - Must be non-negative.\n  //!\n  //! `trajopt/geometric_planning/max_iterations` [`int`]\n  //!   - Maximum number of iterations for the geometric planner per attempt.\n  //!   - Higher values increase the likelihood of finding a solution in complex environments but\n  //!     increase the maximum runtime of each geometric planning attempt.\n  //!   - Default value: 10000\n  //!   - Must be positive.\n  //!\n  //! `trajopt/initial_timestep_scaling_factor` [`double`]\n  //! - Scales the saturating timestep calculated on the initial trajectory seeds.\n  //! - Smaller values result in shorter initial trajectories where the derivative limits are\n  //!   violated for a greater percentage of the trajectory.\n  //! - A value of 1.0 or more will produce an initial trajectory that does not violate any\n  //!   kinematic derivatives along the trajectory.\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - Should typically be less than 1.0, since values greater than 1.0 will undersaturate the\n  //!   derivative limits, leading to longer trajectory times.\n  //! - Default value: 0.85\n  //! - Must be positive.\n  //!\n  //! `trajopt/timestep_scaling_factor` [`double`]\n  //! - Scales the saturating timestep calculated on the retimed trajectory seeds (after the initial\n  //!   solve).\n  //! - Smaller values result in shorter retimed trajectories where the derivative limits are\n  //!   violated for a greater percentage of the trajectory.\n  //! - A value of 1.0 or more will produce a retimed trajectory that does not violate any\n  //!   kinematic derivatives along the trajectory.\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - Should typically be less than 1.0, since values greater than 1.0 will undersaturate the\n  //!   derivative limits, leading to longer trajectory times.\n  //! - Default value: 0.85\n  //! - Must be positive.\n  //!\n  //! `trajopt/num_retiming_iterations` [`int`]\n  //! - Number of times the trajectory is retimed after the initial solve.\n  //! - A smaller number of iterations may result in a shorter planning time and a longer final\n  //!   trajectory time. A higher number of iterations may result in a shorter final trajectory\n  //!   time.\n  //! - The optimizer may exit early if no trajectories are valid after retiming.\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - A value of 0 will disable retiming and the trajectory will use the timestep calculated\n  //!   by saturating the kinematic limits of the initial solution.\n  //! - Default value: 5\n  //! - Must be non-negative.\n  [[nodiscard]] virtual bool setParam(const std::string &param_name, ParamValue value) = 0;\n};\n\n//! Load a set of `TrajectoryOptimizer` configuration parameters from file.\n//!\n//! These parameters are combined with `robot_description`, `tool_frame_name`, and `world_view`\n//! to create a configuration for trajectory optimization.\n//!\n//! Default values will be used for any parameters not specified in\n//! `trajectory_optimizer_config_file`.\n//!\n//! A configuration will *NOT* be created if:\n//!   1. `trajectory_optimizer_config_file` path is invalid or empty,\n//!   2. `trajectory_optimizer_config_file` points to an invalid YAML file,\n//!   3. `trajectory_optimizer_config_file` contains invalid contents (e.g., parameters, version),\n//!   4. `robot_description` is invalid, *OR*\n//!   5. `tool_frame_name` is not a valid frame in `robot_description`.\n//!\n//! In the case of failure, a non-fatal error will be logged and a `nullptr` will be returned.\n[[nodiscard]] CUMO_EXPORT std::unique_ptr<TrajectoryOptimizerConfig>\nCreateTrajectoryOptimizerConfigFromFile(\n    const std::filesystem::path &trajectory_optimizer_config_file,\n    const RobotDescription &robot_description,\n    const std::string &tool_frame_name,\n    const WorldViewHandle &world_view);\n\n//! Use default parameters to create a configuration for trajectory optimization.\n//!\n//! These default parameters are combined with `robot_description`, `tool_frame_name`, and\n//! `world_view`.\n//!\n//! A configuration will *NOT* be created if:\n//!   1. `robot_description` is invalid, *OR*\n//!   2. `tool_frame_name` is not a valid frame in `robot_description`.\n//!\n//! In the case of failure, a non-fatal error will be logged and a `nullptr` will be returned.\n[[nodiscard]] CUMO_EXPORT std::unique_ptr<TrajectoryOptimizerConfig>\nCreateDefaultTrajectoryOptimizerConfig(const RobotDescription &robot_description,\n                                       const std::string &tool_frame_name,\n                                       const WorldViewHandle &world_view);\n\n//! Interface for using numerical optimization to generate collision-free trajectories for a robot.\n//!\n//! \\rst\n//! See documentation for corresponding :py:class:`Python class <cumotion.TrajectoryOptimizer>`.\n//! \\endrst\nclass CUMO_EXPORT TrajectoryOptimizer {\n public:\n  virtual ~TrajectoryOptimizer() = default;\n\n  //! Translation constraints restrict the position of the origin of a tool frame.\n  //!\n  //! These constraints are always active at the terminal point of the trajectory; partial\n  //! constraints may be active along the path.\n  class CUMO_EXPORT TranslationConstraint {\n   public:\n    //! Create a `TranslationConstraint` such that a `translation_target` is specified at\n    //! termination, but no translation constraints are active along the path.\n    //!\n    //! The optional parameter `terminal_deviation_limit` can be used to allow deviation from the\n    //! `translation_target` at termination. This limit specifies the maximum allowable deviation\n    //! from the desired position.\n    //!\n    //! If `terminal_deviation_limit` is not input, then the deviation limit is assumed to be zero\n    //! (i.e., it is desired that terminal translation be exactly `translation_target`).\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `terminal_deviation_limit` is negative.\n    //!\n    //! NOTE: The `translation_target` is specified in world frame coordinates (i.e., relative to\n    //! the base of the robot).\n    static TranslationConstraint Target(const Eigen::Vector3d &translation_target,\n                                        const double *terminal_deviation_limit = nullptr);\n\n    //! Create a `TranslationConstraint` such that a `translation_target` is specified at\n    //! termination *AND* a linear translation constraint is active along the path.\n    //!\n    //! The linear path constraint is defined by the line **segment** between `translation_target`\n    //! and the origin of the tool frame at the initial c-space position. This path constraint is\n    //! satisfied if the origin of the tool frame is on this line segment between the initial and\n    //! final tool positions at all points in the trajectory. It is considered a constraint\n    //! violation if the origin of the tool frame \"overshoots\" either the initial or final\n    //! positions.\n    //!\n    //! If the distance between the initial and final tool frames are nearly the same, the path\n    //! constraint becomes a constraint on the distance from the initial and final positions in any\n    //! direction.\n    //!\n    //! The optional parameters `path_deviation_limit` and `terminal_deviation_limit` can be used\n    //! to allow deviations from the specified line along the path and at the end of the path,\n    //! respectively.\n    //!\n    //! If both limits are input, the `path_deviation_limit` must be greater than or equal to\n    //! (i.e., no more restrictive than) the `terminal_deviation_limit`.\n    //!\n    //! If neither is input, then both deviation limits are assumed to be zero, indicating that\n    //! translation should always be exactly coincident with the specified line.\n    //!\n    //! It is valid to input a `path_deviation_limit` without a `terminal_deviation_limit`\n    //! (implicitly setting the latter to zero) but not to input a `terminal_deviation_limit`\n    //! without a `path_deviation_limit`. This ensures that the terminal constraint is more\n    //! restrictive than the corresponding path constraint.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `path_deviation_limit` is negative,\n    //!   2. `terminal_deviation_limit` is negative,\n    //!   3. both `path_deviation_limit` and `terminal_deviation_limit` are defined, but\n    //!      `path_deviation_limit` < `terminal_deviation_limit`, *OR*\n    //!   4. `terminal_deviation_limit` is defined without defining a `path_deviation_limit`.\n    //!\n    //! NOTE: The `translation_target` is specified in world frame coordinates (i.e., relative to\n    //! the base of the robot).\n    static TranslationConstraint LinearPathConstraint(\n        const Eigen::Vector3d &translation_target,\n        const double *path_deviation_limit = nullptr,\n        const double *terminal_deviation_limit = nullptr);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `TranslationConstraint` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of `TranslationConstraint`s are considered concurrently. Each\n  //! `TranslationConstraint` in the goalset must have the same mode (e.g., \"terminal target\n  //! with linear path constraint\") but may have different data for each `TranslationConstraint`.\n  class CUMO_EXPORT TranslationConstraintGoalset {\n   public:\n    //! Create a `TranslationConstraintGoalset` such that `translation_targets` are specified at\n    //! termination, but no translation constraints are active along the path.\n    //!\n    //! See `TranslationConstraint::Target()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `TranslationConstraint::Target()` is not met, *OR*\n    //!   2. `translation_targets` is empty.\n    static TranslationConstraintGoalset Target(\n        const std::vector<Eigen::Vector3d> &translation_targets,\n        const double *terminal_deviation_limit = nullptr);\n\n    //! Create a `TranslationConstraintGoalset` such that `translation_targets` are specified at\n    //! termination *AND* linear translation constraints are active along the path.\n    //!\n    //! See `TranslationConstraint::LinearPathConstraint()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `TranslationConstraint::LinearPathConstraint()` is not met, *OR*\n    //!   2. `translation_targets` is empty.\n    static TranslationConstraintGoalset LinearPathConstraint(\n        const std::vector<Eigen::Vector3d> &translation_targets,\n        const double *path_deviation_limit = nullptr,\n        const double *terminal_deviation_limit = nullptr);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Orientation constraints restrict the orientation of a tool frame.\n  //!\n  //! These constraints may be active at the terminal point of the trajectory and/or along the path.\n  //! Each constraint may fully or partially constrain the orientation.\n  class CUMO_EXPORT OrientationConstraint {\n   public:\n    //! Create an `OrientationConstraint` such that no tool frame orientation constraints are\n    //! active along the path *OR* at termination.\n    static OrientationConstraint None();\n\n    //! Create an `OrientationConstraint` such that the tool frame orientation along the path *AND*\n    //! at termination are constrained to the tool frame orientation at the initial\n    //! c-space position.\n    //!\n    //! The optional parameters `path_deviation_limit` and `terminal_deviation_limit` can be used\n    //! to allow deviations from this constant orientation along the path and at termination\n    //! respectively. If input, the limits are expressed in radians. These limits specify the\n    //! maximum allowable deviation from the desired orientation.\n    //!\n    //! If both limits are input, the `path_deviation_limit` must be greater than or equal to\n    //! (i.e., no more restrictive than) the `terminal_deviation_limit`.\n    //!\n    //! If neither is input, then both deviation limits are assumed to be zero, indicating that\n    //! orientation should be exactly constant.\n    //!\n    //! It is valid to input a `path_deviation_limit` without a `terminal_deviation_limit`\n    //! (implicitly setting the latter to zero) but not to input a `terminal_deviation_limit`\n    //! without a `path_deviation_limit`. This ensures that the terminal constraint is more\n    //! restrictive than the corresponding path constraint.\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `path_deviation_limit` is near or greater than pi, *OR*\n    //!   2. `terminal_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `path_deviation_limit` is negative,\n    //!   2. `terminal_deviation_limit` is negative,\n    //!   3. both `path_deviation_limit` and `terminal_deviation_limit` are defined, but\n    //!      `path_deviation_limit` < `terminal_deviation_limit`, *OR*\n    //!   4. `terminal_deviation_limit` is defined without defining a `path_deviation_limit`.\n    static OrientationConstraint Constant(const double *path_deviation_limit = nullptr,\n                                          const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraint` such that a tool frame `orientation_target` is specified\n    //! at termination, but no orientation constraints are active along the path.\n    //!\n    //! The optional parameter `terminal_deviation_limit` can be used to allow deviation from the\n    //! `orientation_target` at termination. If input, `terminal_deviation_limit` is expressed in\n    //! radians. This limit specifies the maximum allowable deviation from the desired\n    //! orientation.\n    //!\n    //! If `terminal_deviation_limit` is not input, then the deviation limit is assumed to be zero\n    //! (i.e., it is desired that terminal orientation be exactly `orientation_target`).\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   - `terminal_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   - `terminal_deviation_limit` is negative.\n    //!\n    //! NOTE: The `orientation_target` is specified in world frame coordinates (i.e., relative to\n    //! the base of the robot).\n    static OrientationConstraint TerminalTarget(const Rotation3 &orientation_target,\n                                                const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraint` such that a tool frame `orientation_target` is specified\n    //! along the path *AND* at termination.\n    //!\n    //! The optional parameters `path_deviation_limit` and `terminal_deviation_limit` can be used\n    //! to allow deviations from the `orientation_target` along the path and at termination\n    //! respectively. If input, the limits are expressed in radians. These limits specify the\n    //! maximum allowable deviation from the desired orientation.\n    //!\n    //! If both limits are input, the `path_deviation_limit` must be greater than or equal to\n    //! (i.e., no more restrictive than) the `terminal_deviation_limit`.\n    //!\n    //! If the `path_deviation_limit` is *NOT* input, its value is set to the most restrictive\n    //! feasible value. This feasibility is determined by the tool frame orientation at the initial\n    //! c-space position. If the initial orientation exactly matches `orientation_target`, then\n    //! the effective `path_deviation_limit` will be zero. Otherwise, the `path_deviation_limit`\n    //! will be set to the angular distance between the initial orientation and\n    //! `orientation_target`. If the `path_deviation_limit` is input, it must be greater than or\n    //! equal to this angular distance.\n    //!\n    //! If the `terminal_deviation_limit` is *NOT* input, its value is set to zero (i.e., the most\n    //! restrictive feasible value). If `terminal_deviation_limit` *AND* `path_deviation_limit` are\n    //! input, then `terminal_deviation_limit` must be less than or equal to `path_deviation_limit`.\n    //! If `terminal_deviation_limit` is specified but `path_deviation_limit` is auto-computed, then\n    //! `terminal_deviation_limit` will be clamped to the minimum of the input value and the\n    //! auto-computed value of `path_deviation_limit`.\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `path_deviation_limit` is near or greater than pi, *OR*\n    //!   2. `terminal_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `path_deviation_limit` is negative,\n    //!   2. `path_deviation_limit` results in an infeasible initial c-space position,\n    //!   3. `terminal_deviation_limit` is negative, *OR*\n    //!   4. `terminal_deviation_limit` is greater than `path_deviation_limit`.\n    //!\n    //! NOTE: Condition [3] will only be validated when the resulting `OrientationConstraint` is\n    //!       used for trajectory optimization (i.e., it is used as input to\n    //!       `planToTaskSpaceTarget()`).\n    //!\n    //! NOTE: The `orientation_target` is specified in world frame coordinates (i.e., relative to\n    //!       the base of the robot).\n    static OrientationConstraint TerminalAndPathTarget(\n        const Rotation3 &orientation_target,\n        const double *path_deviation_limit = nullptr,\n        const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraint` such that the terminal tool frame orientation is\n    //! constrained to rotate about a \"free axis\", but no orientation constraints are active along\n    //! the path.\n    //!\n    //! The \"free axis\" for rotation is defined by a `tool_frame_axis` (specified in the tool frame\n    //! coordinates) and a corresponding `world_target_axis` (specified in world frame coordinates).\n    //!\n    //! The optional `terminal_axis_deviation_limit` can be used to allow deviation from the\n    //! desired axis alignment at termination. If input, `terminal_axis_deviation_limit` is\n    //! expressed in radians and the limit specifies the maximum allowable deviation of the\n    //! rotation axis at termination. If `axis_deviation_limit` is not input, then the deviation\n    //! limit is assumed to be zero (i.e., it is desired that the tool frame axis be exactly\n    //! aligned with `world_target_axis`).\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   - `terminal_axis_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `terminal_axis_deviation_limit` is negative,\n    //!   2. `tool_frame_axis` is (nearly) zero, *OR*\n    //!   3. `world_target_axis` is (nearly) zero.\n    //!\n    //! NOTE: `tool_frame_axis` and `world_target_axis` inputs will be normalized.\n    static OrientationConstraint TerminalAxis(\n        const Eigen::Vector3d &tool_frame_axis,\n        const Eigen::Vector3d &world_target_axis,\n        const double *terminal_axis_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraint` such that the tool frame orientation is constrained to\n    //! rotate about a \"free axis\" along the path *AND* at termination.\n    //!\n    //! The \"free axis\" for rotation is defined by a `tool_frame_axis` (specified in the tool frame\n    //! coordinates) and a corresponding `world_target_axis` (specified in world frame coordinates).\n    //!\n    //! The optional parameters `path_axis_deviation_limit` and `terminal_axis_deviation_limit`\n    //! can be used to allow deviation from the desired axis alignment along the path and at\n    //! termination respectively. If input, the limits are expressed in radians.\n    //!\n    //! If both limits are input, the `path_axis_deviation_limit` must be greater than or equal to\n    //! (i.e., no more restrictive than) the `terminal_axis_deviation_limit`.\n    //!\n    //! If the `path_axis_deviation_limit` is *NOT* input, its value is set to the most restrictive\n    //! feasible value. This feasibility is determined by the tool frame orientation at the initial\n    //! c-space position. If the initial orientation exactly satisfies the axis constraint,\n    //! then the effective `path_axis_deviation_limit` will be zero. Otherwise, the\n    //! `path_axis_deviation_limit` will be set to the angular distance between the initial tool\n    //! frame rotation axis and the desired tool frame rotation axis. If the\n    //! `path_axis_deviation_limit` is input, it must be greater than or equal to this angular\n    //! distance.\n    //!\n    //! If the `terminal_deviation_limit` is *NOT* input, its value is set to zero (i.e., the most\n    //! restrictive feasible value). If `terminal_deviation_limit` *AND* `path_deviation_limit` are\n    //! input, then `terminal_deviation_limit` must be less than or equal to `path_deviation_limit`.\n    //! If `terminal_deviation_limit` is specified but `path_deviation_limit` is auto-computed, then\n    //! `terminal_deviation_limit` will be clamped to the minimum of the input value and the\n    //! auto-computed value of `path_deviation_limit`.\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `path_axis_deviation_limit` is near or greater than pi, *OR*\n    //!   2. `terminal_axis_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `path_axis_deviation_limit` is negative,\n    //!   2. `path_axis_deviation_limit` results in an infeasible initial c-space position,\n    //!   3. `terminal_axis_deviation_limit` is negative,\n    //!   4. `terminal_axis_deviation_limit` is > to `path_deviation_limit`,\n    //!   5. `tool_frame_axis` is (nearly) zero, *OR*\n    //!   6. `world_target_axis` is (nearly) zero.\n    //!\n    //! NOTE: Condition [3] will only be validated when the resulting `OrientationConstraint` is\n    //!       used for trajectory optimization (i.e., it is used as input to\n    //!       `planToTaskSpaceTarget()`).\n    //!\n    //! NOTE: `tool_frame_axis` and `world_target_axis` inputs will be normalized.\n    static OrientationConstraint TerminalAndPathAxis(\n        const Eigen::Vector3d &tool_frame_axis,\n        const Eigen::Vector3d &world_target_axis,\n        const double *path_axis_deviation_limit = nullptr,\n        const double *terminal_axis_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraint` such that a tool frame `terminal_orientation_target` is\n    //! specified at termination, *AND* the tool frame orientation is constrained to rotate\n    //! about a \"free axis\" along the path.\n    //!\n    //! The optional parameter `terminal_deviation_limit` can be used to allow deviation from the\n    //! `orientation_target` at termination. If input, `terminal_deviation_limit` is expressed in\n    //! radians. This limit specifies the maximum allowable deviation from the desired\n    //! orientation.\n    //!\n    //! If `terminal_deviation_limit` is not input, then the deviation limit is assumed to be zero\n    //! (i.e., it is desired that terminal orientation be exactly `terminal_orientation_target`).\n    //!\n    //! The \"free axis\" for rotation along the path is defined by a `tool_frame_axis` (specified\n    //! in the tool frame coordinates) and a corresponding `world_target_axis` (specified in\n    //! world frame coordinates).\n    //!\n    //! The optional `path_axis_deviation_limit` can be used to allow deviation from the\n    //! desired axis alignment along the path. If input, `path_axis_deviation_limit` is\n    //! expressed in radians. This limit specifies the maximum allowable deviation of the\n    //! rotation axis along the path.\n    //!\n    //! If the `path_axis_deviation_limit` is *NOT* input, its value is set to the most restrictive\n    //! feasible value. This feasibility is determined by the tool frame orientation at the initial\n    //! c-space position. If the initial orientation exactly satisfies the axis constraint,\n    //! then the effective `path_axis_deviation_limit` will be zero. Otherwise, the\n    //! `path_axis_deviation_limit` will be set to the angular distance between the initial tool\n    //! frame rotation axis and the desired tool frame rotation axis. If the\n    //! `path_axis_deviation_limit` is input, it must be greater than or equal to this angular\n    //! distance.\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `path_axis_deviation_limit` is near or greater than pi, *OR*\n    //!   2. `terminal_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `terminal_deviation_limit` is negative,\n    //!   2. `path_axis_deviation_limit` is negative,\n    //!   3. `path_axis_deviation_limit` results in an infeasible initial c-space position,\n    //!   4. `tool_frame_axis` is (nearly) zero, *OR*\n    //!   5. `world_target_axis` is (nearly) zero.\n    //!\n    //! NOTE: Condition [5] will only be validated when the resulting `OrientationConstraint` is\n    //!       used for trajectory optimization (i.e., it is used as input to\n    //!       `planToTaskSpaceTarget()`).\n    //!\n    //! NOTE: `tool_frame_axis` and `world_target_axis` inputs will be normalized.\n    //!\n    //! NOTE: The `terminal_orientation_target` is specified in world frame coordinates\n    //!       (i.e., relative to the base of the robot).\n    static OrientationConstraint TerminalTargetAndPathAxis(\n        const Rotation3 &terminal_orientation_target,\n        const Eigen::Vector3d &tool_frame_axis,\n        const Eigen::Vector3d &world_target_axis,\n        const double *terminal_deviation_limit = nullptr,\n        const double *path_axis_deviation_limit = nullptr);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `OrientationConstraint` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of `OrientationConstraint`s are considered concurrently. Each\n  //! `OrientationConstraint` in the goalset must have the same mode (e.g., \"full terminal target\n  //! with free axis path constraint\"), but may have different data for each\n  //! `OrientationConstraint`.\n  class CUMO_EXPORT OrientationConstraintGoalset {\n   public:\n    //! Create an `OrientationConstraintGoalset` such that no tool frame orientation constraints\n    //! are active along the path *OR* at termination.\n    static OrientationConstraintGoalset None();\n\n    //! Create an `OrientationConstraintGoalset` such that the tool frame orientation along the\n    //! path *AND* at termination are constrained to the tool frame orientation at the initial\n    //! c-space position.\n    //!\n    //! See `OrientationConstraint::Constant()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::Constant()` is not met.\n    static OrientationConstraintGoalset Constant(const double *path_deviation_limit = nullptr,\n                                                 const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraintGoalset` such that tool frame `orientation_targets` are\n    //! specified at termination, but no orientation constraints are active along the path.\n    //!\n    //! See `OrientationConstraint::TerminalTarget()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::TerminalTarget()` is not met, *OR*\n    //!   2. `orientation_targets` is empty.\n    static OrientationConstraintGoalset TerminalTarget(\n        const std::vector<Rotation3> &orientation_targets,\n        const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraintGoalset` such that tool frame `orientation_targets` are\n    //! specified along the path *AND* at termination.\n    //!\n    //! See `OrientationConstraint::TerminalAndPathTarget()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::TerminalAndPathTarget()` is not met, *OR*\n    //!   2. `orientation_targets` is empty.\n    static OrientationConstraintGoalset TerminalAndPathTarget(\n        const std::vector<Rotation3> &orientation_targets,\n        const double *path_deviation_limit = nullptr,\n        const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraintGoalset` such that the terminal tool frame orientation is\n    //! constrained to rotate about a \"free axis\", but no orientation constraints are active along\n    //! the path.\n    //!\n    //! See `OrientationConstraint::TerminalAxis()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::TerminalAxis()` is not met,\n    //!   2. `tool_frame_axes` and `world_target_axes` do not have the same number of elements, *OR*\n    //!   3. `tool_frame_axes` and `world_target_axes` are empty.\n    static OrientationConstraintGoalset TerminalAxis(\n        const std::vector<Eigen::Vector3d> &tool_frame_axes,\n        const std::vector<Eigen::Vector3d> &world_target_axes,\n        const double *terminal_axis_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraintGoalset` such that the tool frame orientation is\n    //! constrained to rotate about a \"free axis\" along the path *AND* at termination.\n    //!\n    //! See `OrientationConstraint::TerminalAndPathAxis()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::TerminalAndPathAxis()` is not met,\n    //!   2. `tool_frame_axes` and `world_target_axes` do not have the same number of elements, *OR*\n    //!   3. `tool_frame_axes` and `world_target_axes` are empty.\n    static OrientationConstraintGoalset TerminalAndPathAxis(\n        const std::vector<Eigen::Vector3d> &tool_frame_axes,\n        const std::vector<Eigen::Vector3d> &world_target_axes,\n        const double *path_axis_deviation_limit = nullptr,\n        const double *terminal_axis_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraintGoalset` such that tool frame `terminal_orientation_targets`\n    //! are specified at termination, *AND* the tool frame orientation is constrained to rotate\n    //! about a \"free axis\" along the path.\n    //!\n    //! See `OrientationConstraint::TerminalTargetAndPathAxis()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::TerminalTargetAndPathAxis()` is not met,\n    //!   2. `terminal_orientation_targets`, `tool_frame_axes`, and `world_target_axes` do not have\n    //!      the same number of elements, *OR*\n    //!   3. `terminal_orientation_targets`, `tool_frame_axes`, and `world_target_axes` are empty.\n    static OrientationConstraintGoalset TerminalTargetAndPathAxis(\n        const std::vector<Rotation3> &terminal_orientation_targets,\n        const std::vector<Eigen::Vector3d> &tool_frame_axes,\n        const std::vector<Eigen::Vector3d> &world_target_axes,\n        const double *terminal_deviation_limit = nullptr,\n        const double *path_axis_deviation_limit = nullptr);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Task-space targets restrict the position and (optionally) orientation of the tool frame at the\n  //! termination of a trajectory and (optionally) along the path.\n  struct CUMO_EXPORT TaskSpaceTarget {\n    //! Create a task-space target.\n    explicit TaskSpaceTarget(\n        const TranslationConstraint &translation_constraint,\n        const OrientationConstraint &orientation_constraint = OrientationConstraint::None());\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `TaskSpaceTarget` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of pose constraints are considered concurrently. Each pose\n  //! constraint in the goalset must have the same mode (e.g., \"terminal target\n  //! with linear path constraint and no orientation constraints\") but may have different data for\n  //! each constraint.\n  struct CUMO_EXPORT TaskSpaceTargetGoalset {\n    //! Create a task-space target goalset.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. The number of `translation_constraints` does not match the number of\n    //!      `orientation_constraints`.\n    explicit TaskSpaceTargetGoalset(const TranslationConstraintGoalset &translation_constraints,\n                                    const OrientationConstraintGoalset &orientation_constraints =\n                                        OrientationConstraintGoalset::None());\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! C-space targets fully restrict the c-space configuration at the termination of the trajectory\n  //! while allowing (optional) task-space constraints along the path.\n  class CUMO_EXPORT CSpaceTarget {\n   public:\n    //! Translation path constraints restrict the position of the origin of a tool frame along the\n    //! path.\n    class CUMO_EXPORT TranslationPathConstraint {\n     public:\n      //! Create a `TranslationPathConstraint` such that the position of the tool frame is not\n      //! restricted along the path.\n      static TranslationPathConstraint None();\n\n      //! Create a `TranslationPathConstraint` such that a linear translation constraint is active\n      //! along the path.\n      //!\n      //! The linear path constraint is defined by the line **segment** between the origin of the\n      //! tool frame at the initial c-space position and the origin of the tool frame at the\n      //! terminal c-space target. This path constraint is satisfied if the origin of the tool frame\n      //! is on this line segment between the initial and final tool positions at all points in the\n      //! trajectory. It is considered a constraint violation if the origin of the tool frame\n      //! \"overshoots\" either the initial or final tool positions.\n      //!\n      //! If the distance between the initial and final tool frames are nearly the same, the path\n      //! constraint becomes a constraint on the distance from the initial and final positions in\n      //! any direction.\n      //!\n      //! The optional parameter `path_deviation_limit` can be used to allow deviation from the\n      //! nominal task-space line along the path. This limit specifies the maximum allowable\n      //! deviation from the desired linear path.\n      //!\n      //! If `path_deviation_limit` is not input, then the deviation limit is assumed to be zero\n      //! (i.e., it is desired that translation is always exactly coincident with the specified\n      //! line).\n      //!\n      //! A fatal error will be logged if:\n      //!   1. `path_deviation_limit` is negative.\n      static TranslationPathConstraint Linear(const double *path_deviation_limit = nullptr);\n\n      struct Impl;\n      std::shared_ptr<Impl> impl;\n    };\n\n    //! Orientation path constraints restrict the orientation of a tool frame along the path.\n    class CUMO_EXPORT OrientationPathConstraint {\n     public:\n      //! Create a `OrientationPathConstraint` such that the orientation of the tool frame is not\n      //! restricted along the path.\n      static OrientationPathConstraint None();\n\n      //! Create an `OrientationPathConstraint` such that the tool frame orientation along the path\n      //! is constant.\n      //!\n      //! If `use_terminal_orientation` is set to `true`, then the tool frame orientation\n      //! corresponding to the terminal c-space target will be used to define the orientation\n      //! target along the path. Otherwise, the tool frame orientation corresponding to the initial\n      //! c-space position will be used.\n      //!\n      //! The optional parameter `path_deviation_limit` can be used to allow deviations from this\n      //! constant orientation along the path. If input, the units are radians. These limits\n      //! specify the maximum allowable deviation from the desired orientation.\n      //!\n      //! If the `path_deviation_limit` is *NOT* input, its value is set to the most restrictive\n      //! feasible value. This feasible limit is determined by the angular distance between the tool\n      //! frame orientations at the initial and terminal c-space positions. If the\n      //! `path_deviation_limit` is input, it must be greater than or equal to this angular\n      //! distance.\n      //!\n      //! In general, it is suggested that angular deviation limits are set to values less than pi.\n      //! A limit that is near or greater than pi essentially disables the constraint (without\n      //! culling the computation). Non-fatal warnings will be logged if:\n      //!   - `path_deviation_limit` is near or greater than pi.\n      //!\n      //! A fatal error will be logged if:\n      //!   1. `path_deviation_limit` is negative, *OR*\n      //!   2. `path_deviation_limit` is infeasible for the given initial c-space position and\n      //!      terminal c-space target.\n      //!\n      //! NOTE: Condition [3] will only be validated when the resulting `OrientationPathConstraint`\n      //!       is used for trajectory optimization (i.e., it is used as input to\n      //!       `planToCSpaceTarget()`).\n      static OrientationPathConstraint Constant(const double *path_deviation_limit = nullptr,\n                                                bool use_terminal_orientation = true);\n\n      //! Create an `OrientationPathConstraint` such that the tool frame orientation is constrained\n      //! to rotate about a \"free axis\" along the path.\n      //!\n      //! The \"free axis\" for rotation is defined by a `tool_frame_axis` (specified in the tool\n      //! frame coordinates) and a corresponding `world_target_axis` (specified in world frame\n      //! coordinates).\n      //!\n      //! The optional parameter `path_axis_deviation_limit` can be used to allow deviation from\n      //! the desired axis alignment along the path. If input, the units are radians.\n      //!\n      //! If the `path_axis_deviation_limit` is *NOT* input, its value is set to the most\n      //! restrictive feasible value. This feasibility is determined by the tool frame orientations\n      //! at the initial and terminal c-space positions. For each orientation, the angular distance\n      //! (if any) from satisfying the constraint will be computed. The `path_axis_deviation_limit`\n      //! will be set to the maximum of these angular distances. If the `path_axis_deviation_limit`\n      //! is input, it must be greater than or equal to this minimum feasible deviation limit.\n      //!\n      //! In general, it is suggested that angular deviation limits are set to values less than pi.\n      //! A limit that is near or greater than pi essentially disables the constraint (without\n      //! culling the computation). Non-fatal warnings will be logged if:\n      //!   - `path_axis_deviation_limit` near or greater than pi.\n      //!\n      //! A fatal error will be logged if:\n      //!   1. `path_axis_deviation_limit` is negative,\n      //!   2. `path_axis_deviation_limit` is infeasible for the given initial c-space position and\n      //!      terminal c-space target,\n      //!   3. `tool_frame_axis` is (nearly) zero, *OR*\n      //!   4. `world_target_axis` is (nearly) zero.\n      //!\n      //! NOTE: Condition [3] will only be validated when the resulting `OrientationPathConstraint`\n      //!       is used for trajectory optimization (i.e., it is used as input to\n      //!       `planToCSpaceTarget()`).\n      //!\n      //! NOTE: `tool_frame_axis` and `world_target_axis` inputs will be normalized.\n      static OrientationPathConstraint Axis(const Eigen::Vector3d &tool_frame_axis,\n                                            const Eigen::Vector3d &world_target_axis,\n                                            const double *path_axis_deviation_limit = nullptr);\n\n      struct Impl;\n      std::shared_ptr<Impl> impl;\n    };\n\n    //! Create a c-space target.\n    explicit CSpaceTarget(const Eigen::VectorXd &cspace_position_terminal_target,\n                          const TranslationPathConstraint &translation_path_constraint =\n                              TranslationPathConstraint::None(),\n                          const OrientationPathConstraint &orientation_path_constraint =\n                              OrientationPathConstraint::None());\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Results from a trajectory optimization.\n  class CUMO_EXPORT Results {\n   public:\n    //! Indicate the success or failure of the trajectory optimization.\n    enum class Status {\n      //! Valid trajectory found.\n      SUCCESS,\n\n      //! Invalid initial c-space position.\n      //!\n      //! NOTE: The `RobotWorldInspector` can be used to determine if this invalid state is\n      //!       due to world-collision, self-collision, c-space position limit violations, etc.\n      INVALID_INITIAL_CSPACE_POSITION,\n\n      //! Invalid target c-space position.\n      //!\n      //! Only applicable when planning to c-space targets.\n      //!\n      //! NOTE: The `RobotWorldInspector` can be used to determine if this invalid state is\n      //!       due to world-collision, self-collision, c-space position limit violations, etc.\n      INVALID_TARGET_CSPACE_POSITION,\n\n      //! The c-space or task-space target specification is invalid.\n      INVALID_TARGET_SPECIFICATION,\n\n      //! The inverse kinematics solver failed to find a valid solution.\n      INVERSE_KINEMATICS_FAILURE,\n\n      //! Initial trajectory optimization attempts failed and geometric planning was attempted as\n      //! a fallback, but this geometric planning routine also failed.\n      GEOMETRIC_PLANNING_FAILURE,\n\n      //! Initial trajectory optimization attempts failed and geometric planning was attempted as\n      //! a fallback (and was successful), but secondary trajectory optimization using the\n      //! geometrically planned path as a warm start failed to produce a valid trajectory.\n      TRAJECTORY_OPTIMIZATION_FAILURE\n    };\n\n    virtual ~Results() = default;\n\n    //! Return the `Status` from the trajectory optimization.\n    [[nodiscard]] virtual Status status() const = 0;\n\n    //! If `status()` returns `SUCCESS`, then `trajectory()` returns a valid `Trajectory`.\n    //!\n    //! If `status()` returns `INVALID_INITIAL_CSPACE_POSITION`,\n    //! `INVALID_TARGET_CSPACE_POSITION`, `INVALID_TARGET_SPECIFICATION`, or\n    //! `INVERSE_KINEMATICS_FAILURE`, then `nullptr` will be returned.\n    //!\n    //! If `status()` returns `GEOMETRIC_PLANNING_FAILURE` or `TRAJECTORY_OPTIMIZATION_FAILURE`,\n    //! then the lowest-cost (but *invalid*) `Trajectory` will be returned.\n    [[nodiscard]] virtual std::unique_ptr<Trajectory> trajectory() const = 0;\n\n    //! Return the index of the target selected for the terminal knot point.\n    //!\n    //! For goalset planning (e.g., `planToTaskSpaceGoalset()`) this represents an index into the\n    //! constraint vectors used to create the goalset (e.g., `TaskSpaceTargetGoalset`).\n    //!\n    //! For single target planning (e.g., `planToTaskSpaceTarget()` and `planToCSpaceTarget()`),\n    //! zero will be returned for successful trajectory optimization.\n    //!\n    //! In all cases, -1 will be returned if trajectory optimization is unsuccessful.\n    [[nodiscard]] virtual int targetIndex() const = 0;\n  };\n\n  //! Attempt to find a trajectory from `initial_cspace_position` to `task_space_target`.\n  [[nodiscard]] virtual std::unique_ptr<Results> planToTaskSpaceTarget(\n      const Eigen::VectorXd &initial_cspace_position,\n      const TaskSpaceTarget &task_space_target) const = 0;\n\n  //! Attempt to find a trajectory from `initial_cspace_position` to `task_space_target_goalset`.\n  [[nodiscard]] virtual std::unique_ptr<Results> planToTaskSpaceGoalset(\n      const Eigen::VectorXd &initial_cspace_position,\n      const TaskSpaceTargetGoalset &task_space_target_goalset) const = 0;\n\n  //! Attempt to find a trajectory from `initial_cspace_position` to `cspace_target`.\n  [[nodiscard]] virtual std::unique_ptr<Results> planToCSpaceTarget(\n      const Eigen::VectorXd &initial_cspace_position,\n      const CSpaceTarget &cspace_target) const = 0;\n};\n\n//! Create a `TrajectoryOptimizer` with the given `config`.\nCUMO_EXPORT std::unique_ptr<TrajectoryOptimizer> CreateTrajectoryOptimizer(\n    const TrajectoryOptimizerConfig &config);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/version.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#define CUMOTION_VERSION_MAJOR 1\n#define CUMOTION_VERSION_MINOR 1\n#define CUMOTION_VERSION_PATCH 0\n#define CUMOTION_VERSION_SUFFIX \"\"\n\n#define CUMOTION_VERSION_STRING \"1.1.0\"\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/vision.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2024-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for handling depth camera data\n//!\n//! @note This interface is experimental and may evolve in a future release.  It also lacks\n//!       a corresponding Python API.\n\n#pragma once\n\n#include <cstdint>\n#include <memory>\n#include <optional>\n#include <type_traits>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Camera intrinsic parameters.\n//!\n//! The intrinsics define the mapping from 3D camera coordinates to 2D image coordinates. The\n//! standard pinhole camera model uses four parameters (`fx`, `fy`, `cx`, `cy`) that form the\n//! intrinsic matrix:\n//!\n//! @code\n//!   [fx   0  cx]\n//!   [ 0  fy  cy]\n//!   [ 0   0   1]\n//! @endcode\n//!\n//! where `fx` and `fy` are the focal lengths in pixels, and `cx` and `cy` are the principal point\n//! coordinates (optical center) in pixels.\nclass CUMO_EXPORT CameraIntrinsics {\n public:\n  //! Construct from the four standard pinhole camera parameters.\n  //!\n  //! The parameters `fx` and `fy` represent the focal lengths in the x and y directions\n  //! (in pixels), while `cx` and `cy` represent the principal point (optical center) coordinates.\n  CameraIntrinsics(double fx, double fy, double cx, double cy);\n\n  //! Construct from a full 3x3 intrinsic matrix.\n  explicit CameraIntrinsics(const Eigen::Matrix3d &matrix);\n\n  //! Return the focal length in the x direction (in pixels).\n  [[nodiscard]] double fx() const;\n\n  //! Return the focal length in the y direction (in pixels).\n  [[nodiscard]] double fy() const;\n\n  //! Return the x coordinate of the principal point (optical center) in pixels.\n  [[nodiscard]] double cx() const;\n\n  //! Return the y coordinate of the principal point (optical center) in pixels.\n  [[nodiscard]] double cy() const;\n\n  //! Return the full 3x3 intrinsic matrix.\n  [[nodiscard]] Eigen::Matrix3d matrix() const;\n\n  //! Return whether the intrinsic matrix has the standard pinhole camera structure.\n  //!\n  //! Returns `true` if the matrix follows the canonical pinhole format with zero skew, zeros in the\n  //! lower triangular entries, and 1 in the bottom-right corner. This allows for optimized\n  //! operations that only use the four key parameters (`fx`, `fy`, `cx`, `cy`).\n  [[nodiscard]] bool isPinhole() const;\n\n  struct Impl;\n  std::shared_ptr<Impl> impl;\n};\n\n//! Memory residency for buffer data.\nenum class BufferResidency {\n  //! Host memory allocated via `malloc()`, `new`, `cudaMallocHost()`, etc.\n  HOST,\n\n  //! Device memory allocated via `cudaMalloc()` or similar CUDA device allocators.\n  DEVICE,\n\n  //! CUDA unified/managed memory allocated via `cudaMallocManaged()`.\n  //!\n  //! This memory is accessible from both host and device, with migration handled as needed by the\n  //! CUDA runtime.\n  MANAGED\n};\n\n//! Type-agnostic base class for depth images and (non-owning) views of depth images.\n//!\n//! See `DepthImage` for member function documentation.\nclass CUMO_EXPORT DepthImageBase {\n public:\n  //! Scalar type used to represent the depth values.\n  enum class ScalarType {\n    FLOAT,  //!< 32-bit floating point (corresponds to `DepthImage<float>`)\n    UINT16  //!< 16-bit unsigned integer (corresponds to `DepthImage<uint16_t>`)\n  };\n\n  virtual ~DepthImageBase() = default;\n\n  [[nodiscard]] virtual int width() const = 0;\n  [[nodiscard]] virtual int height() const = 0;\n  [[nodiscard]] virtual int stride() const = 0;\n  [[nodiscard]] virtual BufferResidency residency() const = 0;\n  [[nodiscard]] virtual double metersPerUnit() const = 0;\n  [[nodiscard]] virtual bool isView() const = 0;\n  [[nodiscard]] virtual ScalarType scalarType() const = 0;\n  [[nodiscard]] virtual bool isConst() const = 0;\n  [[nodiscard]] virtual double depthInMeters(int x, int y) const = 0;\n};\n\n//! A depth image or non-owning view of a depth image buffer.\n//!\n//! The data buffer is assumed to be contiguous with pixels in the conventional row-major order\n//! (with the x coordinate varying fastest) and with the origin at the top-left corner.\n//!\n//! Supported types are `float` and `uint16_t`.  The latter is often used for raw depth data where\n//! distance is measured in millimeters.  Const-qualified variants are also supported.\n//!\n//! The `stride()` will always be equal to or greater than `width()`.  The pixel at coordinates\n//! (x, y) is located at index `y * stride() + x` in the buffer, where `x` is the column index and\n//! `y` is the row index.\ntemplate<typename T = float>\nclass DepthImage : public DepthImageBase {\n  using BaseT = std::remove_const_t<T>;\n  static_assert(std::is_same_v<BaseT, float> || std::is_same_v<BaseT, uint16_t>,\n                \"'DepthImage' supports 'float' and 'uint16_t' types.\");\n\n public:\n  //! Return the default `meters_per_unit` value for the scalar type `T`:\n  //!\n  //! - `0.001` for `uint16_t`, corresponding to units of millimeters.\n  //! - `1.0` for `float`, corresponding to units of meters.\n  [[nodiscard]] static constexpr double DefaultMetersPerUnit() {\n    return std::is_same_v<const T, const uint16_t> ? 0.001 : 1.0;\n  }\n\n  //! Construct a `DepthImage` that owns its own buffer.\n  //!\n  //! This constructor is only available for non-const scalar types (`float` or `uint16_t`).\n  //!\n  //! The optional `stride` parameter specifies the number of elements between consecutive rows. If\n  //! not provided, the stride defaults to `width` (tightly-packed layout). The `residency`\n  //! parameter indicates where to allocate the buffer, and the `meters_per_unit` parameter\n  //! specifies the scaling factor to convert depth values to meters.\n  //!\n  //! A fatal error will be logged if:\n  //!\n  //! 1. `width` or `height` is zero or negative, or\n  //! 2. `stride` is smaller than `width`.\n  template<typename U = T, typename = std::enable_if_t<!std::is_const_v<U>>>\n  DepthImage(int width, int height,\n             std::optional<int> stride = std::nullopt,\n             BufferResidency residency = BufferResidency::HOST,\n             double meters_per_unit = DefaultMetersPerUnit());\n\n  ~DepthImage() override = default;\n\n  DepthImage(DepthImage&&) noexcept = default;\n  DepthImage& operator=(DepthImage&&) noexcept = default;\n\n  // Copying is disallowed.\n  DepthImage(const DepthImage&) = delete;\n  DepthImage& operator=(const DepthImage&) = delete;\n\n  //! Return pointer to the underlying depth data buffer.\n  [[nodiscard]] T *data();\n\n  //! Return const pointer to the underlying depth data buffer.\n  [[nodiscard]] const T *data() const;\n\n  //! Return the width of the depth image in pixels.\n  [[nodiscard]] int width() const override;\n\n  //! Return the height of the depth image in pixels.\n  [[nodiscard]] int height() const override;\n\n  //! Return the stride of the depth image in elements.\n  //!\n  //! The stride represents the number of elements between consecutive rows in the buffer. For\n  //! tightly-packed images, stride equals width. For padded or aligned images, stride may be\n  //! larger than width.\n  [[nodiscard]] int stride() const override;\n\n  //! Return the memory residency of the depth image buffer.\n  [[nodiscard]] BufferResidency residency() const override;\n\n  //! Return the scaling factor to convert depth values to meters.\n  //!\n  //! This value represents the number of meters per depth unit. Common values include 1.0 for\n  //! depth already in meters, 0.001 for millimeters, and 0.0001 for tenths of millimeters.\n  [[nodiscard]] double metersPerUnit() const override;\n\n  //! Return whether this is a (non-owning) view or a `DepthImage` that owns its memory.\n  //!\n  //! Views do not manage the lifetime of the underlying buffer. Owning instances allocate and\n  //! deallocate the buffer automatically.\n  [[nodiscard]] bool isView() const override;\n\n  //! Return the scalar type of the depth image data (needed for `DepthImageBase` interface).\n  [[nodiscard]] ScalarType scalarType() const override {\n    using BaseT = std::remove_const_t<T>;\n    return std::is_same_v<BaseT, float> ? ScalarType::FLOAT : ScalarType::UINT16;\n  }\n\n  //! Return whether the depth image data is const-qualified (needed for `DepthImageBase`\n  //! interface).\n  [[nodiscard]] bool isConst() const override {\n    return std::is_const_v<T>;\n  }\n\n  //! Return the depth value at pixel coordinates (x, y).\n  //!\n  //! The returned value is the raw depth measurement without unit conversion. The `x` coordinate\n  //! corresponds to the column index, and `y` corresponds to the row index, with the origin at\n  //! the top-left corner.\n  //!\n  //! @warning This accessor does not perform bounds checking and does not check that the data is\n  //!          resident on the host.\n  [[nodiscard]] T at(int x, int y) const;\n\n  //! Return a reference to the depth value at pixel coordinates (x, y).\n  //!\n  //! This function allows modification of the depth value and is only available for non-const\n  //! template instantiations. The `x` coordinate corresponds to the column index, and `y`\n  //! corresponds to the row index, with the origin at the top-left corner.\n  //!\n  //! @warning This accessor does not perform bounds checking and does not check that the data is\n  //!          resident on the host.\n  [[nodiscard]] T &at(int x, int y);\n\n  //! Return the depth value at pixel coordinates (`x`, `y`) converted to meters.\n  //!\n  //! This method returns the depth value scaled by `metersPerUnit()`. The `x` coordinate\n  //! corresponds to the column index, and `y` corresponds to the row index, with the origin at\n  //! the top-left corner.\n  //!\n  //! A fatal error will be logged if the data is not resident on the host or if either `x` or `y`\n  //! is out of bounds.\n  [[nodiscard]] double depthInMeters(int x, int y) const override;\n\n  struct Impl;\n\n  //! Constructor used by the internal implementation.\n  explicit DepthImage(std::shared_ptr<Impl> impl);\n\n private:\n  std::shared_ptr<Impl> impl_;\n};\n\nextern template class CUMO_EXTERN_TEMPLATE_EXPORT DepthImage<float>;\nextern template class CUMO_EXTERN_TEMPLATE_EXPORT DepthImage<const float>;\nextern template class CUMO_EXTERN_TEMPLATE_EXPORT DepthImage<uint16_t>;\nextern template class CUMO_EXTERN_TEMPLATE_EXPORT DepthImage<const uint16_t>;\n\n//! Create a non-owning `DepthImage` view from a depth data buffer.\n//!\n//! The template parameter `T` can be `float`, `uint16_t`, `const float`, or `const uint16_t`.\n//!\n//! The `data` parameter points to a contiguous buffer containing depth values arranged in\n//! row-major order. The buffer must remain valid for the lifetime of the returned `DepthImage`\n//! object, as the view does not take ownership of the memory.\n//!\n//! The optional `stride` parameter specifies the number of elements between consecutive rows. If\n//! not provided, the stride defaults to `width` (tightly-packed layout). The `residency`\n//! parameter indicates where the buffer is allocated, and the `meters_per_unit` parameter\n//! specifies the scaling factor to convert depth values to meters.\n//!\n//! A fatal error will be logged if:\n//!\n//! 1. `data` is null,\n//! 2. `width` or `height` is zero or negative, or\n//! 3. `stride` is smaller than `width`.\ntemplate<typename T>\nCUMO_EXPORT DepthImage<T> CreateDepthImageView(\n  T *data,\n  int width,\n  int height,\n  std::optional<int> stride = std::nullopt,\n  BufferResidency residency = BufferResidency::HOST,\n  double meters_per_unit = DepthImage<T>::DefaultMetersPerUnit());\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/world.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n#include <optional>\n#include <vector>\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/obstacle.h\"\n#include \"cumotion/pose3.h\"\n\nnamespace cumotion {\n\n//! Forward declaration of `WorldViewHandle` for use by `World::addWorldView()`.\nstruct WorldViewHandle;\n\n//! World represents a collection of obstacles.\nclass CUMO_EXPORT World {\n public:\n  //! Opaque handle to an obstacle.\n  struct CUMO_EXPORT ObstacleHandle {\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  virtual ~World() = default;\n\n  //! Add `obstacle` to the world.\n  //!\n  //! All attributes of obstacle are copied to world and subsequent changes to `obstacle` will\n  //! not be reflected in the world.\n  //!\n  //! If a `pose` is not provided for the `obstacle`, `Pose3::Identity()` will be used.\n  //!\n  //! Obstacles are automatically enabled when added.\n  virtual ObstacleHandle addObstacle(const Obstacle &obstacle,\n                                     std::optional<Pose3> pose = std::nullopt) = 0;\n\n  //! Permanently remove obstacle, invalidating its handle.\n  virtual void removeObstacle(const ObstacleHandle &obstacle) = 0;\n\n  //! Enable an obstacle for the purpose of collision checks and distance evaluations.\n  virtual void enableObstacle(const ObstacleHandle &obstacle) = 0;\n\n  //! Disable an obstacle for the purpose of collision checks and distance evaluations.\n  virtual void disableObstacle(const ObstacleHandle &obstacle) = 0;\n\n  //! Set the pose of the given obstacle.\n  virtual void setPose(const ObstacleHandle &obstacle, const Pose3 &pose) = 0;\n\n  //! Set the grid values for an obstacle of type SDF using a host-resident `values` buffer.\n  //!\n  //! It is assumed that `values` is stored with the `z` index varying fastest and has dimensions\n  //! given by the `Obstacle::Grid` associated with `obstacle`.  For example, for an obstacle with\n  //! `Grid` parameters `num_voxels_x`, `num_voxels_y`, and `num_voxels_z`, the length of `values`\n  //! should be `num_voxels_x * num_voxels_y * num_voxels_z`, and in the provided coordinates,\n  //! adjacent elements in memory should correspond to voxels with adjacent Z coordinates, and\n  //! voxels with adjacent X coordinates should be separated by `num_voxels_y * num_voxels_z`\n  //! elements.\n  //!\n  //! `precision` specifies the floating-point type of `values`.  `Obstacle::Grid::Precision::HALF`\n  //! corresponds to the `__half` data type defined in the `cuda_fp16.h` header.\n  //!\n  //! If the type of `obstacle` is not `Obstacle::Type::SDF`, a fatal error will be logged.\n  virtual void setSdfGridValuesFromHost(const ObstacleHandle &obstacle,\n                                        const void *values,\n                                        Obstacle::Grid::Precision grid_precision) = 0;\n\n  //! Set the grid values for an obstacle of type SDF using a device-resident buffer `values`.\n  //!\n  //! It is assumed that `values` is stored with the `z` index varying fastest and has dimensions\n  //! given by the `Obstacle::Grid` associated with `obstacle`.  For example, for an obstacle with\n  //! `Grid` parameters `num_voxels_x`, `num_voxels_y`, and `num_voxels_z`, the length of `values`\n  //! should be `num_voxels_x * num_voxels_y * num_voxels_z`, and in the provided coordinates,\n  //! adjacent elements in memory should correspond to voxels with adjacent Z coordinates, and\n  //! voxels with adjacent X coordinates should be separated by `num_voxels_y * num_voxels_z`\n  //! elements.\n  //!\n  //! `precision` specifies the floating-point type of `values`.  `Obstacle::Grid::Precision::HALF`\n  //! corresponds to the `__half` data type defined in the `cuda_fp16.h` header.\n  //!\n  //! If the type of `obstacle` is not `Obstacle::Type::SDF`, a fatal error will be logged.\n  virtual void setSdfGridValuesFromDevice(const ObstacleHandle &obstacle,\n                                          const void *values,\n                                          Obstacle::Grid::Precision grid_precision) = 0;\n\n  //! Tolerances used when inspecting an SDF.\n  //!\n  //! Accumulation of numerical error from type-casting is handled independently of the tolerances\n  //! in `SdfInspectionTolerances`.  It is expected that any SDF computed from an analytically\n  //! correct distance function will pass `InspectCudaSdfAndSync()` with the default tolerances of\n  //! `0.0f`.\n  //!\n  //! Inspection tolerances are specified in single-precision (`float`) because only single- and\n  //! half- precision data for device-resident SDFs are supported.\n  struct CUMO_EXPORT SdfInspectionTolerances {\n    //! Numerical tolerance on checking whether an SDF voxel value is equal to its neighbor.\n    //!\n    //! The effective tolerance used is the combination of this tolerance and an internally computed\n    //! `numerical_tolerance`.  `numerical_tolerance` is a derived upper bound on accumulated error\n    //! under the assumption that SDF data was computed analytically with infinite precision, then\n    //! cast to the `SDF` data-type specified by `Obstacle::Grid::device_precision`.\n    //!\n    //! A voxel is said to be equal to its neighbor if:\n    //! `abs(voxel_value - neighbor_value) <= voxel_matches_neighbor_tolerance +\n    //!                                       numerical_tolerance`.\n    float voxel_matches_neighbor_tolerance = 0.0f;\n\n    //! Numerical tolerance on checking whether an SDF voxel value is too far from a neighboring\n    //! voxel.\n    //!\n    //! The effective tolerance used is the combination of this tolerance and an internally computed\n    //! `numerical_tolerance`.  `numerical_tolerance` is a derived upper bound on accumulated error\n    //! under the assumption that SDF data was computed analytically with infinite precision, then\n    //! cast to the `SDF` data-type specified by `Obstacle::Grid::device_precision`.\n    //!\n    //! A voxel is said to be too far from its neighbor if:\n    //! `abs(voxel_value - neighbor_value) > voxel_too_far_from_neighbor_tolerance +\n    //!                                      numerical_tolerance`.\n    float voxel_too_far_from_neighbor_tolerance = 0.0f;\n  };\n\n  //! Inspection results associated with an SDF.\n  //!\n  //! When `numErrors() == 0`, this does *NOT* guarantee that the SDF is globally valid.  It only\n  //! shows that none of the specific error cases covered by the inspector were present.\n  struct CUMO_EXPORT SdfInspectionResults {\n    //! This count will be set to the number of voxels whose values match the values of `6`\n    //! neighboring voxels within the specified `voxel_matches_neighbor_tolerance`.  This can only\n    //! be triggered by non-boundary voxels, as boundary voxels do not have `6` neighbors.\n    //!\n    //! There is no geometrically consistent way this can happen in a valid distance field without\n    //! voxel-scale obstacles.\n    int num_voxels_matching_all_neighbors = 0;\n\n    //! This count will be set to the number of voxels whose distance to a neighboring voxel is\n    //! greater than the `voxel_size` parameter of the SDF inflated by the specified\n    //! `voxel_too_far_from_neighbor_tolerance`.\n    //!\n    //! There is no geometrically consistent way this can happen in a valid distance field.\n    int num_voxels_too_far_from_neighbors = 0;\n\n    //! Returns the sum of all error conditions in `SdfInspectionResults`.\n    [[nodiscard]] int numErrors() const {\n      return num_voxels_matching_all_neighbors + num_voxels_too_far_from_neighbors;\n    }\n  };\n\n  //! Inspect the data for an obstacle of type `SDF`.\n  //!\n  //! The returned `SdfInspectionResults` reports error-cases in the SDF data that are likely to\n  //! cause issues such as undetected collision with the environment.\n  //!\n  //! The default `inspection_tolerances` are recommended, and only need to be replaced if there is\n  //! known systematic error in the generation of SDF data within known bounds.\n  //!\n  //! A fatal error will be logged if:\n  //!  - `obstacle` does not have type `SDF`.\n  //!  - `obstacle` has not been populated with data.\n  //!  - `obstacle` has been removed from the world.\n  [[nodiscard]] virtual SdfInspectionResults inspectSdf(\n      const ObstacleHandle &obstacle,\n      std::optional<SdfInspectionTolerances> inspection_tolerances = std::nullopt) const = 0;\n\n  //! Create a view into the world that can be used for collision checks and distance evaluations.\n  //!\n  //! Each world view will maintain a static view of the world until it is updated. When a\n  //! world view is updated, it will reflect any changes to the world since its last update.\n  virtual WorldViewHandle addWorldView() = 0;\n};\n\n//! Create an empty world with no obstacles.\nCUMO_EXPORT std::shared_ptr<World> CreateWorld();\n\n//! A handle to a view of a `cumotion::World`.\n//!\n//! This view can be independently updated to track updates made to a `cumotion::World` object.\n//! A `WorldViewHandle` may be copied, with all copies sharing the same underlying view.\n//!\n//! To query spatial relationships in a world view, use `cumotion::WorldInspector`.\nstruct CUMO_EXPORT WorldViewHandle {\n  //! Update world view such that any changes to the underlying world are reflected in this view.\n  void update();\n\n  struct Impl;\n  std::shared_ptr<Impl> impl;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/include/cumotion/world_inspector.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for querying spatial relationships in a world.\n\n#pragma once\n\n#include <memory>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/pose3.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Interface for querying properties and spatial relationships within a world.\nclass CUMO_EXPORT WorldInspector {\n public:\n  virtual ~WorldInspector() = default;\n\n  //! Return whether the obstacle specified by `obstacle` is enabled in the current view of the\n  //! world.\n  [[nodiscard]] virtual bool isEnabled(const World::ObstacleHandle &obstacle) const = 0;\n\n  //! Return the pose of the obstacle specified by `obstacle`.\n  [[nodiscard]] virtual Pose3 pose(const World::ObstacleHandle &obstacle) const = 0;\n\n  //! Test whether a sphere defined by `center` and `radius` is in collision with ANY enabled\n  //! obstacle in the world.\n  [[nodiscard]] virtual bool inCollision(const Eigen::Vector3d &center, double radius) const = 0;\n\n  //! Test whether a sphere defined by `center` and `radius` is in collision with the obstacle\n  //! specified by `obstacle`.\n  [[nodiscard]] virtual bool inCollision(const World::ObstacleHandle &obstacle,\n                                         const Eigen::Vector3d &center,\n                                         double radius) const = 0;\n\n  //! Compute the distance from `point` to the obstacle specified by `obstacle`.\n  //!\n  //! Returns distance between the `point` and `obstacle`. If the `point` intersects `obstacle`, a\n  //! negative distance is returned.  The distance gradient is written to `gradient` if provided.\n  virtual double distanceTo(const World::ObstacleHandle &obstacle,\n                            const Eigen::Vector3d &point,\n                            Eigen::Vector3d *gradient = nullptr) const = 0;\n\n  //! Compute distances from `point` to all enabled obstacles.\n  //!\n  //! Distances between the `point` and each enabled obstacle are written to `distances`.\n  //! If the `point` intersects an obstacle, the resulting distance will be negative. The distance\n  //! gradients are written to `distance_gradients` if provided.\n  //!\n  //! The number of `distances` and/or `distance_gradients` that are written (i.e., the number of\n  //! enabled obstacles) is returned.\n  //!\n  //! If the length of `distances` is less than the number of enabled obstacles, it will be resized\n  //! to the number of enabled obstacles. The same applies to `distance_gradients` if provided. If\n  //! the vectors lengths are larger than the number of enabled obstacles, the vectors will NOT be\n  //! resized. Only the first N elements will be written to, where N is the number of enabled\n  //! obstacles. The values of extra elements at the end of the input vectors will NOT be changed.\n  virtual int distancesTo(const Eigen::Vector3d &point,\n                          std::vector<double> &distances,\n                          std::vector<Eigen::Vector3d> *distance_gradients = nullptr) const = 0;\n\n  //! Compute distances from `point` to all enabled obstacles.\n  //!\n  //! Distances between the `point` and each enabled obstacle are written to `distances`, which must\n  //! not be `nullptr`.\n  //! If the `point` intersects an obstacle, the resulting distance will be negative. The distance\n  //! gradients are written to `distance_gradients` if provided.\n  //!\n  //! The number of `distances` and/or `distance_gradients` that are written (i.e., the number of\n  //! enabled obstacles) is returned.\n  //!\n  //! If the length of `distances` is less than the number of enabled obstacles, it will be resized\n  //! to the number of enabled obstacles. The same applies to `distance_gradients` if provided. If\n  //! the vectors lengths are larger than the number of enabled obstacles, the vectors will NOT be\n  //! resized. Only the first N elements will be written to, where N is the number of enabled\n  //! obstacles. The values of extra elements at the end of the input vectors will NOT be changed.\n  //!\n  //! A fatal error will be logged if `distances` is `nullptr`.\n  //!\n  //! DEPRECATED: This function overload is deprecated and will be removed in a future version. Use\n  //!             the reference overload (`WorldInspector::distancesTo(const Eigen::Vector3d &point,\n  //!             std::vector<double> &distances, std::vector<Eigen::Vector3d> *distance_gradients =\n  //!             nullptr) const`) instead.\n  virtual CUMO_DEPRECATED int distancesTo(\n      const Eigen::Vector3d &point,\n      std::vector<double> *distances,\n      std::vector<Eigen::Vector3d> *distance_gradients = nullptr) const = 0;\n\n  //! Compute the minimum distance from `point` to ANY enabled obstacle in the current\n  //! view of the world.\n  //!\n  //! Returns distance between the `point` and the nearest obstacle. If the `point` is inside an\n  //! obstacle, a negative distance is returned. The distance gradient is written to `gradient` if\n  //! provided.\n  virtual double minDistance(const Eigen::Vector3d &point,\n                             Eigen::Vector3d *gradient = nullptr) const = 0;\n\n  //! Return the number of enabled obstacles in the current view of the world.\n  [[nodiscard]] virtual int numEnabledObstacles() const = 0;\n};\n\n//! Create a `WorldInspector` for a given `world_view`.\nCUMO_EXPORT std::unique_ptr<WorldInspector> CreateWorldInspector(const WorldViewHandle &world_view);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/lib/cmake/cumotion/cumotionConfig.cmake",
    "content": "# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES.\n#                         All rights reserved.\n# SPDX-License-Identifier: Apache-2.0\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n\n\n####### Expanded from @PACKAGE_INIT@ by configure_package_config_file() #######\n####### Any changes to this file will be overwritten by the next CMake run ####\n####### The input file was CMakeConfig.cmake.in                            ########\n\nget_filename_component(PACKAGE_PREFIX_DIR \"${CMAKE_CURRENT_LIST_DIR}/../../../\" ABSOLUTE)\n\n####################################################################################\n\ninclude(CMakeFindDependencyMacro)\nfind_dependency(Eigen3 3.3)\nfind_dependency(CUDAToolkit)\n\nset(CUDA_PATH ${CUDAToolkit_LIBRARY_ROOT})\nif (${CUDAToolkit_VERSION} VERSION_GREATER_EQUAL 13.0)\n  if(${CMAKE_SYSTEM_PROCESSOR} MATCHES \"aarch64\")\n    set(CCCL_DIR \"${CUDA_PATH}/targets/sbsa-linux/lib/cmake/cccl\")  # for Jetpack 7.0\n  else()\n    set(CCCL_DIR \"${CUDA_PATH}/lib64/cmake/cccl\")\n  endif()\n  find_dependency(CCCL REQUIRED)\nendif()\n\ninclude(${CMAKE_CURRENT_LIST_DIR}/cumotionConfigVersion.cmake)\ninclude(${CMAKE_CURRENT_LIST_DIR}/cumotionTargets.cmake)\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/lib/cmake/cumotion/cumotionConfigVersion.cmake",
    "content": "# This is a basic version file for the Config-mode of find_package().\n# It is used by write_basic_package_version_file() as input file for configure_file()\n# to create a version-file which can be installed along a config.cmake file.\n#\n# The created file sets PACKAGE_VERSION_EXACT if the current version string and\n# the requested version string are exactly the same and it sets\n# PACKAGE_VERSION_COMPATIBLE if the current version is >= requested version,\n# but only if the requested major version is the same as the current one.\n# The variable CVF_VERSION must be set before calling configure_file().\n\n\nset(PACKAGE_VERSION \"1.1.0\")\n\nif(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION)\n  set(PACKAGE_VERSION_COMPATIBLE FALSE)\nelse()\n\n  if(\"1.1.0\" MATCHES \"^([0-9]+)\\\\.\")\n    set(CVF_VERSION_MAJOR \"${CMAKE_MATCH_1}\")\n    if(NOT CVF_VERSION_MAJOR VERSION_EQUAL 0)\n      string(REGEX REPLACE \"^0+\" \"\" CVF_VERSION_MAJOR \"${CVF_VERSION_MAJOR}\")\n    endif()\n  else()\n    set(CVF_VERSION_MAJOR \"1.1.0\")\n  endif()\n\n  if(PACKAGE_FIND_VERSION_RANGE)\n    # both endpoints of the range must have the expected major version\n    math (EXPR CVF_VERSION_MAJOR_NEXT \"${CVF_VERSION_MAJOR} + 1\")\n    if (NOT PACKAGE_FIND_VERSION_MIN_MAJOR STREQUAL CVF_VERSION_MAJOR\n        OR ((PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL \"INCLUDE\" AND NOT PACKAGE_FIND_VERSION_MAX_MAJOR STREQUAL CVF_VERSION_MAJOR)\n          OR (PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL \"EXCLUDE\" AND NOT PACKAGE_FIND_VERSION_MAX VERSION_LESS_EQUAL CVF_VERSION_MAJOR_NEXT)))\n      set(PACKAGE_VERSION_COMPATIBLE FALSE)\n    elseif(PACKAGE_FIND_VERSION_MIN_MAJOR STREQUAL CVF_VERSION_MAJOR\n        AND ((PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL \"INCLUDE\" AND PACKAGE_VERSION VERSION_LESS_EQUAL PACKAGE_FIND_VERSION_MAX)\n        OR (PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL \"EXCLUDE\" AND PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION_MAX)))\n      set(PACKAGE_VERSION_COMPATIBLE TRUE)\n    else()\n      set(PACKAGE_VERSION_COMPATIBLE FALSE)\n    endif()\n  else()\n    if(PACKAGE_FIND_VERSION_MAJOR STREQUAL CVF_VERSION_MAJOR)\n      set(PACKAGE_VERSION_COMPATIBLE TRUE)\n    else()\n      set(PACKAGE_VERSION_COMPATIBLE FALSE)\n    endif()\n\n    if(PACKAGE_FIND_VERSION STREQUAL PACKAGE_VERSION)\n      set(PACKAGE_VERSION_EXACT TRUE)\n    endif()\n  endif()\nendif()\n\n\n# if the installed project requested no architecture check, don't perform the check\nif(\"FALSE\")\n  return()\nendif()\n\n# if the installed or the using project don't have CMAKE_SIZEOF_VOID_P set, ignore it:\nif(\"${CMAKE_SIZEOF_VOID_P}\" STREQUAL \"\" OR \"8\" STREQUAL \"\")\n  return()\nendif()\n\n# check that the installed version has the same 32/64bit-ness as the one which is currently searching:\nif(NOT CMAKE_SIZEOF_VOID_P STREQUAL \"8\")\n  math(EXPR installedBits \"8 * 8\")\n  set(PACKAGE_VERSION \"${PACKAGE_VERSION} (${installedBits}bit)\")\n  set(PACKAGE_VERSION_UNSUITABLE TRUE)\nendif()\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/lib/cmake/cumotion/cumotionTargets-release.cmake",
    "content": "#----------------------------------------------------------------\n# Generated CMake target import file for configuration \"Release\".\n#----------------------------------------------------------------\n\n# Commands may need to know the format version.\nset(CMAKE_IMPORT_FILE_VERSION 1)\n\n# Import target \"cumotion::cumotion\" for configuration \"Release\"\nset_property(TARGET cumotion::cumotion APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE)\nset_target_properties(cumotion::cumotion PROPERTIES\n  IMPORTED_LOCATION_RELEASE \"${_IMPORT_PREFIX}/lib/libcumotion.so.1.1.0\"\n  IMPORTED_SONAME_RELEASE \"libcumotion.so.1\"\n  )\n\nlist(APPEND _cmake_import_check_targets cumotion::cumotion )\nlist(APPEND _cmake_import_check_files_for_cumotion::cumotion \"${_IMPORT_PREFIX}/lib/libcumotion.so.1.1.0\" )\n\n# Commands beyond this point should not need to know the version.\nset(CMAKE_IMPORT_FILE_VERSION)\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/lib/cmake/cumotion/cumotionTargets.cmake",
    "content": "# Generated by CMake\n\nif(\"${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}\" LESS 2.8)\n   message(FATAL_ERROR \"CMake >= 2.8.0 required\")\nendif()\nif(CMAKE_VERSION VERSION_LESS \"2.8.3\")\n   message(FATAL_ERROR \"CMake >= 2.8.3 required\")\nendif()\ncmake_policy(PUSH)\ncmake_policy(VERSION 2.8.3...3.22)\n#----------------------------------------------------------------\n# Generated CMake target import file.\n#----------------------------------------------------------------\n\n# Commands may need to know the format version.\nset(CMAKE_IMPORT_FILE_VERSION 1)\n\n# Protect against multiple inclusion, which would fail when already imported targets are added once more.\nset(_cmake_targets_defined \"\")\nset(_cmake_targets_not_defined \"\")\nset(_cmake_expected_targets \"\")\nforeach(_cmake_expected_target IN ITEMS cumotion::cumotion)\n  list(APPEND _cmake_expected_targets \"${_cmake_expected_target}\")\n  if(TARGET \"${_cmake_expected_target}\")\n    list(APPEND _cmake_targets_defined \"${_cmake_expected_target}\")\n  else()\n    list(APPEND _cmake_targets_not_defined \"${_cmake_expected_target}\")\n  endif()\nendforeach()\nunset(_cmake_expected_target)\nif(_cmake_targets_defined STREQUAL _cmake_expected_targets)\n  unset(_cmake_targets_defined)\n  unset(_cmake_targets_not_defined)\n  unset(_cmake_expected_targets)\n  unset(CMAKE_IMPORT_FILE_VERSION)\n  cmake_policy(POP)\n  return()\nendif()\nif(NOT _cmake_targets_defined STREQUAL \"\")\n  string(REPLACE \";\" \", \" _cmake_targets_defined_text \"${_cmake_targets_defined}\")\n  string(REPLACE \";\" \", \" _cmake_targets_not_defined_text \"${_cmake_targets_not_defined}\")\n  message(FATAL_ERROR \"Some (but not all) targets in this export set were already defined.\\nTargets Defined: ${_cmake_targets_defined_text}\\nTargets not yet defined: ${_cmake_targets_not_defined_text}\\n\")\nendif()\nunset(_cmake_targets_defined)\nunset(_cmake_targets_not_defined)\nunset(_cmake_expected_targets)\n\n\n# Compute the installation prefix relative to this file.\nget_filename_component(_IMPORT_PREFIX \"${CMAKE_CURRENT_LIST_FILE}\" PATH)\nget_filename_component(_IMPORT_PREFIX \"${_IMPORT_PREFIX}\" PATH)\nget_filename_component(_IMPORT_PREFIX \"${_IMPORT_PREFIX}\" PATH)\nget_filename_component(_IMPORT_PREFIX \"${_IMPORT_PREFIX}\" PATH)\nif(_IMPORT_PREFIX STREQUAL \"/\")\n  set(_IMPORT_PREFIX \"\")\nendif()\n\n# Create imported target cumotion::cumotion\nadd_library(cumotion::cumotion SHARED IMPORTED)\n\nset_target_properties(cumotion::cumotion PROPERTIES\n  INTERFACE_INCLUDE_DIRECTORIES \"${_IMPORT_PREFIX}/include\"\n  INTERFACE_LINK_LIBRARIES \"Eigen3::Eigen\"\n)\n\nif(CMAKE_VERSION VERSION_LESS 2.8.12)\n  message(FATAL_ERROR \"This file relies on consumers using CMake 2.8.12 or greater.\")\nendif()\n\n# Load information for each installed configuration.\nfile(GLOB _cmake_config_files \"${CMAKE_CURRENT_LIST_DIR}/cumotionTargets-*.cmake\")\nforeach(_cmake_config_file IN LISTS _cmake_config_files)\n  include(\"${_cmake_config_file}\")\nendforeach()\nunset(_cmake_config_file)\nunset(_cmake_config_files)\n\n# Cleanup temporary variables.\nset(_IMPORT_PREFIX)\n\n# Loop over all imported files and verify that they actually exist\nforeach(_cmake_target IN LISTS _cmake_import_check_targets)\n  foreach(_cmake_file IN LISTS \"_cmake_import_check_files_for_${_cmake_target}\")\n    if(NOT EXISTS \"${_cmake_file}\")\n      message(FATAL_ERROR \"The imported target \\\"${_cmake_target}\\\" references the file\n   \\\"${_cmake_file}\\\"\nbut this file does not exist.  Possible reasons include:\n* The file was deleted, renamed, or moved to another location.\n* An install or uninstall procedure did not complete successfully.\n* The installation package was faulty and contained\n   \\\"${CMAKE_CURRENT_LIST_FILE}\\\"\nbut not all the files it references.\n\")\n    endif()\n  endforeach()\n  unset(_cmake_file)\n  unset(\"_cmake_import_check_files_for_${_cmake_target}\")\nendforeach()\nunset(_cmake_target)\nunset(_cmake_import_check_targets)\n\n# This file does not depend on other imported targets which have\n# been exported from the same project but in a separate export set.\n\n# Commands beyond this point should not need to know the version.\nset(CMAKE_IMPORT_FILE_VERSION)\ncmake_policy(POP)\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/lib/libcumotion.so.1.1.0",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:6977efcb757839a3d992c368d8bb9ef12c4ee728469b567ae3baa26d89717c76\nsize 22457760\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/python_wheels/cumotion-1.1.0-cp312-cp312-linux_aarch64.whl",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:60f6aa4703e201ab9c1b5dd4f78d85c1aa5b1c1dfc83b785f85f66309f766359\nsize 17490116\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/aarch64_jetpack70/python_wheels/cumotion_vis-1.1.0-py3-none-any.whl",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:0dce7c072d865d71fbb2ed44381d812d2601e836d82276341af7d2c39eaf24f2\nsize 11885\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/LICENSE",
    "content": "Copyright (c) 2019-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\nThis software is licensed under the NVIDIA Isaac ROS Software License (see below),\nexcept as otherwise noted in individual file headers.\n\nSee the accompanying NOTICE file for third-party attributions and licenses.\n\n\nNVIDIA ISAAC ROS SOFTWARE LICENSE\n\nThis license is a legal agreement between you and NVIDIA Corporation (\"NVIDIA\") and governs the use of the NVIDIA Isaac ROS software and materials provided hereunder (“SOFTWARE”).\n\nThis license can be accepted only by an adult of legal age of majority in the country in which the SOFTWARE is used.\n\nIf you are entering into this license on behalf of a company or other legal entity, you represent that you have the legal authority to bind the entity to this license, in which case “you” will mean the entity you represent.\n\nIf you don’t have the required age or authority to accept this license, or if you don’t accept all the terms and conditions of this license, do not download, install or use the SOFTWARE.\n\nYou agree to use the SOFTWARE only for purposes that are permitted by (a) this license, and (b) any applicable law, regulation or generally accepted practices or guidelines in the relevant jurisdictions.\n\n1. LICENSE. Subject to the terms of this license, NVIDIA hereby grants you a non-exclusive, non-transferable license, without the right to sublicense (except as expressly provided in this license) to:\na. Install and use the SOFTWARE,\nb. Modify and create derivative works of sample or reference source code delivered in the SOFTWARE, and\nc. Distribute any part of the SOFTWARE (i) as incorporated into a software application that has material additional functionality beyond the included portions of the SOFTWARE, or (ii) unmodified in binary format, in each case subject to the distribution requirements indicated in this license.\n\n2. DISTRIBUTION REQUIREMENTS. These are the distribution requirements for you to exercise the distribution grant above:\n    a. The following notice shall be included in modifications and derivative works of source code distributed: “This software contains source code provided by NVIDIA Corporation.”\n    b. You agree to distribute the SOFTWARE subject to the terms at least as protective as the terms of this license, including (without limitation) terms relating to the license grant, license restrictions and protection of NVIDIA’s intellectual property rights. Additionally, you agree that you will protect the privacy, security and legal rights of your application users.\n    c. You agree to notify NVIDIA in writing of any known or suspected distribution or use of the SOFTWARE not in compliance with the requirements of this license, and to enforce the terms of your agreements with respect to the distributed portions of the SOFTWARE.\n3. AUTHORIZED USERS. You may allow employees and contractors of your entity or of your subsidiary(ies) to access and use the SOFTWARE from your secure network to perform work on your behalf. If you are an academic institution you may allow users enrolled or employed by the academic institution to access and use the SOFTWARE from your secure network. You are responsible for the compliance with the terms of this license by your authorized users.\n\n4. LIMITATIONS. Your license to use the SOFTWARE is restricted as follows:\n    a. The SOFTWARE is licensed for you to develop applications only for their use in systems with NVIDIA GPUs.\n    b. You may not reverse engineer, decompile or disassemble, or remove copyright or other proprietary notices from any portion of the SOFTWARE or copies of the SOFTWARE.\n    c. Except as expressly stated above in this license, you may not sell, rent, sublicense, transfer, distribute, modify, or create derivative works of any portion of the SOFTWARE.\n    d.  Unless you have an agreement with NVIDIA for this purpose, you may not indicate that an application created with the SOFTWARE is sponsored or endorsed by NVIDIA.\n    e.  You may not bypass, disable, or circumvent any technical limitation, encryption, security, digital rights management or authentication mechanism in the SOFTWARE.\n    f. You may not use the SOFTWARE in any manner that would cause it to become subject to an open source software license. As examples, licenses that require as a condition of use, modification, and/or distribution that the SOFTWARE be: (i) disclosed or distributed in source code form; (ii) licensed for the purpose of making derivative works; or (iii) redistributable at no charge.\n    g.  You acknowledge that the SOFTWARE as delivered is not tested or certified by NVIDIA for use in connection with the design, construction, maintenance, and/or operation of any system where the use or failure of such system could result in a situation that threatens the safety of human life or results in catastrophic damages (each, a \"Critical Application\"). Examples of Critical Applications include use in avionics, navigation, autonomous vehicle applications, ai solutions for automotive products, military, medical, life support or other life critical applications. NVIDIA shall not be liable to you or any third party, in whole or in part, for any claims or damages arising from such uses. You are solely responsible for ensuring that any product or service developed with the SOFTWARE as a whole includes sufficient features to comply with all applicable legal and regulatory standards and requirements.\n    h. You agree to defend, indemnify and hold harmless NVIDIA and its affiliates, and their respective employees, contractors, agents, officers and directors, from and against any and all claims, damages, obligations, losses, liabilities, costs or debt, fines, restitutions and expenses (including but not limited to attorney’s fees and costs incident to establishing the right of indemnification) arising out of or related to your use of goods and/or services that include or utilize the SOFTWARE, or for use of the SOFTWARE outside of the scope of this license or not in compliance with its terms.\n\n5. UPDATES. NVIDIA may, at its option, make available patches, workarounds or other updates to this SOFTWARE. Unless the updates are provided with their separate governing terms, they are deemed part of the SOFTWARE licensed to you as provided in this license.\n\n6. PRE-RELEASE VERSIONS. SOFTWARE versions identified as alpha, beta, preview, early access or otherwise as pre-release may not be fully functional, may contain errors or design flaws, and may have reduced or different security, privacy, availability, and reliability standards relative to commercial versions of NVIDIA software and materials. You may use a pre-release SOFTWARE version at your own risk, understanding that these versions are not intended for use in production or business-critical systems.\n\n7. COMPONENTS UNDER OTHER LICENSES. The SOFTWARE may include NVIDIA or third-party components with separate legal notices or terms as may be described in proprietary notices accompanying the SOFTWARE, such as components governed by open source software licenses. If and to the extent there is a conflict between the terms in this license and the license terms associated with a component, the license terms associated with the component controls only to the extent necessary to resolve the conflict.\n\n8. OWNERSHIP.\n\n8.1 NVIDIA reserves all rights, title and interest in and to the SOFTWARE not expressly granted to you under this license. NVIDIA and its suppliers hold all rights, title and interest in and to the SOFTWARE, including their respective intellectual property rights. The SOFTWARE is copyrighted and protected by the laws of the United States and other countries, and international treaty provisions.\n\n8.2 Subject to the rights of NVIDIA and its suppliers in the SOFTWARE, you hold all rights, title and interest in and to your applications and your derivative works of the sample or reference source code delivered in the SOFTWARE including their respective intellectual property rights. With respect to source code samples or reference source code licensed to you, NVIDIA and its affiliates are free to continue independently developing source code samples and you covenant not to sue NVIDIA, its affiliates or their licensees with respect to later versions of NVIDIA released source code.\n\n9. FEEDBACK. You may, but are not obligated to, provide to NVIDIA Feedback. “Feedback” means suggestions, fixes, modifications, feature requests or other feedback regarding the SOFTWARE. Feedback, even if designated as confidential by you, shall not create any confidentiality obligation for NVIDIA. NVIDIA and its designees have a perpetual, non-exclusive, worldwide, irrevocable license to use, reproduce, publicly display, modify, create derivative works of, license, sublicense, and otherwise distribute and exploit Feedback as NVIDIA sees fit without payment and without obligation or restriction of any kind on account of intellectual property rights or otherwise.\n\n10. NO WARRANTIES. THE SOFTWARE IS PROVIDED AS-IS. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW NVIDIA AND ITS AFFILIATES EXPRESSLY DISCLAIM ALL WARRANTIES OF ANY KIND OR NATURE, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, OR FITNESS FOR A PARTICULAR PURPOSE. NVIDIA DOES NOT WARRANT THAT THE SOFTWARE WILL MEET YOUR REQUIREMENTS OR THAT THE OPERATION THEREOF WILL BE UNINTERRUPTED OR ERROR-FREE, OR THAT ALL ERRORS WILL BE CORRECTED.\n\n11. LIMITATIONS OF LIABILITY. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW NVIDIA AND ITS AFFILIATES SHALL NOT BE LIABLE FOR ANY SPECIAL, INCIDENTAL, PUNITIVE OR CONSEQUENTIAL DAMAGES, OR FOR ANY LOST PROFITS, PROJECT DELAYS, LOSS OF USE, LOSS OF DATA OR LOSS OF GOODWILL, OR THE COSTS OF PROCURING SUBSTITUTE PRODUCTS, ARISING OUT OF OR IN CONNECTION WITH THIS LICENSE OR THE USE OR PERFORMANCE OF THE SOFTWARE, WHETHER SUCH LIABILITY ARISES FROM ANY CLAIM BASED UPON BREACH OF CONTRACT, BREACH OF WARRANTY, TORT (INCLUDING NEGLIGENCE), PRODUCT LIABILITY OR ANY OTHER CAUSE OF ACTION OR THEORY OF LIABILITY, EVEN IF NVIDIA HAS PREVIOUSLY BEEN ADVISED OF, OR COULD REASONABLY HAVE FORESEEN, THE POSSIBILITY OF SUCH DAMAGES. IN NO EVENT WILL NVIDIA’S AND ITS AFFILIATES TOTAL CUMULATIVE LIABILITY UNDER OR ARISING OUT OF THIS LICENSE EXCEED US$10.00. THE NATURE OF THE LIABILITY OR THE NUMBER OF CLAIMS OR SUITS SHALL NOT ENLARGE OR EXTEND THIS LIMIT.\n\n12. TERMINATION. Your rights under this license will terminate automatically without notice from NVIDIA if you fail to comply with any term and condition of this license or if you commence or participate in any legal proceeding against NVIDIA with respect to the SOFTWARE. NVIDIA may terminate this license with advance written notice to you, if NVIDIA decides to no longer provide the SOFTWARE in a country or, in NVIDIA’s sole discretion, the continued use of it is no longer commercially viable. Upon any termination of this license, you agree to promptly discontinue use of the SOFTWARE and destroy all copies in your possession or control. Your prior distributions in accordance with this license are not affected by the termination of this license. All provisions of this license will survive termination, except for the license granted to you.\n\n13. APPLICABLE LAW. This license will be governed in all respects by the laws of the United States and of the State of Delaware, without regard to the conflicts of laws principles. The United Nations Convention on Contracts for the International Sale of Goods is specifically disclaimed. You agree to all terms of this license in the English language. The state or federal courts residing in Santa Clara County, California shall have exclusive jurisdiction over any dispute or claim arising out of this license. Notwithstanding this, you agree that NVIDIA shall still be allowed to apply for injunctive remedies or urgent legal relief in any jurisdiction.\n\n14. NO ASSIGNMENT. This license and your rights and obligations thereunder may not be assigned by you by any means or operation of law without NVIDIA’s permission. Any attempted assignment not approved by NVIDIA in writing shall be void and of no effect. NVIDIA may assign, delegate or transfer this license and its rights and obligations, and if to a non-affiliate you will be notified.\n\n15. EXPORT. The SOFTWARE is subject to United States export laws and regulations. You agree to comply with all applicable U.S. and international export laws, including the Export Administration Regulations (EAR) administered by the U.S. Department of Commerce and economic sanctions administered by the U.S. Department of Treasury’s Office of Foreign Assets Control (OFAC). These laws include restrictions on destinations, end-users and end-use. By accepting this license, you confirm that you are not currently residing in a country or region currently embargoed by the U.S. and that you are not otherwise prohibited from receiving the SOFTWARE.\n\n16. GOVERNMENT USE. The SOFTWARE is, and shall be treated as being, “Commercial Items” as that term is defined at 48 CFR § 2.101, consisting of “commercial computer software” and “commercial computer software documentation”, respectively, as such terms are used in, respectively, 48 CFR § 12.212 and 48 CFR §§ 227.7202 & 252.227-7014(a)(1). Use, duplication or disclosure by the U.S. Government or a U.S. Government subcontractor is subject to the restrictions in this license pursuant to 48 CFR § 12.212 or 48 CFR § 227.7202. In no event shall the US Government user acquire rights in the SOFTWARE beyond those specified in 48 C.F.R. 52.227-19(b)(1)-(2).\n\n17. NOTICES. Please direct your legal notices or other correspondence to NVIDIA Corporation, 2788 San Tomas Expressway, Santa Clara, California 95051, United States of America, Attention: Legal Department.\n\n18. ENTIRE AGREEMENT. This license is the final, complete and exclusive agreement between the parties relating to the subject matter of this license and supersedes all prior or contemporaneous understandings and agreements relating to this subject matter, whether oral or written. If any court of competent jurisdiction determines that any provision of this license is illegal, invalid or unenforceable, the remaining provisions will remain in full force and effect. Any amendment or waiver under this license shall be in writing and signed by representatives of both parties.\n\n(v. November 17, 2021)\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/NOTICE",
    "content": "cuMotion is distributed with the third-party components listed below.\n\nIn addition, installing the cuMotion Python package via pip will download and\ninstall additional third-party open source software projects. Review the\nlicense terms of those projects before use.\n\n\npybind11 3.0.1\n==============\n\nThe cuMotion Python package leverages pybind11 for its Python bindings.\n\n   Site: https://github.com/pybind/pybind11\nLicense: https://github.com/pybind/pybind11/blob/v3.0.1/LICENSE\n\nCopyright (c) 2016 Wenzel Jakob <wenzel.jakob@epfl.ch>, All rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\n   list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\n   this list of conditions and the following disclaimer in the documentation\n   and/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its contributors\n   may be used to endorse or promote products derived from this software\n   without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\nANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\nWARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n\nyaml-cpp 0.7.0\n==============\n\ncuMotion statically links against the yaml-cpp library, without modification.\n\n   Site: https://github.com/jbeder/yaml-cpp\nLicense: https://github.com/jbeder/yaml-cpp/blob/yaml-cpp-0.7.0/LICENSE\n\nCopyright (c) 2008-2015 Jesse Beder.\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in\nall copies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN\nTHE SOFTWARE.\n\n\nurdfdom 3.0.1\n=============\n\ncuMotion statically links against the urdfdom library, without modification.\n\n   Site: https://github.com/ros/urdfdom\nLicense: https://github.com/ros/urdfdom/blob/3.0.1/LICENSE\n\nCopyright (c) 2008, Willow Garage, Inc.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions\nare met:\n\n * Redistributions of source code must retain the above copyright\n   notice, this list of conditions and the following disclaimer.\n * Redistributions in binary form must reproduce the above\n   copyright notice, this list of conditions and the following\n   disclaimer in the documentation and/or other materials provided\n   with the distribution.\n * Neither the name of the Willow Garage nor the names of its\n   contributors may be used to endorse or promote products derived\n   from this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\nFOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\nCOPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\nINCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\nBUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\nLOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\nLIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\nANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGE.\n\n\nTinyXML 2.6.2\n=============\n\ncuMotion statically links against the TinyXML library, without modification.\n\n   Site: https://sourceforge.net/projects/tinyxml/\nLicense: https://sourceforge.net/p/tinyxml/git/ci/master/tree/readme.txt\n\nTinyXML is released under the zlib license:\n\nThis software is provided 'as-is', without any express or implied\nwarranty. In no event will the authors be held liable for any\ndamages arising from the use of this software.\n\nPermission is granted to anyone to use this software for any\npurpose, including commercial applications, and to alter it and\nredistribute it freely, subject to the following restrictions:\n\n1. The origin of this software must not be misrepresented; you must\nnot claim that you wrote the original software. If you use this\nsoftware in a product, an acknowledgment in the product documentation\nwould be appreciated but is not required.\n\n2. Altered source versions must be plainly marked as such, and\nmust not be misrepresented as being the original software.\n\n3. This notice may not be removed or altered from any source\ndistribution.\n\n\nconsole_bridge 1.0.1\n====================\n\ncuMotion statically links against the console_bridge library, without\nmodification.\n\n   Site: https://github.com/ros/console_bridge\nLicense: https://github.com/ros/console_bridge/blob/1.0.1/LICENSE\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n   * Redistributions of source code must retain the above copyright\n     notice, this list of conditions and the following disclaimer.\n\n   * Redistributions in binary form must reproduce the above copyright\n     notice, this list of conditions and the following disclaimer in the\n     documentation and/or other materials provided with the distribution.\n\n   * Neither the name of the copyright holder nor the names of its\n     contributors may be used to endorse or promote products derived from\n     this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\nARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\nLIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\nCONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\nSUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\nINTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\nCONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\nARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGE.\n\n\nur_description 1.2.7 and 1.3.0\n==============================\n\nThe URDF and mesh files in content/third_party/universal_robots/ are derived\nfrom those found in the \"universal_robot\" package of the ROS-Industrial\nproject.\n\nSite: http://wiki.ros.org/universal_robot\n\nCopyright 2013-2020 Felix Messmer, Kelsey Hawkins, Shaun Edwards,\n    Stuart Glaser, Wim Meeussen, and Contributors\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\nlist of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\nthis list of conditions and the following disclaimer in the documentation\nand/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its contributors\nmay be used to endorse or promote products derived from this software without\nspecific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\nANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\nWARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n\nros2_robotiq_gripper/robotiq_description\n========================================\n\nThe URDF and mesh files in content/third_party/robotiq/ are derived from those\nfound in the \"ros2_robotiq_gripper\" package provided by PickNik Robotics.\n\n   Site: https://github.com/PickNikRobotics/ros2_robotiq_gripper\nLicense: https://github.com/PickNikRobotics/ros2_robotiq_gripper/blob/12e62321/LICENSE\n\nCopyright (c) 2022, PickNik Robotics\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\n   list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\n   this list of conditions and the following disclaimer in the documentation\n   and/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its\n   contributors may be used to endorse or promote products derived from\n   this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n\nros-industrial/fanuc 0.5.1\n==========================\n\nThe URDF and mesh files in content/third_party/fanuc/ are derived from those\nfound in the \"fanuc\" package of the ROS-Industrial project.\n\n   Site: http://wiki.ros.org/fanuc\nLicense: https://github.com/ros-industrial/fanuc/blob/0.5.1/LICENSE\n\nCopyright (c) 2012-2015, TU Delft Robotics Institute\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions\nare met:\n\n * Redistributions of source code must retain the above copyright\n   notice, this list of conditions and the following disclaimer.\n * Redistributions in binary form must reproduce the above\n   copyright notice, this list of conditions and the following\n   disclaimer in the documentation and/or other materials provided\n   with the distribution.\n * Neither the name of the TU Delft Robotics Institute nor the names\n   of its contributors may be used to endorse or promote products\n   derived from this software without specific prior written\n   permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n\"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\nLIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\nFOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\nCOPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\nINCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\nBUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\nLOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\nLIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\nANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGE.\n\n\nunitree_ros (g1_description)\n============================\n\nThe URDF and mesh files in content/third_party/unitree/ are derived from those\nfound in the \"g1_description\" package provided by Unitree Robotics.\n\n   Site: https://github.com/unitreerobotics/unitree_ros/\nLicense: https://github.com/unitreerobotics/unitree_ros/blob/ad5c7db5/LICENSE\n\nCopyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. (\"Unitree Robotics\")\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n* Redistributions of source code must retain the above copyright notice, this\n  list of conditions and the following disclaimer.\n\n* Redistributions in binary form must reproduce the above copyright notice,\n  this list of conditions and the following disclaimer in the documentation\n  and/or other materials provided with the distribution.\n\n* Neither the name of the copyright holder nor the names of its\n  contributors may be used to endorse or promote products derived from\n  this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n\nfranka_ros 0.7.0\n================\n\nThe Franka mesh files in content/third_party/franka/meshes/ were converted\n(with minor modifications) from those in the \"franka_description\" component\nof the franka_ros package.\n\nSite:    https://github.com/frankaemika/franka_ros/\nLicense: https://github.com/frankaemika/franka_ros/blob/0.7.0/NOTICE\n         https://github.com/frankaemika/franka_ros/blob/0.7.0/LICENSE\n\nCopyright 2017 Franka Emika GmbH\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nyou may not use this file except in compliance with the License.\nYou may obtain a copy of the License at\n\n    http://www.apache.org/licenses/LICENSE-2.0\n\nUnless required by applicable law or agreed to in writing, software\ndistributed under the License is distributed on an \"AS IS\" BASIS,\nWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\nSee the License for the specific language governing permissions and\nlimitations under the License.\n\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/collision_free_ik_solver.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n#include <optional>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/rotation3.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Configuration parameters for a `CollisionFreeIkSolver`.\nclass CUMO_EXPORT CollisionFreeIkSolverConfig {\n public:\n  virtual ~CollisionFreeIkSolverConfig() = default;\n\n  //! Specify the value for a given parameter.\n  //!\n  //! The required `ParamValue` constructor for each param is detailed in the\n  //! documentation for `setParam()`.\n  struct CUMO_EXPORT ParamValue {\n    //! Create `ParamValue` from `int`.\n    ParamValue(int value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `double`.\n    ParamValue(double value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Set the value of the parameter.\n  //!\n  //! `setParam()` returns `true` if the parameter has been successfully updated and `false` if an\n  //! error has occurred (e.g., invalid parameter).\n  //!\n  //! The following parameters can be set for `CollisionFreeIkSolver`:\n  //!\n  //! `task_space_position_tolerance` [`double`]\n  //!   - Maximum allowable violation of the user-specified constraint on the position of the tool\n  //!     frame.\n  //!   - It determines when a solution is considered to have satisfied position targets.\n  //!     Smaller values require more precise solutions but may be harder to achieve.\n  //!   - Units are in meters.\n  //!   - A default value of 1e-3 (1mm) is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `task_space_orientation_tolerance` [`double`]\n  //!   - Maximum allowable violation of the user-specified constraint on the orientation of the\n  //!     tool frame.\n  //!   - It determines when a solution is considered to have satisfied orientation targets.\n  //!     Smaller values require more precise solutions but may be harder to achieve.\n  //!   - Units are in radians.\n  //!   - A default value of 5e-3 (approximately 0.29 degrees) is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `num_seeds` [`int`]\n  //!   - Number of seeds used to solve each inverse kinematics (IK) problem.\n  //!   - The IK solver generates multiple pseudo-random c-space configurations per problem and\n  //!     optimizes them to find diverse collision-free configurations for the desired tool pose.\n  //!     Higher values increase the likelihood of finding valid solutions but increase\n  //!     computational cost.\n  //!   - A default value of 32 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `max_reattempts` [`int`]\n  //!   - Maximum number of times to restart the IK problem with different random seeds, in case of\n  //!     failure, before giving up.\n  //!   - Higher values increase the likelihood of finding valid IK solutions but increase\n  //!     memory usage and the maximum possible time to find a solution. A value of 0 means no\n  //!     retries (i.e. only perform the initial attempt).\n  //!   - A default value of 10 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!   - NOTE: This parameter is not currently used for batch (multiple problems) planning.\n  //!\n  //! `lbfgs/max_iterations` [`int`]\n  //!   - Maximum number of L-BFGS iterations for the inverse kinematics solver.\n  //!   - Higher values allow the IK solver to converge more precisely but increase computational\n  //!     cost per IK attempt.\n  //!   - A default value of 100 is recommended for most use-cases.\n  //!   - Must be positive.\n  [[nodiscard]] virtual bool setParam(const std::string &param_name, ParamValue value) = 0;\n};\n\n//! Use default parameters to create a configuration for collision-free inverse kinematics.\n//!\n//! These default parameters are combined with `robot_description`, `tool_frame_name`, and\n//! `world_view`.\n//!\n//! A configuration will *NOT* be created if:\n//!   1. `robot_description` is invalid, *OR*\n//!   2. `tool_frame_name` is not a valid frame in `robot_description`.\n//!\n//! In the case of failure, a non-fatal error will be logged and a `nullptr` will be returned.\nCUMO_EXPORT std::unique_ptr<CollisionFreeIkSolverConfig>\nCreateDefaultCollisionFreeIkSolverConfig(const RobotDescription &robot_description,\n                                         const std::string &tool_frame_name,\n                                         const WorldViewHandle &world_view);\n\n//! Interface for using numerical optimization to generate collision-free c-space positions for\n//! task-space targets.\nclass CUMO_EXPORT CollisionFreeIkSolver {\n public:\n  virtual ~CollisionFreeIkSolver() = default;\n\n  //! Translation constraints restrict the position of the origin of a tool frame.\n  class CUMO_EXPORT TranslationConstraint {\n   public:\n    //! Create a `TranslationConstraint` such that the desired position is fully specified as\n    //! `translation_target`.\n    //!\n    //! The optional parameter `deviation_limit` can be used to allow deviation from the\n    //! `translation_target`. This limit specifies the maximum allowable deviation from the desired\n    //! position.\n    //!\n    //! If `deviation_limit` is not input, then the deviation limit is assumed to be zero\n    //! (i.e., it is desired that the tool frame position be exactly `translation_target`).\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `deviation_limit` is negative.\n    //!\n    //! NOTE: The `translation_target` is specified in world frame coordinates (i.e., relative to\n    //! the base of the robot).\n    static TranslationConstraint Target(const Eigen::Vector3d &translation_target,\n                                        std::optional<double> deviation_limit = std::nullopt);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `TranslationConstraint` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of `TranslationConstraint`s are considered concurrently. Each\n  //! `TranslationConstraint` in the goalset must have the same mode (e.g., \"target\") but may have\n  //! different data for each `TranslationConstraint`.\n  //!\n  //! DEPRECATED: This class is deprecated and will be removed in a future version. Use\n  //! `TranslationConstraintArray` instead.\n  class CUMO_EXPORT TranslationConstraintGoalset {\n   public:\n    //! Create a `TranslationConstraintGoalset` such that `translation_targets` are fully specified.\n    //!\n    //! See `TranslationConstraint::Target()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `TranslationConstraint::Target()` is not met, *OR*\n    //!   2. `translation_targets` is empty.\n    static TranslationConstraintGoalset Target(\n      const std::vector<Eigen::Vector3d> &translation_targets,\n      std::optional<double> deviation_limit = std::nullopt);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Jagged two-dimensional (2D) array of `TranslationConstraint`s for solving IK problem(s).\n  //!\n  //! The outer dimension of the array represents the number of problems to solve, and the inner\n  //! dimension represents the number of constraints for each problem, which may vary in size across\n  //! problems.  All constraints must have the same mode (e.g., \"target\") but may have different\n  //! data for each constraint.\n  //!\n  //! NOTE: The current version supports either multiple constraints for a single problem (goalset\n  //! planning) or a single constraint for multiple problems (single-target batch planning).\n  //! However, using multiple constraints for multiple problems (goalset batch planning) is not\n  //! currently supported.\n  class CUMO_EXPORT TranslationConstraintArray {\n   public:\n    //! Create a `TranslationConstraintArray` such that `translation_targets` are fully specified.\n    //!\n    //! Each element in the outer vector represents a separate problem, and each element in the\n    //! inner vector represents a separate constraint for that problem.\n    //!\n    //! See `TranslationConstraint::Target()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `TranslationConstraint::Target()` is not met,\n    //!   2. `translation_targets` is empty,\n    //!   3. `translation_targets[i]` is empty for any problem, *OR*\n    //!   4. The size of `translation_targets` is greater than `1`, and the size of any of its inner\n    //!      vectors is greater than `1`. (This restriction will be removed when support for\n    //!      \"goalset batch IK\" is added.)\n    static TranslationConstraintArray Target(\n      const std::vector<std::vector<Eigen::Vector3d>> &translation_targets,\n      std::optional<double> deviation_limit = std::nullopt);\n\n    //! Return the number of problems in the array.\n    [[nodiscard]] int numProblems() const;\n\n    //! Return the number of constraints for a specific problem in the array.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `problem_index` is out of bounds.\n    [[nodiscard]] int numConstraints(int problem_index) const;\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Orientation constraints restrict the orientation of a tool frame.\n  //!\n  //! Each constraint may fully or partially constrain the orientation.\n  class CUMO_EXPORT OrientationConstraint {\n   public:\n    //! Create an `OrientationConstraint` such that no tool frame orientation constraints are\n    //! active.\n    static OrientationConstraint None();\n\n    //! Create an `OrientationConstraint` such that a tool frame `orientation_target` is fully\n    //! specified.\n    //!\n    //! The optional parameter `deviation_limit` can be used to allow deviation from the\n    //! `orientation_target`. If input, `deviation_limit` is expressed in radians. This limit\n    //! specifies the maximum allowable deviation from the desired orientation.\n    //!\n    //! If `deviation_limit` is not input, then the deviation limit is assumed to be zero (i.e., it\n    //! is desired that orientation be exactly `orientation_target`).\n    //!\n    //! In general, it is suggested that the `deviation_limit` be set to a value less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `deviation_limit` is negative.\n    //!\n    //! NOTE: The `orientation_target` is specified in world frame coordinates (i.e., relative to\n    //! the base of the robot).\n    static OrientationConstraint Target(const Rotation3 &orientation_target,\n                                        std::optional<double> deviation_limit = std::nullopt);\n\n    //! Create an `OrientationConstraint` such that the tool frame orientation is constrained to\n    //! rotate about a \"free axis\".\n    //!\n    //! The \"free axis\" for rotation is defined by a `tool_frame_axis` (specified in the tool frame\n    //! coordinates) and a corresponding `world_target_axis` (specified in world frame coordinates).\n    //!\n    //! The optional `axis_deviation_limit` can be used to allow deviation from the\n    //! desired axis alignment. If input, `axis_deviation_limit` is expressed in radians and the\n    //! limit specifies the maximum allowable deviation of the rotation axis. If\n    //! `axis_deviation_limit` is not input, then the deviation limit is assumed to be zero\n    //! (i.e., it is desired that the tool frame axis be exactly aligned with `world_target_axis`).\n    //!\n    //! In general, it is suggested that the `axis_deviation_limit` be set to a value less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `axis_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `axis_deviation_limit` is negative,\n    //!   2. `tool_frame_axis` is (nearly) zero, *OR*\n    //!   3. `world_target_axis` is (nearly) zero.\n    //!\n    //! NOTE: `tool_frame_axis` and `world_target_axis` inputs will be normalized.\n    static OrientationConstraint Axis(const Eigen::Vector3d &tool_frame_axis,\n                                      const Eigen::Vector3d &world_target_axis,\n                                      std::optional<double> axis_deviation_limit = std::nullopt);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `OrientationConstraint` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of `OrientationConstraint`s are considered concurrently. Each\n  //! `OrientationConstraint` in the goalset must have the same mode (e.g., \"full target\") but\n  //! may have different data for each `OrientationConstraint`.\n  //!\n  //! DEPRECATED: This class is deprecated and will be removed in a future version. Use\n  //! `OrientationConstraintArray` instead.\n  class CUMO_EXPORT OrientationConstraintGoalset {\n   public:\n    //! Create an `OrientationConstraintGoalset` such that no tool frame orientation constraints\n    //! are active.\n    static OrientationConstraintGoalset None();\n\n    //! Create an `OrientationConstraintGoalset` such that tool frame `orientation_targets` are\n    //! fully specified.\n    //!\n    //! See `OrientationConstraint::Target()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::Target()` is not met, *OR*\n    //!   2. `orientation_targets` is empty.\n    static OrientationConstraintGoalset Target(\n        const std::vector<Rotation3> &orientation_targets,\n        std::optional<double> deviation_limit = std::nullopt);\n\n    //! Create an `OrientationConstraintGoalset` such that the tool frame orientation is constrained\n    //! to rotate about a \"free axis\".\n    //!\n    //! See `OrientationConstraint::Axis()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::Axis()` is not met,\n    //!   2. `tool_frame_axes` and `world_target_axes` do not have the same number of elements, *OR*\n    //!   3. `tool_frame_axes` and `world_target_axes` are empty.\n    static OrientationConstraintGoalset Axis(\n        const std::vector<Eigen::Vector3d> &tool_frame_axes,\n        const std::vector<Eigen::Vector3d> &world_target_axes,\n        std::optional<double> axis_deviation_limit = std::nullopt);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Jagged 2D array of `OrientationConstraint`s for solving IK problem(s).\n  //!\n  //! The outer dimension of the array represents the number of problems to solve, and the inner\n  //! dimension represents the number of constraints for each problem, which may vary in size across\n  //! problems.  All constraints must have the same mode (e.g., \"full target\") but may have\n  //! different data for each constraint.\n  //!\n  //! NOTE: The current version supports either multiple constraints for a single problem (goalset\n  //! planning) or a single constraint for multiple problems (single-target batch planning).\n  //! However, using multiple constraints for multiple problems (goalset batch planning) is not\n  //! currently supported.\n  class CUMO_EXPORT OrientationConstraintArray {\n   public:\n    //! Create an `OrientationConstraintArray` such that no tool frame orientation constraints\n    //! are active.\n    static OrientationConstraintArray None();\n\n    //! Create an `OrientationConstraintArray` such that tool frame `orientation_targets` are\n    //! fully specified.\n    //!\n    //! Each element in the outer vector represents a separate problem, and each element in the\n    //! inner vector represents a separate constraint for that problem.\n    //!\n    //! See `OrientationConstraint::Target()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::Target()` is not met,\n    //!   2. `orientation_targets` is empty,\n    //!   3. `orientation_targets[i]` is empty for any problem, *OR*\n    //!   4. The size of `orientation_targets` is greater than `1`, and the size of any of its inner\n    //!      vectors is greater than `1`. (This restriction will be removed when support for\n    //!      \"goalset batch IK\" is added.)\n    static OrientationConstraintArray Target(\n        const std::vector<std::vector<Rotation3>> &orientation_targets,\n        std::optional<double> deviation_limit = std::nullopt);\n\n    //! Create an `OrientationConstraintArray` such that the tool frame orientation is constrained\n    //! to rotate about a \"free axis\".\n    //!\n    //! Each element in the outer vector represents a separate problem, and each element in the\n    //! inner vector represents a separate constraint for that problem.\n    //!\n    //! See `OrientationConstraint::Axis()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::Axis()` is not met,\n    //!   2. `tool_frame_axes` and `world_target_axes` do not have the same number of elements,\n    //!   3. `tool_frame_axes[i]` and `world_target_axes[i]` do not have the same number of elements\n    //!      for each problem,\n    //!   4. `tool_frame_axes` and `world_target_axes` are empty,\n    //!   5. `tool_frame_axes[i]` and `world_target_axes[i]` are empty for any problem, *OR*\n    //!   6. The size of `tool_frame_axes` and `world_target_axes` are greater than `1`, and the\n    //!      size of any of their inner vectors is greater than `1`. (This restriction will be\n    //!      removed when support for \"goalset batch IK\" is added.)\n\n    static OrientationConstraintArray Axis(\n        const std::vector<std::vector<Eigen::Vector3d>> &tool_frame_axes,\n        const std::vector<std::vector<Eigen::Vector3d>> &world_target_axes,\n        std::optional<double> axis_deviation_limit = std::nullopt);\n\n    //! Return the number of problems in the array.\n    //!\n    //! If the array is unconstrained (i.e., created using `None()`), then `nullopt` will be\n    //! returned.\n    [[nodiscard]] std::optional<int> numProblems() const;\n\n    //! Return the number of constraints for a specific problem in the array.\n    //!\n    //! If the array is unconstrained (i.e., created using `None()`), then `nullopt` will be\n    //! returned for any `problem_index`. Otherwise, the number of constraints for the specified\n    //! problem will be returned.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. The array is constrained (i.e., *NOT* created using `None()`) and `problem_index` is\n    //!      out of bounds.\n    [[nodiscard]] std::optional<int> numConstraints(int problem_index) const;\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Task-space targets restrict the position and (optionally) orientation of the tool frame.\n  struct CUMO_EXPORT TaskSpaceTarget {\n    //! Create a task-space target.\n    explicit TaskSpaceTarget(const TranslationConstraint &translation_constraint,\n                             const OrientationConstraint &orientation_constraint =\n                                 OrientationConstraint::None());\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `TaskSpaceTarget` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of pose constraints are considered concurrently. Each pose\n  //! constraint in the goalset must have the same mode (e.g., \"position target and no orientation\n  //! constraints\") but may have different data for each constraint.\n  //!\n  //! DEPRECATED: This class is deprecated and will be removed in a future version. Use\n  //! `TaskSpaceTargetArray` instead.\n  struct CUMO_EXPORT TaskSpaceTargetGoalset {\n    //! Create a task-space target goalset.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. The number of `translation_constraints` does not match the number of\n    //!      `orientation_constraints`.\n    explicit TaskSpaceTargetGoalset(const TranslationConstraintGoalset &translation_constraints,\n                                    const OrientationConstraintGoalset &orientation_constraints =\n                                        OrientationConstraintGoalset::None());\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Jagged 2D array of `TaskSpaceTarget`s for solving IK problem(s).\n  //!\n  //! The outer dimension of the array represents the number of problems to solve, and the inner\n  //! dimension represents the number of targets for each problem, which may vary in size across\n  //! problems.  All task-space targets must have the same mode (e.g., \"position target and full\n  //! orientation constraint\") but may have different data for each target.\n  //!\n  //! NOTE: The current version supports either multiple targets for a single problem (goalset\n  //! planning) or a single target for multiple problems (single-target batch planning).\n  //! However, using multiple targets for multiple problems (goalset batch planning) is not\n  //! currently supported.\n  struct CUMO_EXPORT TaskSpaceTargetArray {\n    //! Create a task-space target array.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. The number of problems in `translation_constraints` does not match the number of\n    //!      problems in `orientation_constraints`, *OR*\n    //!   2. The number of constraints for any problem in `translation_constraints` does not\n    //!      match the number of constraints for that same problem in `orientation_constraints`.\n    //!\n    //! NOTE: Conditions 1 and 2 are not checked when `orientation_constraints` is unconstrained\n    //! (i.e., `OrientationConstraintArray::None()`). Unconstrained orientation constraints can be\n    //! paired with translation constraints of any size.\n    explicit TaskSpaceTargetArray(const TranslationConstraintArray &translation_constraints,\n                                  const OrientationConstraintArray &orientation_constraints =\n                                      OrientationConstraintArray::None());\n\n    //! Return the number of problems in the array.\n    [[nodiscard]] int numProblems() const;\n\n    //! Return the number of task-space targets for a specific problem in the array.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `problem_index` is out of bounds.\n    [[nodiscard]] int numTargets(int problem_index) const;\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Results from an inverse kinematics solve.\n  class CUMO_EXPORT Results {\n   public:\n    //! Indicate the success or failure of the inverse kinematics solve.\n    enum class Status {\n      //! One or more valid c-space positions found.\n      SUCCESS,\n\n      //! The inverse kinematics solver failed to find a valid solution.\n      INVERSE_KINEMATICS_FAILURE,\n    };\n\n    virtual ~Results() = default;\n\n    //! Return the `Status` from the inverse kinematics solve.\n    [[nodiscard]] virtual Status status() const = 0;\n\n    //! If `status()` returns `SUCCESS`, then `cSpacePositions()` returns unique IK solutions.\n    //!\n    //! A `SUCCESS` status indicates that at least one IK solution was found. Returned\n    //! solutions are ordered from the lowest cost to the highest cost.\n    //!\n    //! If `status()` returns `INVERSE_KINEMATICS_FAILURE`, then an empty vector will be returned.\n    [[nodiscard]] virtual std::vector<Eigen::VectorXd> cSpacePositions() const = 0;\n\n    //! Return the indices of the targets selected for each valid c-space position.\n    //!\n    //! For goalset planning (e.g., `solveGoalset()`), returned values represent indices into\n    //! the constraint vectors used to create the goalset (e.g., `TaskSpaceTargetGoalset`).\n    //!\n    //! For single target planning (e.g., `solve()`), zero will be returned for each valid\n    //! c-space position in `cSpacePositions()`.\n    //!\n    //! In all cases, the length of returned `targetIndices()` will be equal to the length of\n    //! returned `cSpacePositions()`.\n    [[nodiscard]] virtual std::vector<int> targetIndices() const = 0;\n  };\n\n  //! Results from an inverse kinematics solve for multiple problems.\n  class CUMO_EXPORT ResultsArray {\n   public:\n    virtual ~ResultsArray() = default;\n\n    //! Return the number of problems that were successfully solved.\n    //!\n    //! The number of problems with `Results::Status::SUCCESS`.\n    [[nodiscard]] virtual int numSuccesses() const = 0;\n\n    //! Return the number of problems processed by the solver.\n    [[nodiscard]] virtual int numProblems() const = 0;\n\n    //! Return the results of the IK solve for a specific problem.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `problem_index` is out of bounds.\n    [[nodiscard]] virtual std::shared_ptr<const Results> problem(int problem_index) const = 0;\n  };\n\n  //! Attempt to find c-space solutions that satisfy the constraints specified in\n  //! `task_space_target`.\n  //!\n  //! The `cspace_seeds` are optional inputs that can be used to \"warm start\" the IK optimization.\n  //! NOTE: It is *not* required that the `cspace_seeds` represent valid c-space positions (i.e., be\n  //!       within position limits, be collision-free, etc.).\n  //!\n  //! A fatal error will be logged if:\n  //!   1. Any c-space position in `cspace_seeds` does not have the same number of c-space\n  //!      coordinates as the `RobotDescription` used to configure the\n  //!      `CollisionFreeIkSolverConfig`.\n  [[nodiscard]] virtual std::unique_ptr<Results> solve(\n      const TaskSpaceTarget &task_space_target,\n      const std::vector<Eigen::VectorXd> &cspace_seeds = {}) const = 0;\n\n  //! Attempt to find c-space solutions that satisfy the constraints specified in\n  //! `task_space_target_goalset`.\n  //!\n  //! To be considered a valid solution, a c-space position must satisfy the constraints of any\n  //! *one* of the targets specified in `task_space_target_goalset`.\n  //!\n  //! The `cspace_seeds` are optional inputs that can be used to \"warm start\" the IK optimization.\n  //! NOTE: It is *not* required that the `cspace_seeds` represent valid c-space positions (i.e., be\n  //!       within position limits, be collision-free, etc.).\n  //!\n  //! DEPRECATED: This function is deprecated and will be removed in a future version. Use\n  //! `solveArray()` instead. To achieve the same goalset planning functionality with\n  //! `solveArray()`, create a `TaskSpaceTargetArray` with a single problem (i.e., a single element\n  //! in the outer constraint vectors), and place the goalset data as the inner constraint vectors.\n  //! Then call `solveArray()` and inspect the results for the first problem.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. Any c-space position in `cspace_seeds` does not have the same number of c-space\n  //!      coordinates as the `RobotDescription` used to configure the\n  //!      `CollisionFreeIkSolverConfig`.\n  [[nodiscard]] CUMO_DEPRECATED virtual std::unique_ptr<Results> solveGoalset(\n      const TaskSpaceTargetGoalset &task_space_target_goalset,\n      const std::vector<Eigen::VectorXd> &cspace_seeds = {}) const = 0;\n\n  //! Attempt to find c-space solutions for each problem in `task_space_target_array`.\n  //!\n  //! The `cspace_seeds` are optional inputs that can be used to \"warm start\" each IK optimization.\n  //! NOTE: It is *not* required that the `cspace_seeds` represent valid c-space positions (i.e., be\n  //!       within position limits, be collision-free, etc.).\n  //!\n  //! Non-fatal warnings will be logged if:\n  //!   1. The number of `cspace_seeds` is larger than `num_seeds` in the\n  //!      `CollisionFreeIkSolverConfig`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. Any c-space position in `cspace_seeds` does not have the same number of c-space\n  //!      coordinates as the `RobotDescription` used to configure the\n  //!      `CollisionFreeIkSolverConfig`.\n  [[nodiscard]] virtual std::unique_ptr<ResultsArray> solveArray(\n      const TaskSpaceTargetArray &task_space_target_array,\n      const std::vector<Eigen::VectorXd> &cspace_seeds = {}) const = 0;\n};\n\n//! Create a `CollisionFreeIkSolver` with the given `config`.\nCUMO_EXPORT std::unique_ptr<CollisionFreeIkSolver>\nCreateCollisionFreeIkSolver(const CollisionFreeIkSolverConfig &config);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/collision_sphere_generator.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! The `CollisionSphereGenerator` generates a set of spheres to approximate the volume enclosed by\n//! a triangular mesh.\nclass CUMO_EXPORT CollisionSphereGenerator {\n public:\n  virtual ~CollisionSphereGenerator() = default;\n\n  //! Simple representation of a sphere.\n  struct CUMO_EXPORT Sphere {\n    //! 3d coordinates for the center position of the sphere.\n    Eigen::Vector3d center;\n\n    //! Radius of the sphere.\n    double radius;\n  };\n\n  //! Generate a set of `num_spheres` that approximate the volume of the specified mesh.\n  //!\n  //! If `radius_offset` is set to zero, all of the generated spheres will be (approximately)\n  //! tangent to at least two triangles in the mesh. A positive `radius_offset` will increase the\n  //! radii of all spheres by adding `radius_offset` (e.g., a `radius_offset` of 0.05 will allow\n  //! spheres to extend beyond the mesh by up to 0.05). A negative value will shrink the radii of\n  //! the returned spheres (down to a minimum radius of `min_sphere_radius` as described in the\n  //! documentation for `setParam()`).\n  virtual std::vector<Sphere> generateSpheres(int num_spheres, double radius_offset) = 0;\n\n  //! Specify the value for a given parameter.\n  //!\n  //! The required `ParamValue` constructor for each parameter is detailed in the documentation for\n  //! `setParam()`.\n  struct CUMO_EXPORT ParamValue {\n    //! Create `ParamValue` from `int`.\n    ParamValue(int value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `double`.\n    ParamValue(double value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `bool`.\n    ParamValue(bool value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Set the value of the parameter.\n  //!\n  //! If the value has been successfully updated, `true` is returned. If an invalid value has been\n  //! specified for a parameter, a verbose warning is logged, `false` is returned and the parameter\n  //! value is *NOT* updated.\n  //!\n  //! Default parameters are expected to work well for most meshes commonly used to represent\n  //! robots (i.e., generally \"meter scale\"). If the meshes are particularly larger or smaller\n  //! (or represented in units other than meters), then `min_sphere_radius`,\n  //! `convergence_radius_tol`, 'surface_offset`, `min_triangle_area`, and 'num_voxels' can be\n  //! scaled appropriately.\n  //!\n  //! Likewise, the default parameters assume a conventional counter-clockwise winding order for\n  //! vertex indices. If this is *not* the case, `flip_normals` can be set to `true`.\n  //!\n  //! Finally, increasing `num_medial_sphere_samples` may increase the quality of the selected\n  //! spheres (at the expense of more processing time).\n  //!\n  //! The full set of parameters that can be set are:\n  //!\n  //! `num_medial_sphere_samples` [`int`]\n  //!   - The collision sphere generation begins by sampling medial spheres uniformly from the\n  //!     surface of the provided mesh. The medial axis of a mesh is the set of all points having\n  //!     more than one closest point to the mesh. The medial spheres are spheres centered on the\n  //!     medial axis with radius set to the minimum distance from the mesh. By definition, these\n  //!     medial spheres are tangent to at least two faces of the mesh.\n  //!   - These sampled spheres are the set from which the minimal set of spheres to approximate\n  //!     the mesh will be selected. A larger number of samples may result in a better selection of\n  //!     spheres, and more spheres may be valuable if the mesh is particularly complex.\n  //!   - `num_medial_sphere_samples` must be positive.\n  //!   - Default value is 500.\n  //!\n  //! `flip_normals` [`bool`]\n  //!   - The mesh input to the `CollisionSphereGenerator ` is not required to be strictly\n  //!     watertight, but is expected to generally represent a volume. Spheres will be generated by\n  //!     sampling points on the triangular mesh faces and \"growing\" the spheres inwards to find a\n  //!     sphere that is tangent to both the original triangle and some other triangle on the mesh.\n  //!   - It is expected that for any point on any triangular face, a sphere that is tangent to that\n  //!     point on the \"inside\" (negative normal direction) of the triangle with diameter set to the\n  //!     maximum extent on the mesh will intersect at least one other triangle.\n  //!   - The normal is by default computed assuming a counter-clockwise winding direction for\n  //!     vertex indices. If the mesh is input with clockwise winding direction for vertex indices,\n  //!     then `flip_normals` should be set to `true` to reverse winding order.\n  //!   - Default value is `false`.\n  //!\n  //! `min_sphere_radius` [`double`]\n  //!   - When generating medial spheres, any spheres that are found to have radius less than\n  //!     `min_sphere_radius` will be discarded.\n  //!   - NOTE: Discarded spheres still count towards `num_medial_sphere_samples`. E.g., if\n  //!     `generateSpheres()` is called with `num_medial_sphere_samples` = 500, and 20 spheres are\n  //!     discarded for being too small, then the actual number of spheres that will be used to\n  //!     select the final spheres is 480.\n  //!   - `min_sphere_radius` must be positive.\n  //!   - Default value is 1e-3.\n  //!\n  //! `seed` [`int`]\n  //!   - Random seed used for sampling surface points on mesh from which to sample spheres.\n  //!   - `seed` must be positive.\n  //!   - Default value is 12345.\n  //!\n  //! `max_iterations` [`int`]\n  //!   - Maximum number of iterations of binary search used to approximate the radius of each\n  //!     medial sphere.\n  //!   - NOTE: If `max_iterations` is reached, the sphere will be discarded. Discarded spheres\n  //!     still count towards `num_medial_sphere_samples`. E.g., if `generateSpheres()` is called\n  //!     with `num_medial_sphere_samples` = 500, and 20 spheres are discarded for not converging to\n  //!     the medial sphere radius tolerance within the `max_iterations`, then the actual number of\n  //!     spheres that will be used to select the final spheres is 480.\n  //!   - `max_iterations` must be positive.\n  //!   - Default value is 100.\n  //!\n  //! `convergence_radius_tol` [`double`]\n  //!   - Convergence criteria for binary search used to approximate the radius of each medial\n  //!     sphere. The search will end when the sphere radius is within `convergence_radius_tol` of\n  //!     the actual medial sphere radius.\n  //!   - `convergence_radius_tol` must be positive.\n  //!   - Default value is 1e-3.\n  //!\n  //! `surface_offset` [`double`]\n  //!   - When points are sampled from the mesh surface to generate medial spheres, they are offset\n  //!     by `surface_offset` towards the \"inside\" of the mesh in order to avoid collisions with\n  //!     faces very close to the sampling face. This is to help avoid very small spheres that would\n  //!     be culled due to the `min_sphere_radius`.\n  //!   - `surface_offset` must be positive.\n  //!   - Default value is 1e-4.\n  //!\n  //! `min_triangle_area` [`double`]\n  //!   - Triangles with area below `min_triangle_area` are discarded when creating the mesh for\n  //!     generating medial spheres. This includes degenerate triangles where surface area\n  //!     approaches zero as well as relatively small triangles that will increase computation cost\n  //!     without being likely to improve the set of sampled spheres.\n  //!   - `min_triangle_area` must be positive.\n  //!   - Default value is 1e-8.\n  //!\n  //!  `num_voxels` [`int`]\n  //!    - A voxel grid is used to estimate volume additions from each sphere when selecting spheres\n  //!      from the full sample of medial spheres. The `num_voxels` provides a trade-off between\n  //!      accurate volume approximation (with larger `num_voxels`) and faster selection (with\n  //!      smaller `num_voxels`).\n  //!    - `num_voxels` specifies the number of voxels along the longest dimension of an\n  //!      axis-aligned bounding box (AABB) encompassing the specified mesh. For example, if the\n  //!      AABB has dimensions {20, 50, 30} and `num_voxels is set to 100, then each voxel will\n  //!      have side length 50 / 100 = 0.5. Thus, the number of voxels along each axis of the grid\n  //!      will be [40, 100, 60].\n  //!    - `num_voxels` must be positive.\n  //!    - Default value is 50.\n  virtual bool setParam(const std::string &param_name, ParamValue value) = 0;\n\n  //! Return the number of validated triangles that have been included in the mesh used for sampling\n  //! spheres to approximate volume.\n  //! NOTE: The number of returned triangles may be less than the number of `triangles` passed to\n  //!       `CreateCollisionSphereGenerator()` if triangles are discarded for invalid indices or\n  //!       having an area smaller than `min_triangle_area` (see `setParam()` documentation for\n  //!       details).\n  virtual int numTriangles() = 0;\n\n  //! Return all of the medial axis spheres used to approximate the volume of the mesh. The spheres\n  //! returned by `generateSpheres()` are selected from this set.\n  //! NOTE: This function is intended primarily for debugging functionality and/or tuning parameters\n  //!       in `setParam()` and is not likely to be needed by most users.\n  virtual std::vector<Sphere> getSampledSpheres() = 0;\n};\n\n//! Create a `CollisionSphereGenerator` to generate spheres that approximate the volume of a mesh\n//! represented by `vertices` and `triangles`.\n//!\n//! Each vertex in `vertices` is a set of (x,y,z) coordinates that can be referenced by one or more\n//! triangles in `triangles`. Each triangle is a set of indices in the `vertices` vector.\n//!\n//! Vertex indices for a given triangle are considered valid if:\n//!   [a] The index for each vertex is in range [0, vertices.size()), and\n//!   [b] The same index is *NOT* included twice.\n//!\n//! Triangles with invalid indices will be discarded with verbose warnings.\n//!\n//! Additionally, each triangle in `triangles` is tested to ensure that its area is greater than or\n//! equal to `min_triangle_area` (see `CollisionSphereGenerator::setParam()` documentation for\n//! details). Triangles that are too small will be discarded.\n//!\n//! By default, the triangle normals will be computed assuming a counter-clockwise winding\n//! direction. This convention can be flipped by setting `flip_normals` (see\n//! `CollisionSphereGenerator::setParam()` documentation for details).\nCUMO_EXPORT std::unique_ptr<CollisionSphereGenerator> CreateCollisionSphereGenerator(\n    const std::vector<Eigen::Vector3d> &vertices,\n    const std::vector<Eigen::Vector3i> &triangles);\n\n//! Default input parameters for `GenerateCollisionSpheres()`.\nconstexpr double kCollisionSphereDefaultMaxOvershoot = 0.05;\nconstexpr int kCollisionSphereDefaultMaxNumSpheres = 3000;\nconstexpr double kCollisionSphereDefaultSurfacePointDensity = 30000.;\nconstexpr int kCollisionSphereDefaultSurfacePointSamplingSeed = 123;\nconstexpr bool kCollisionSphereDefaultFlipNormals = false;\nconstexpr double kCollisionSphereDefaultMinTriangleArea = 1e-8;\n\n//! Generate a selection of collision spheres from a grid of points filling a triangle mesh.\n//!\n//! NOTE: For most meshes, this `GenerateCollisionSpheres()` function is expected to perform better\n//!       than `CollisionSphereGenerator()` (which is likely to be deprecated).\n//!\n//! The mesh is represented by `triangles` and `vertices`, where each triangle in `triangles` is a\n//! set of indices referencing points in `vertices`. At least three `vertices` and at least one\n//! triangle must be provided.\n//!\n//! The `max_overshoot` serves as an upper bound for how far any collision sphere may extend beyond\n//! the surface of the mesh. The units of the `max_overshoot` are the same as the mesh, typically\n//! meters for robotics applications. The `max_overshoot` must be positive.\n//!\n//! Larger `max_overshoot` values will create conservative collision sphere representations with\n//! relatively few spheres (increasing performance for motion generation and collision checking).\n//! Smaller `max_overshoot` values will fit spheres more tightly to the mesh and will (in general)\n//! require more spheres to fit a given mesh (reducing performance for motion generation and\n//! collision checking).\n//!\n//! The `max_num_spheres` sets an upper bound on the number of candidate spheres from which\n//! collision spheres will be selected. Specifically, the `max_num_spheres` is used to generate a\n//! grid of points filling the axis-aligned bounding box enclosing the mesh. These points serve as\n//! potential sphere centers. The `max_num_spheres` may need to be increased from the default value\n//! for particularly large meshes or for particularly small `max_overshoot` values. Increasing\n//! `max_num_spheres` will (in general) improve the selection of collision spheres, but result in\n//! slower generation time. The `max_num_spheres` must be positive.\n//!\n//! The goal of sphere selection is to select spheres such that the entire mesh surface is\n//! enclosed by one or more spheres. As an approximation, points are distributed uniformly on the\n//! mesh surface and used to test whether the surface is fully enclosed. The `surface_point_density`\n//! determines how many test point are distributed. The units for `surface_point_density` are\n//! 1 / [mesh-length-unit]^2. For the typical robotics case (with the mesh represented in meters),\n//! the default value corresponds to 3 surface points per cm^2. Using higher values for\n//! `surface_point_density` will increase the likelihood of full surface enclosure (at the cost of\n//! slower generation time). The `surface_point_density` must be positive. Changing the parameter\n//! `surface_point_sampling_seed` will result in different surface points being selected and cause\n//! different candidate spheres to be selected. This seed may be used, e.g.,  to randomly generate\n//! multiple sphere sets with the same `max_overshoot`.\n//!\n//! The mesh input to `GenerateCollisionSpheres()` is not required to be strictly watertight\n//! but is expected to generally represent a volume. For each triangle, the normal is (by default)\n//! computed assuming a counter-clockwise winding direction for vertex indices. If the mesh is input\n//! with clockwise winding direction for vertex indices, then `flip_normals` should be set to\n//! `true` to reverse winding order. The correct surface normal orientation for `triangles` is\n//! required for accurately determining whether grid points are interior or exterior to the mesh.\n//!\n//! Very small and/or degenerate triangles are unlikely to aid in collision sphere generation. Thus,\n//! triangles with surface area less than `min_triangle_area` are culled prior to sphere generation\n//! and selection. The `min_triangle_area` must be positive.\nCUMO_EXPORT std::vector<CollisionSphereGenerator::Sphere> GenerateCollisionSpheres(\n    const std::vector<Eigen::Vector3d> &vertices,\n    const std::vector<Eigen::Vector3i> &triangles,\n    double max_overshoot = kCollisionSphereDefaultMaxOvershoot,\n    int max_num_spheres = kCollisionSphereDefaultMaxNumSpheres,\n    double surface_point_density = kCollisionSphereDefaultSurfacePointDensity,\n    int surface_point_sampling_seed = kCollisionSphereDefaultSurfacePointSamplingSeed,\n    bool flip_normals = kCollisionSphereDefaultFlipNormals,\n    double min_triangle_area = kCollisionSphereDefaultMinTriangleArea);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/composite_path_spec.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cspace_path_spec.h\"\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/task_space_path_spec.h\"\n\nnamespace cumotion {\n\n//! The `CompositePathSpec` is used to procedurally composite `CSpacePathSpec` and\n//! `TaskSpacePathSpec` segments into a single path specification.\nclass CUMO_EXPORT CompositePathSpec {\n public:\n  virtual ~CompositePathSpec() = default;\n\n  //! Return the number of configuration space coordinates for the path specification.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Return the number of path specifications contained in the `CompositePathSpec`.\n  [[nodiscard]] virtual int numPathSpecs() const = 0;\n\n  //! Indicate whether a path specification is a `TaskSpacePathSpec` (i.e., `TASK_SPACE`) or a\n  //! `CSpacePathSpec` (i.e., `CSPACE`).\n  enum class PathSpecType {\n    TASK_SPACE,\n    CSPACE\n  };\n\n  //! Given a `path_spec_index` in range [0, `numPathSpecs()`), return the type of the corresponding\n  //! path specification.\n  //!\n  //! A fatal error will be logged if `path_spec_index` is not in range [0, `numPathSpecs()`).\n  //!\n  //! The `path_spec_index` corresponds to the order in which path specifications are added to the\n  //! `CompositePathSpec`.\n  [[nodiscard]] virtual PathSpecType pathSpecType(int path_spec_index) const = 0;\n\n  //! Return a `TaskSpacePathSpec` at the given `path_spec_index`.\n  //!\n  //! If the `path_spec_index` is invalid (i.e., not in range `[0, `numPathSpecs()`)) *OR* the\n  //! `path_spec_index` does not correspond to a `TaskSpacePathSpec`, then `nullptr` will be\n  //! returned and an error will be logged.\n  [[nodiscard]]\n  virtual std::unique_ptr<TaskSpacePathSpec> taskSpacePathSpec(int path_spec_index) const = 0;\n\n  //! Return a `CSpacePathSpec` at the given `path_spec_index`.\n  //!\n  //! If the `path_spec_index` is invalid (i.e., not in range `[0, `numPathSpecs()`)) *OR* the\n  //! `path_spec_index` does not correspond to a `CSpacePathSpec`, then `nullptr` will be\n  //! returned and an error will be logged.\n  [[nodiscard]]\n  virtual std::unique_ptr<CSpacePathSpec> cSpacePathSpec(int path_spec_index) const = 0;\n\n  //! Specify the transition preceding a `TaskSpacePathSpec` or `CSpacePathSpec`.\n  enum class TransitionMode {\n    //! Skip either the initial task space pose of the `TaskSpacePathSpec` or the initial c-space\n    //! configuration of the `CSpacePathSpec`.\n    //!\n    //! For a `TaskSpacePathSpec`, the first task space path segment in the appended\n    //! `TaskSpacePathSpec` will instead originate from the current task space pose of the\n    //! `CompositePathSpec`.\n    //!\n    //! For a `CSpacePathSpec`, The first c-space waypoint of the added `CSpacePathSpec` will be\n    //! added directly after the current c-space configuration of the `CompositePathSpec`.\n    SKIP,\n\n    //! Add a path from the current pose of the `CompositePathSpec` to the initial task space pose\n    //! of the `TaskSpacePathSpec` or the initial c-space configuration of the `CSpacePathSpec`,\n    //! with no restrictions on the form of the path.\n    FREE,\n\n    //! Add a path that is linear in task space from the current task space pose of the\n    //! `CompositePathSpec` to the initial task space pose of the `TaskSpacePathSpec`.\n    //!\n    //! *WARNING* This mode is *ONLY* available for adding a `TaskSpacePathSpec`\n    //! (via `addTaskSpacePathSpec()`). Usage with `addCSpacePathSpec()` will result in an error\n    //! and the `CSpacePathSpec` will *NOT* be added.\n    LINEAR_TASK_SPACE\n  };\n\n  //! Given a `path_spec_index` in range [0, `numPathSpecs()`), return the corresponding transition\n  //! mode.\n  //!\n  //! A fatal error will be logged if `path_spec_index` is not in range [0, `numPathSpecs()`).\n  //!\n  //! The `path_spec_index` corresponds to the order in which path specifications are added to the\n  //! `CompositePathSpec`.\n  [[nodiscard]] virtual TransitionMode transitionMode(int path_spec_index) const = 0;\n\n  //! Add a task space `path_spec` to the `CompositePathSpec` with the specified `transition_mode`.\n  //!\n  //! Returns `true` if path specification is successfully added. Else, returns `false`.\n  virtual bool addTaskSpacePathSpec(const TaskSpacePathSpec &path_spec,\n                                    TransitionMode transition_mode) = 0;\n\n  //! Add a c-space `path_spec` to the `CompositePathSpec` with the specified `transition_mode`.\n  //!\n  //! `path_spec` will be discarded (with logged error) if it does not have the same number of\n  //! c-space coordinates as the `CompositePathSpec` (i.e., `numCSpaceCoords()`).\n  //!\n  //! `path_spec` will be discarded (with logged error) if the `transition_mode` is invalid\n  //! (i.e., `LINEAR_TASK_SPACE`).\n  //!\n  //! Returns `true` if path specification is successfully added. Else, returns `false`.\n  virtual bool addCSpacePathSpec(const CSpacePathSpec &path_spec,\n                                 TransitionMode transition_mode) = 0;\n};\n\n//! Create a `CompositePathSpec` with the specified `initial_cspace_position`.\nCUMO_EXPORT std::unique_ptr<CompositePathSpec> CreateCompositePathSpec(\n    const Eigen::VectorXd &initial_cspace_position);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/cspace_path.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Represent a path through configuration space (i.e., c-space or \"joint space\").\n//!\n//! This path is parameterized by independent variable `s` and is generally expected to be\n//! continuous, but not necessarily smooth.\nclass CUMO_EXPORT CSpacePath {\n public:\n  virtual ~CSpacePath() = default;\n\n  //! Return the number of configuration space coordinates for the path.\n  virtual int numCSpaceCoords() const = 0;\n\n  //! Indicates continuous range of independent values, `s`, for which the path is defined.\n  struct CUMO_EXPORT Domain {\n    //! Minimum value of `s` defining domain.\n    double lower;\n\n    //! Maximum value of `s` defining domain.\n    double upper;\n\n    //! Convenience function to return the span of `s` values included in domain.\n    [[nodiscard]] double span() const { return upper - lower; }\n  };\n\n  //! Return the domain for the path.\n  [[nodiscard]] virtual Domain domain() const = 0;\n\n  //! Evaluate the path at the given `s`.\n  //!\n  //! A fatal error is logged if `s` is outside of the path `domain()`.\n  [[nodiscard]] virtual Eigen::VectorXd eval(double s) const = 0;\n\n  //! Return the total distance accumulated along the path, where distance is computed using the\n  //! L2-norm.\n  //!\n  //! This length is not, in general, equal to the distance between the configurations at the lower\n  //! and upper bounds of the domain.\n  [[nodiscard]] virtual double pathLength() const = 0;\n\n  //! Return the minimum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These minimum position values are not, in general, synchronous. Instead, they represent\n  //!       the minimum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd minPosition() const = 0;\n\n  //! Return the maximum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These maximum position values are not, in general, synchronous. Instead, they represent\n  //!       the maximum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd maxPosition() const = 0;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/cspace_path_spec.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! The `CSpacePathSpec` is used to procedurally specify a `CSpacePath` from a series of\n//! configuration space points.\nclass CUMO_EXPORT CSpacePathSpec {\n public:\n  virtual ~CSpacePathSpec() = default;\n\n  //! Return the number of configuration space coordinates for the path specification.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Add a path to connect the previous configuration to the `waypoint`.\n  //!\n  //! The `waypoint` must have dimension equal to `numCSpaceCoords()` or it will be discarded.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addCSpaceWaypoint(const Eigen::VectorXd &waypoint) = 0;\n};\n\n//! Create a `CSpacePathSpec` with the specified `initial_cspace_position`.\nCUMO_EXPORT std::unique_ptr<CSpacePathSpec> CreateCSpacePathSpec(\n    const Eigen::VectorXd &initial_cspace_position);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/cumotion.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2019-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Shared interface functions for the cuMotion library.\n\n#pragma once\n\n#include <exception>\n#include <memory>\n#include <ostream>\n#include <string>\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/version.h\"\n\nnamespace cumotion {\n\n//! @brief Get cuMotion library version as a `std::string`.\n//!\n//! Also see the `CUMOTION_VERSION_STRING` macro, which serves a similar purpose.  The two should\n//! always agree.  If they don't, it indicates a mismatch between the installed cuMotion headers\n//! and installed cuMotion libraries.\nCUMO_EXPORT std::string VersionString();\n\n// On Windows, `wingdi.h` (included transitively via `windows.h` and certain CUDA headers)\n// contains a `#define ERROR 0` that collides with the `LogLevel::ERROR` enum value.\n// The following temporarily undefines the macro.\n#ifdef _WIN32\n#  pragma push_macro(\"ERROR\")\n#  undef ERROR\n#endif\n\n//! @brief Logging levels, ordered from least to most verbose.\nenum class CUMO_EXPORT LogLevel {\n  FATAL,    //!< Logging level for nonrecoverable errors (minimum level, so always enabled).\n  ERROR,    //!< Logging level for recoverable errors.\n  WARNING,  //!< Logging level for warnings, indicating possible cause for concern.\n  INFO,     //!< Logging level for informational messages.\n  VERBOSE   //!< Logging level for highly verbose informational messages.\n};\n\n#ifdef _WIN32\n#  pragma pop_macro(\"ERROR\")\n#endif\n\n//! @brief Base class for user-defined logger that allows custom handling of log messages, warnings,\n//! and errors.\n//!\n//! Such a custom logger may be installed via `SetLogger()`.\nclass CUMO_EXPORT Logger {\n public:\n  virtual ~Logger() = default;\n\n  //! Return an `ostream` suitable for logging messages at the given `log_level`.\n  virtual std::ostream &log(LogLevel log_level) = 0;\n\n  //! Perform any required post-processing of individual log messages.  This function is called\n  //! after the message has been logged to the `std::ostream` returned by `log()` but before\n  //! `handleFatalError()` is called (if applicable).\n  virtual void finalizeLogMessage([[maybe_unused]] LogLevel log_level) {\n    // Do nothing by default.\n  }\n\n  //! Handle fatal errors.  This callback will be called after any associated error message has\n  //! been written to the ostream returned by `log(FATAL)`.  Because fatal errors are\n  //! non-recoverable, this function must ensure that any active cuMotion objects are discarded.\n  virtual void handleFatalError() = 0;\n};\n\n//! @brief Exception thrown by the default logger for fatal errors.\n//!\n//! This is the only exception thrown by cuMotion, so clients may avoid exceptions completely by\n//! installing a custom logger.\nclass CUMO_EXPORT FatalException : public std::exception {\n public:\n  explicit FatalException(const std::string &message) : message_(message) {}\n\n  [[nodiscard]] const char *what() const noexcept override { return message_.c_str(); }\n\n private:\n  std::string message_;\n};\n\n//! @brief Suppress output for all log messages with associated verbosity higher than `log_level`.\n//!\n//! Messages suppressed in this way incur very low overhead, so it's best to take advantage of\n//! this facility even if a custom `Logger` is provided.\n//!\n//! Until `SetLogLevel()` is called, the default log level is `WARNING`.  The lowest supported\n//! `log_level` is `FATAL`, since it is not possible to supress fatal errors.\nCUMO_EXPORT void SetLogLevel(LogLevel log_level);\n\n//! @brief Install a custom logger, derived from the above `Logger` class.\n//!\n//! If `logger` is a null pointer, then the default logger is reenabled.  The default logger\n//! directs all output to stdout and throws a `FatalException` in the event of a fatal error.\nCUMO_EXPORT void SetLogger(std::shared_ptr<Logger> logger = nullptr);\n\n//! Set color/style used by the default logger for messages of a given `log_level`.  The `style`\n//! string may contain one or more ANSI control sequences (e.g., enabling fatal error messages to\n//! be rendered in bold with a red foreground color).\n//!\n//! For convenience, a selection of common control sequences is provided in\n//! include/cumotion/text_style.h\nCUMO_EXPORT void SetDefaultLoggerTextStyle(LogLevel log_level, const std::string &style);\n\n//! Set prefix used by the default logger for all messages. The `prefix` string is logged after\n//! the `style` string set for each `log_level` by `SetDefaultLoggerTextStyle()` and prior to the\n//! content of the logged message.\n//!\n//! The default prefix is an empty string.\nCUMO_EXPORT void SetDefaultLoggerPrefix(const std::string &prefix);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/cumotion_export.h",
    "content": "\n#ifndef CUMO_EXPORT_H\n#define CUMO_EXPORT_H\n\n#ifdef CUMO_STATIC_DEFINE\n#  define CUMO_EXPORT\n#  define CUMO_NO_EXPORT\n#else\n#  ifndef CUMO_EXPORT\n#    ifdef cumotion_cumotion_EXPORTS\n        /* We are building this library */\n#      define CUMO_EXPORT __attribute__((visibility(\"default\")))\n#    else\n        /* We are using this library */\n#      define CUMO_EXPORT __attribute__((visibility(\"default\")))\n#    endif\n#  endif\n\n#  ifndef CUMO_NO_EXPORT\n#    define CUMO_NO_EXPORT __attribute__((visibility(\"hidden\")))\n#  endif\n#endif\n\n#ifndef CUMO_DEPRECATED\n#  define CUMO_DEPRECATED __attribute__ ((__deprecated__))\n#endif\n\n#ifndef CUMO_DEPRECATED_EXPORT\n#  define CUMO_DEPRECATED_EXPORT CUMO_EXPORT CUMO_DEPRECATED\n#endif\n\n#ifndef CUMO_DEPRECATED_NO_EXPORT\n#  define CUMO_DEPRECATED_NO_EXPORT CUMO_NO_EXPORT CUMO_DEPRECATED\n#endif\n\n#if 0 /* DEFINE_NO_DEPRECATED */\n#  ifndef CUMO_NO_DEPRECATED\n#    define CUMO_NO_DEPRECATED\n#  endif\n#endif\n#ifdef _MSC_VER\n#  define CUMO_EXTERN_TEMPLATE_EXPORT\n#  define CUMO_TEMPLATE_EXPORT CUMO_EXPORT\n#else\n#  define CUMO_EXTERN_TEMPLATE_EXPORT CUMO_EXPORT\n#  define CUMO_TEMPLATE_EXPORT\n#endif\n\n#endif /* CUMO_EXPORT_H */\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/ik_solver.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for inverse kinematics solvers.\n\n#pragma once\n\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/kinematics.h\"\n\nnamespace cumotion {\n\n//! Configuration for solving inverse kinematics.\nstruct CUMO_EXPORT IkConfig {\n  //! Construct configuration with default values likely to be appropriate for most 6- and 7-dof\n  //! robotic arms.\n  IkConfig() = default;\n\n  //! The maximum number of c-space positions that will be used as seeds (i.e., initial positions)\n  //! for cyclic coordinate descent.\n  //!\n  //! The inverse kinematic solver will terminate when a valid c-space configuration is found OR\n  //! the number of attempted descents reaches `max_num_descents`.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 100\n  int max_num_descents = 100;\n\n  //! Optional parameter to set the initial c-space configurations for each descent.\n  //!\n  //! These `cspace_seeds` will be used to attempt cyclic coordinate descent in the order\n  //! provided, terminating before all `cspace_seeds` are tried if a valid `cspace_position` is\n  //! found.\n  //!\n  //! If the number of attempted descents is greater than the number of provided `cspace_seeds`,\n  //! random starting configurations will be generated (see `irwin_hall_sampling_order` for\n  //! details).\n  //!\n  //! It is valid to provide no `cspace_seeds`. In this case all initial configurations will be\n  //! randomly sampled.\n  std::vector<Eigen::VectorXd> cspace_seeds = {};\n\n  //! If the number of attempted descents exceeds the provided number of `cspace_seeds` (including\n  //! the case where no `cspace_seeds` are provided), then random c-space configurations will be\n  //! sampled for each descent.\n  //!\n  //! Each c-space coordinate value will be sampled from an Irwin-Hall distribution spanning the\n  //! joint limits. The distribution mean will always be set to the midpoint between upper and\n  //! lower joint limits.\n  //!\n  //! Setting `irwin_hall_sampling_order` to 1 corresponds to a uniform distribution, setting to\n  //! 2 corresponds to a triangular distribution, and higher values approximate a normal\n  //! distribution.\n  //!\n  //! See https://en.wikipedia.org/wiki/Irwin%E2%80%93Hall_distribution for reference, noting that\n  //! the internal distribution is scaled s.t. all sampled values will be within joint limits.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 2\n  int irwin_hall_sampling_order = 2;\n\n  //! Seed for random number generator used for Irwin-Hall sampling for initial c-space positions.\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 123456\n  int sampling_seed = 123456;\n\n  //! Maximum position error (L2-norm) of the task space pose for a successful IK solution.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 1e-3\n  double position_tolerance = 1e-3;\n\n  //! Maximum orientation error of the task space pose for a successful IK solution.\n  //!\n  //! For each axis in task space, error is defined as the L2-norm of the difference between the\n  //! axis direction and the target pose axis direction.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 0.01\n  double orientation_tolerance = 0.01;\n\n  //================================================================================================\n  // Parameters for cyclic coordinate descent (CCD) algorithm.\n\n  //! The number of iterations used for cyclic coordinate descent from each initial c-space\n  //! position (i.e., seed).\n  //!\n  //! Smaller values will decrease the amount of computation time per descent, but may require\n  //! more descents to converge to valid c-space configuration.\n  //!\n  //! CCD will be disabled for values less than 1.\n  //!\n  //! Default: 10\n  int ccd_max_iterations = 10;\n\n  //! Each descent can terminate early if the L2-norm of the change to c-space coordinates is less\n  //! than the `descent_termination_delta`.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 1e-1\n  double ccd_descent_termination_delta = 1e-1;\n\n  //! Weight for the relative importance of position error term when performing\n  //! cyclic coordinate descent.\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 1.0\n  double ccd_position_weight = 1.0;\n\n  //! Weight for the relative importance of orientation error term when performing\n  //! cyclic coordinate descent.\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 0.05\n  double ccd_orientation_weight = 0.05;\n\n  //! Number of samples used to identify valid three-point bracket for numerical optimization of\n  //! c-space updates.\n  //!\n  //! NOTE: This parameter is *ONLY* used when more than one active upstream joint is controlled by\n  //! a single c-space coordinate.\n  //!\n  //! For the \"many-to-one joint-to-cspace\" case, quadratic fit search (QFS) is used to approximate\n  //! an optimal c-space update during each iteration of cyclic coordinate descent. In order to\n  //! find a valid three-point bracket for QFS, N points are sampled to attempt to find a region\n  //! with minimum error. This sampling is incorporated into the numerical solver since, in\n  //! general, the error function will not be unimodal within the domain bounded by the c-space\n  //! position limits.\n  //!\n  //! Must be greater than one.\n  //!\n  //! Default: 10\n  int ccd_bracket_search_num_uniform_samples = 10;\n\n  //================================================================================================\n  // Parameters for Broyden-Fletcher-Goldfarb-Shanno (BFGS) solver.\n\n  //! The number of iterations used for Broyden-Fletcher-Goldfarb-Shanno (BFGS) descent from each\n  //! initial c-space position (i.e., seed).\n  //!\n  //! Smaller values will decrease the amount of computation time per descent, but may require\n  //! more descents to converge to valid c-space configuration.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 60\n  int bfgs_max_iterations = 60;\n\n  //! The BFGS solver will terminate if the L2-norm of the error function gradient at the current\n  //! best c-space position is less than `bfgs_gradient_norm_termination`.\n  //!\n  //! Low values for `bfgs_gradient_norm_termination` will increase solver accuracy, while high\n  //! values will decrease solve times for a given problem.\n  //!\n  //! Must be positive.\n  //!\n  //! Default: 1e-6\n  double bfgs_gradient_norm_termination = 1e-6;\n\n  //! The BFGS solver is implemented as a two-step process, with a coarse solve used to identify\n  //! whether convergence to a valid c-space position is likely.\n  //!\n  //! For the coarse solve, the `bfgs_gradient_norm_termination` is multiplied by the\n  //! `bfgs_gradient_norm_termination_coarse_scale_factor` such that the optimization will end\n  //! early.\n  //!\n  //! Must be greater than or equal to 1.\n  //!\n  //! Default: 1e7\n  double bfgs_gradient_norm_termination_coarse_scale_factor = 1e7;\n\n  //! The error function for BFGS descent optionally includes a cost-term for avoiding c-space\n  //! coordinate values near position limits.\n  //!\n  //! If set to `ENABLE`, this cost term will always be turned on. If set to `DISABLE`, this cost\n  //! term will always be turned off. Setting to `AUTO` will turn this cost term on for systems\n  //! with 7 or more degrees-of-freedom (DoF), while leaving it off for systems with 6 or fewer\n  //! DoF. NOTE: DoF is defined as the numer of c-space coordinates upstream from the target frame.\n  //!\n  //! Setting to `AUTO` is likely to provide desirable results for most common robots. Redundant\n  //! mechanisms with less than 7-DoF are a potential exception. E.g., a 4-Dof robot restricted to\n  //! planar motion may benefit from enabling a cost term to avoid position limits.\n  enum class CSpaceLimitBiasing {\n    AUTO,\n    ENABLE,\n    DISABLE,\n  };\n\n  //! See documentation for `CSpaceLimitBiasing`.\n  //!\n  //! Default: AUTO\n  CSpaceLimitBiasing bfgs_cspace_limit_biasing = CSpaceLimitBiasing::AUTO;\n\n  //! Define the region near c-space limits which will induce penalties when c-space limit biasing\n  //! is active.\n  //!\n  //! The region is defined as a fraction of the c-space span for each c-space coordinate.\n  //! For example, imagine a 2d system for which the limits of the first coordinate are [-5, 5] and\n  //! the limits of the second coordinate are [0, 2]. If the `cspace_limit_penalty_region` is set\n  //! to 0.1, then the size of each penalty region for first coordinate will be:\n  //!   0.1 * (5 - (-5)) = 1\n  //! Similarly, the size of each penalty region for the second coordinate will be:\n  //!   0.1 * (2 - 0) = 0.2\n  //! This means that for the first coordinate, c-space limit errors will be active from [-5, -4]\n  //! and [4, 5]. For the second coordinate, c-space limit errors will be active from [0, 0.2] and\n  //! [1.8, 2]\n  //!\n  //! Must be in the range [0, 0.5].\n  //!\n  //! Default: 0.01\n  double bfgs_cspace_limit_penalty_region = 0.01;\n\n  //! Relative weight applied to c-space limit error (if active).\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 1.0\n  double bfgs_cspace_limit_biasing_weight = 1.0;\n\n  //! Weight for the relative importance of position error term when performing BFGS descent.\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 1.0\n  double bfgs_position_weight = 1.0;\n\n  //! Weight for the relative importance of orientation error term when performing BFGS descent.\n  //!\n  //! Must be non-negative.\n  //!\n  //! Default: 100.0\n  double bfgs_orientation_weight = 100.0;\n};\n\n//! Results from solving inverse kinematics.\nstruct CUMO_EXPORT IkResults {\n  //! Indicate whether a valid `cspace_position` within the tolerances specified in the\n  //! `IkConfig` has been found.\n  bool success;\n\n  //! If `success` is set to `true`, `cspace_position` will contain a valid joint configuration\n  //! that corresponds to the `target_pose` passed to the IK solver within the tolerances\n  //! specified in the `IkConfig`.\n  Eigen::VectorXd cspace_position;\n\n  //! Position error (L2-norm) of the task space pose corresponding to the returned\n  //! `cspace_position`\n  //!\n  //! If `success` is set to `true`, the `position_error` will be less than or equal to the\n  //! `position_tolerance` set in the `IkConfig`.\n  double position_error;\n\n  //! X-axis orientation error (L2-norm) of the task space pose corresponding to the returned\n  //! `cspace_position`\n  //!\n  //! If `success` is set to `true`, the `x_axis_orientation_error` will be less than or equal to\n  //! the `orientation_tolerance` set in the `IkConfig`.\n  double x_axis_orientation_error;\n\n  //! Y-axis orientation error (L2-norm) of the task space pose corresponding to the returned\n  //! `cspace_position`\n  //!\n  //! If `success` is set to `true`, the `y_axis_orientation_error` will be less than or equal to\n  //! the `orientation_tolerance` set in the `IkConfig`.\n  double y_axis_orientation_error;\n\n  //! Z-axis orientation error (L2-norm) of the task space pose corresponding to the returned\n  //! `cspace_position`\n  //!\n  //! If `success` is set to `true`, the `z_axis_orientation_error` will be less than or equal to\n  //! the `orientation_tolerance` set in the `IkConfig`.\n  double z_axis_orientation_error;\n\n  //! The number of unique c-space positions that were used for attempting cyclic coordinate\n  //! descent prior to the IK solver terminating.\n  //!\n  //! If `success` is set to `true`, this will be the number\n  //! of descents that were attempted in order to get a `cspace_position` within the provided\n  //! tolerance. If `success` is set to `false`, the number of descents will be set to the\n  //! `max_num_descents` set in the `IkConfig`.\n  int num_descents;\n};\n\n//! Attempt to solve inverse kinematics.\n//!\n//! A `target_pose` is provided for the task space `target_frame` and the solver attempts to find\n//! a corresponding set of c-space coordinates. A valid c-space configuration must not exceed\n//! joint limits set in the kinematic structure and must be the target pose tolerances specified\n//! within `config`.\n//!\n//! If the `target_frame` is selected such that some of the c-space coordinates do not affect the\n//! pose of the target frame, these \"downstream\" coordinates will be ignored by the IK solver. For\n//! the case where the successful descent was started from a seed provided in `config.cspace_seeds`,\n//! the \"downstream\" coordinates will not be altered from the provided seed. For the case where\n//! a random seed is used, the \"downstream\" coordinates will be returned with values between their\n//! respective c-space position limits. This behavior is intended to support cases such as branched\n//! kinematic structures (i.e., to solve for palm pose of a robotic system with an arm and multiple\n//! fingers) or a redundant serial linkage for which it is desired to set a pose of a frame not\n//! rigidly fixed to the end effector.\n//!\n//! This implementation of cyclic coordinate descent (CCD) is based on \"A Combined Optimization\n//! Method for Solving the Inverse Kinematics Problem of Mechanical Manipulators\" (1991) by\n//! Wang et al.\n//! Ref: http://web.cse.ohio-state.edu/~parent.1/classes/788/Sp06/ReferenceMaterial/IK/WC91.pdf\n//!\n//! As described by Wang et al., this \"combined\" method uses cyclic coordinate descent (CCD) as a\n//! quick method to find a feasible c-space position that is in the neighborhood of an exact\n//! solution to the inverse kinematics problem. These nearby c-space positions are then used to\n//! \"warm start\" the Broyden-Fletcher-Goldfarb-Shanno (BFGS) algorithm. Given a point sufficiently\n//! near an exact solution, the BFGS algorithm is able to leverage an estimate of curvature to\n//! rapidly converge to a very accurate approximation of the exact solution.\n//!\n//! Concise explanations of the CCD algorithm with interactive demonstrations are available at:\n//! [1] https://zalo.github.io/blog/inverse-kinematics/\n//! [2] http://rodolphe-vaillant.fr/?e=114\nCUMO_EXPORT IkResults SolveIk(const Kinematics &kinematics,\n                              const Pose3 &target_pose,\n                              const Kinematics::FrameHandle &target_frame,\n                              const IkConfig &config);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/kinematics.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2019-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for querying a kinematic structure.\n\n#pragma once\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/pose3.h\"\n\nnamespace cumotion {\n\n//! Class representing the mapping from configuration space to coordinate frames that are rigidly\n//! attached to the kinematic structure.\nclass CUMO_EXPORT Kinematics {\n public:\n  //! Opaque handle to a frame\n  struct CUMO_EXPORT FrameHandle {\n    class Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  virtual ~Kinematics() = default;\n\n  //! Return the number of configuration space coordinates for the kinematic structure.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Return the name of a given configuration space coordinate of the kinematic structure.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `coord_index` < 0, *OR*\n  //!   2. `coord_index` >= `numCSpaceCoords()`.\n  [[nodiscard]] virtual std::string cSpaceCoordName(int coord_index) const = 0;\n\n  //! Lower and upper limits for a configuration space coordinate.\n  struct CUMO_EXPORT Limits {\n    double lower;  //!< lower limit\n    double upper;  //!< upper limit\n  };\n\n  //! Return the limits of a given configuration space coordinate of the kinematic structure.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `coord_index` < 0, *OR*\n  //!   2. `coord_index` >= `numCSpaceCoords()`.\n  [[nodiscard]] virtual Limits cSpaceCoordLimits(int coord_index) const = 0;\n\n  //! Determine whether a specified configuration space position is within limits for each\n  //! coordinate.\n  //!\n  //! If `log_warnings` is set to `true` and `cspace_position` is outside limits, a warning will be\n  //! logged indicating which coordinates are outside limits.\n  virtual bool withinCSpaceLimits(const Eigen::VectorXd &cspace_position,\n                                  bool log_warnings) const = 0;\n\n  //! Return the velocity limit of a given configuration space coordinate of the kinematic\n  //! structure.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `coord_index` < 0, *OR*\n  //!   2. `coord_index` >= `numCSpaceCoords()`.\n  [[nodiscard]] virtual double cSpaceCoordVelocityLimit(int coord_index) const = 0;\n\n  //! Return the acceleration limit of a given configuration space coordinate of the kinematic\n  //! structure.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `coord_index` < 0, *OR*\n  //!   2. `coord_index` >= `numCSpaceCoords()`.\n  [[nodiscard]] virtual double cSpaceCoordAccelerationLimit(int coord_index) const = 0;\n\n  //! Return the jerk limit of a given configuration space coordinate of the kinematic structure.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `coord_index` < 0, *OR*\n  //!   2. `coord_index` >= `numCSpaceCoords()`.\n  [[nodiscard]] virtual double cSpaceCoordJerkLimit(int coord_index) const = 0;\n\n  //! Return all the frame names in the kinematic structure.\n  [[nodiscard]] virtual const std::vector<std::string> &frameNames() const = 0;\n\n  //! Return the name of the given `frame`.\n  [[nodiscard]] virtual const std::string &frameName(const FrameHandle &frame) const = 0;\n\n  //! Return a handle representing the frame with the given `frame_name`.\n  //!\n  //! A fatal error will be logged if `frame_name` is not a valid frame name.\n  [[nodiscard]] virtual FrameHandle frame(const std::string &frame_name) const = 0;\n\n  //! Return a handle representing the base frame of the kinematic structure.\n  [[nodiscard]] virtual FrameHandle baseFrame() const = 0;\n\n  //! Return the pose of the given `frame` with respect to `reference_frame` at the given\n  //! `cspace_position`.\n  [[nodiscard]] virtual Pose3 pose(const Eigen::VectorXd &cspace_position,\n                                   const FrameHandle &frame,\n                                   const FrameHandle &reference_frame) const = 0;\n\n  //! Return the pose of the given `frame` with respect to the base frame (i.e., `baseFrame()`) at\n  //! the given `cspace_position`.\n  [[nodiscard]] virtual Pose3 pose(const Eigen::VectorXd &cspace_position,\n                                   const FrameHandle &frame) const = 0;\n\n  //! Return the position of the origin of the given `frame` with respect to `reference_frame` at\n  //! the given `cspace_position`.\n  [[nodiscard]] virtual Eigen::Vector3d position(const Eigen::VectorXd &cspace_position,\n                                                 const FrameHandle &frame,\n                                                 const FrameHandle &reference_frame) const = 0;\n\n  //! Return the position of the origin of the given `frame` with respect to the base frame\n  //! (i.e., `baseFrame()`) at the given `cspace_position`.\n  [[nodiscard]] virtual Eigen::Vector3d position(const Eigen::VectorXd &cspace_position,\n                                                 const FrameHandle &frame) const = 0;\n\n  //! Return the orientation of the given `frame` with respect to `reference_frame` at the given\n  //! `cspace_position`.\n  [[nodiscard]] virtual Rotation3 orientation(const Eigen::VectorXd &cspace_position,\n                                              const FrameHandle &frame,\n                                              const FrameHandle &reference_frame) const = 0;\n\n  //! Return the orientation of the given `frame` with respect to the base frame\n  //! (i.e., `baseFrame()`) at the given `cspace_position`.\n  [[nodiscard]] virtual Rotation3 orientation(const Eigen::VectorXd &cspace_position,\n                                              const FrameHandle &frame) const = 0;\n\n  //! Return the Jacobian of the origin of the given `frame` with respect to the base frame\n  //! (i.e., `baseFrame()`) at the given `cspace_position`.\n  //!\n  //! The returned Jacobian is a 6 x N matrix where N is the `numCSpaceCoords`. Column `i` of the\n  //! returned Jacobian represents the perturbation contribution to origin of `frame` from the\n  //! `i`th c-space element in the coordinates of the base frame. For each column, the first three\n  //! elements represent the translational portion and the last three elements represent the\n  //! rotational portion.\n  [[nodiscard]] virtual Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian(\n      const Eigen::VectorXd &cspace_position,\n      const FrameHandle &frame) const = 0;\n\n  //! Return the Jacobian of the position of the origin of the given `frame` with respect to the\n  //! base frame (i.e., `baseFrame()`) at the given `cspace_position`.\n  //!\n  //! The returned Jacobian is a 3 x N matrix where N is the `numCSpaceCoords`. Column `i` of the\n  //! returned Jacobian represents the perturbation contribution to position of the origin of\n  //! `frame` from the `i`th c-space element in the coordinates of the base frame.\n  [[nodiscard]] virtual Eigen::Matrix<double, 3, Eigen::Dynamic> positionJacobian(\n      const Eigen::VectorXd &cspace_position,\n      const FrameHandle &frame) const = 0;\n\n  //! Return the Jacobian of the orientation of the given `frame` with respect to the base frame\n  //! (i.e., `baseFrame()`) at the given `cspace_position`.\n  //!\n  //! The returned Jacobian is a 3 x N matrix where N is the `numCSpaceCoords`. Column `i` of the\n  //! returned Jacobian represents the perturbation contribution to orientation of `frame` from the\n  //! `i`th c-space element in the coordinates of the base frame.\n  [[nodiscard]] virtual Eigen::Matrix<double, 3, Eigen::Dynamic> orientationJacobian(\n      const Eigen::VectorXd &cspace_position,\n      const FrameHandle &frame) const = 0;\n\n  //! Return the fixed transform between `frame` and its parent frame.\n  //!\n  //! Internally, the `Kinematics` structure is represented as a directed rooted tree. Each frame\n  //! has a single parent frame (other than the root which by definition has no parent). The edge\n  //! from parent frame to child frame represents both a fixed transform (i.e., a rigid linkage) AND\n  //! a \"joint\" transform (revolute, prismatic, or fixed). The root frame, while not having a\n  //! parent, maintains a fixed transform that defines its pose relative to global coordinates.\n  //!\n  //! This function returns the fixed transform that precedes the \"joint\" transform. The returned\n  //! `Pose3` is expressed relative to the parent frame. For example, if the `wrist_frame` is a\n  //! child of the `elbow_frame`, the returned pose would be the pose of the wrist joint expressed\n  //! in the local coordinates of the `elbow_frame`.\n  //!\n  //! NOTE: In general, the returned pose will NOT be the pose of the child frame expressed in the\n  //! local coordinates of the parent frame, as the \"joint\" transform is NOT included in the\n  //! returned transform, and the \"joint\" transform, in general, will not be identity. In order to\n  //! return the full transform from parent frame to child frame for a given c-space configuration,\n  //! the `pose()` function should be used with the child frame input for `frame` and the parent\n  //! frame input for `reference_frame`.\n  [[nodiscard]] virtual Pose3 getPrecedingFixedTransform(const FrameHandle &frame) const = 0;\n\n  //! Set the fixed transform between `frame` and its parent frame to `transform`.\n  //!\n  //! Internally, the `Kinematics` structure is represented as a directed rooted tree. Each frame\n  //! has a single parent frame (other than the root which by definition has no parent). The edge\n  //! from parent frame to child frame represents both a fixed transform (i.e., a rigid linkage) AND\n  //! a \"joint\" transform (revolute, prismatic, or fixed). The root frame, while not having a\n  //! parent, maintains a fixed transform that defines its pose relative to global coordinates; it\n  //! is valid to use this function to set that tranform to pose the base of the robot relative to\n  //! the global frame.\n  //!\n  //! This function sets the fixed transform that precedes the \"joint\" transform. The input\n  //! `transform` is expressed relative to the parent frame. For example, if the `wrist_frame` is a\n  //! child of the `elbow_frame`, the input `transform` would define the pose of the wrist joint\n  //! expressed in the local coordinates of the `elbow_frame`.\n  virtual void setPrecedingFixedTransform(const FrameHandle &frame, const Pose3 &transform) = 0;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/linear_cspace_path.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n#include <vector>\n\n#include \"cumotion/cspace_path.h\"\n#include \"cumotion/cspace_path_spec.h\"\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Represent a path linearly interpolated through configuration space (i.e., c-space) waypoints.\n//!\n//! This path is parameterized by independent variable `s` and will be continuous but not\n//! (in general) smooth.\nclass CUMO_EXPORT LinearCSpacePath : public CSpacePath {\n public:\n  virtual ~LinearCSpacePath() = default;\n\n  //! Return the number of configuration space coordinates for the path.\n  [[nodiscard]] int numCSpaceCoords() const override = 0;\n\n  //! Return the domain for the path.\n  [[nodiscard]] Domain domain() const override = 0;\n\n  //! Evaluate the path at the given `s`.\n  //!\n  //! A fatal error is logged if `s` is outside of the path `domain()`.\n  [[nodiscard]] Eigen::VectorXd eval(double s) const override = 0;\n\n  //! Return the total distance accumulated along the path, where distance is computed using the\n  //! L2-norm.\n  //!\n  //! This length is not, in general, equal to the distance between the configurations at the lower\n  //! and upper bounds of the domain.\n  [[nodiscard]] double pathLength() const override = 0;\n\n  //! Return the minimum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These minimum position values are not, in general, synchronous. Instead, they represent\n  //!       the minimum position independently achieved by each coordinate.\n  [[nodiscard]] Eigen::VectorXd minPosition() const override = 0;\n\n  //! Return the maximum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These maximum position values are not, in general, synchronous. Instead, they represent\n  //!       the maximum position independently achieved by each coordinate.\n  [[nodiscard]] Eigen::VectorXd maxPosition() const override = 0;\n\n  //! Return all of the waypoints through which the path is linearly interpolated (including the\n  //! initial and final c-space configurations).\n  [[nodiscard]] virtual std::vector<Eigen::VectorXd> waypoints() const = 0;\n};\n\n//! Create a `LinearCSpacePath` from a c-space path specification.\nCUMO_EXPORT std::unique_ptr<LinearCSpacePath> CreateLinearCSpacePath(\n    const CSpacePathSpec &cspace_path_spec);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/motion_planner.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface to cuMotion's global motion planning implementation.\n\n#pragma once\n\n#include <filesystem>\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Configuration parameters for a `MotionPlanner`.\nclass CUMO_EXPORT MotionPlannerConfig {\n public:\n  virtual ~MotionPlannerConfig() = default;\n\n  //! Indicate lower and upper limits for a coordinate.\n  struct CUMO_EXPORT Limit {\n    double lower;\n    double upper;\n  };\n\n  //! Specify the value for a given parameter.\n  //!\n  //! The required `ParamValue` constructor for each param is detailed in the\n  //! documentation for `setParam()`.\n  struct CUMO_EXPORT ParamValue {\n    //! Create `ParamValue` from `int`.\n    ParamValue(int value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `double`.\n    ParamValue(double value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `Eigen::Vector3d`.\n    ParamValue(const Eigen::Vector3d &value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `std::vector<double>`.\n    ParamValue(const std::vector<double> &value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `bool`.\n    ParamValue(bool value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `const char*`.\n    ParamValue(const char *value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `std::string`.\n    ParamValue(const std::string &value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `std::vector<Limit>`.\n    ParamValue(const std::vector<Limit> &value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Set the value of the parameter.\n  //!\n  //! `setParam` returns `true` if the param has been successfully updated and `false` if an error\n  //! has occurred (e.g., invalid parameter).\n  //!\n  //! The following parameters can be set for `MotionPlanner`:\n  //!\n  //! `seed` [`int`]\n  //!   - Used to initialize random sampling.\n  //!   - `seed` must be positive.\n  //!   - Default: 1234\n  //!\n  //! `step_size` [`double`]\n  //!   - Step size for tree extension and (optionally) for discretization of `interpolated_path`\n  //!     in `Results`\n  //!   - It is assumed that a straight path connecting two valid c-space configurations with\n  //!     separation distance <= `step_size` is a valid edge, where separation distance is defined\n  //!     as the L2-norm of the difference between the two configurations.\n  //!   - `step_size` must be positive.\n  //!   - Default: 0.05\n  //!\n  //! `max_iterations` [`int`]\n  //!   - Maximum number of iterations of tree extensions that will be attempted.\n  //!   - If `max_iterations` is reached without finding a valid path, the `Results` will\n  //!     indicate `path_found` is `false` and `path` will be an empty vector.\n  //!   - `max_iterations` must be positive.\n  //!   - Default: 10,000\n  //!\n  //! `max_sampling` [`int`]\n  //!   - Maximum number of configurations used for sampling in the environment setup.\n  //!   - `max_sampling` must be positive.\n  //!   - Default: 10'000\n  //!\n  //! `distance_metric_weights` [`std::vector<double>`]\n  //!   - When selecting a node for tree extension, the closest node is defined using a weighted,\n  //!     squared L2-norm:\n  //!       distance = (q0 - q1)^T * W * (q0 - q1)\n  //!       where q0 and q1 represent two configurations and W is a diagonal matrix formed from\n  //!       `distance_metric_weights`.\n  //!   - The length of the `distance_metric_weights` must be equal to the number of c-space\n  //!     coordinates for the robot and each weight must be positive.\n  //!   - NOTE: In general, it is recommended to use a vector of ones for `distance_metric_weights`\n  //!           (i.e., unweighted squared L2-norm). Doing so enables significant performance\n  //!           optimization for nearest neighbor searches during geometric planning. Non-unity\n  //!           weights should only be considered if the benefits of the weighted distance metric\n  //!           outweigh the cost of more expensive nearest neighbor searches.\n  //!   - Default: Vector of ones with length set by the number of c-space coordinates in\n  //!     `robot_description`.\n  //!\n  //! `tool_frame_name` [`std::string`]\n  //!   - Indicate the name (from URDF) of the frame to be used for task space planning.\n  //!   - With current implementation, setting a `tool_frame_name` that is not found in the\n  //!     kinematics will throw an exception rather than failing gracefully.\n  //!\n  //! `task_space_limits` [`std::vector<Limit>`]\n  //!   - Task space limits define a bounding box used for sampling task space when planning\n  //!     a path to a task space target.\n  //!   - The specified `task_space_limits` vector should be length 3, corresponding to the xyz\n  //!     dimensions of the bounding box.\n  //!   - Each upper limit must be >= the corresponding lower limit.\n  //!   - Default: Lower limits default to -1 and upper limits to 1.\n  //!\n  //! `enable_self_collision_checking` [`bool`]\n  //!  - Set to `true` to enable self-collision checking during motion planning.\n  //!  - When enabled, configurations that result in self-collision (collision between robot\n  //!    self-collision spheres) will be considered invalid during path planning.\n  //!  - Default: `true`\n  //!\n  //! `enable_cuda_tree` [`bool`]\n  //!   - Set to `true` to enable use of CUDA for storing explored configurations and performing\n  //!     nearest neighbor look-up, or `false` to disable usage of CUDA.\n  //!   - When `enable_cuda_tree` is set to `true`, CUDA will only be used when\n  //!     `distance_metric_weights` is not equal to `Eigen::VectorXd::Ones(cspace_dimension)`.\n  //!   - If set to `true` from the `false` state, default values for `cuda_tree_params` will be\n  //!     used.\n  //!   - If set to `false` from the `true` state, current values for `cuda_tree_params` will be\n  //!     discarded (i.e., if returned to `true`, default rather than previous values will be used).\n  //!   - NOTE: this parameter does NOT need to be set in the YAML configuration file. The presence\n  //!     of a `cuda_tree_params` with corresponding parameters in the configuration file indicates\n  //!     that `enable_cuda_tree` should be set to `true`.\n  //!   - Default: `true`\n  //!\n  //! `cuda_tree_params/max_num_nodes` [`int`]\n  //!   - DEPRECATED\n  //!   - The number of configurations that can be stored in CUDA-accelerated trees is no longer\n  //!     bound by a preset value. Growth is now limited by `max_iterations`, rather than the\n  //!     coupling of `max_iterations` and `cuda_tree_params/max_num_nodes`.\n  //!   - A deprecation warning is logged when `cuda_tree_params/max_num_nodes` is set via\n  //!     `setParam()` or when present in a configuration file. It is recommended to *not*\n  //!     provide a value for this parameter, as it will be ignored.\n  //!\n  //! `cuda_tree_params/max_buffer_size` [`int`]\n  //!   - Maximum number of valid configurations that are buffered on CPU prior to transferring to\n  //!     GPU.\n  //!   - A default value of 30 is recommended for most use-cases.\n  //!   - `cuda_tree_params/max_buffer_size` must be positive.\n  //!   - Default: 30\n  //!\n  //! `cuda_tree_params/num_nodes_cpu_gpu_crossover` [`int`]\n  //!   - Number of valid explored configurations added to tree prior to copying all configurations\n  //!     to GPU and using GPU for nearest neighbor lookup.\n  //!   - A default value of 3000 is recommended for most use-cases.\n  //!   - `cuda_tree_params/num_nodes_cpu_gpu_crossover` must be positive.\n  //!   - Default: 3000\n  //!\n  //!  NOTE: For all of the `cuda_tree_params` listed above, the best values for optimal performance\n  //!        will depend on the number of c-space coordinates, the system hardware (e.g., CPU, GPU,\n  //!        and memory configuration), and software versions (e.g., CUDA, NVIDIA GPU driver, and\n  //!        cuMotion versions). The provided default recommendations are chosen to be appropriate\n  //!        for most expected use-cases.\n  //!\n  //! `cspace_planning_params/exploration_fraction` [`double`]\n  //!   - The c-space planner uses RRT-Connect to try to find a path to a c-space target.\n  //!   - RRT-Connect attempts to iteratively extend two trees (one from the initial configuration\n  //!     and one from the target configuration) until the two trees can be connected. The\n  //!     configuration to which a tree is extended can be either a random sample\n  //!     (i.e., exploration) or a node on the tree to which connection is desired\n  //!     (i.e., exploitation). The `exploration_fraction` controls the fraction of steps that are\n  //!     exploration steps. It is generally recommended to set `exploration_fraction` in range\n  //!     [0.5, 1), where 1 corresponds to a single initial exploitation step followed by only\n  //!     exploration steps. Values of between [0, 0.5) correspond to more exploitation than\n  //!     exploration and are not recommended. If a value outside range [0, 1] is provided, a\n  //!     warning is logged and the value is clamped to range [0, 1].\n  //!   - A default value of 0.5 is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Default: 0.5\n  //!\n  //! `task_space_planning_params/translation_target_zone_tolerance` [`double`]\n  //!   - A configuration has reached the task space translation target when task space position has\n  //!     an L2 Norm within `translation_target_zone_tolerance` of the target.\n  //!   - It is assumed that a valid configuration within the target tolerance can be moved directly\n  //!     to the target configuration using an inverse kinematics solver and linearly stepping\n  //!     towards the solution.\n  //!   - In general, it is recommended that the size of the translation target zone be on the same\n  //!     order of magnitude as the translational distance in task-space corresponding to moving the\n  //!     robot in configuration space by one step with an L2 norm of `step_size`.\n  //!   - Must be > 0.\n  //!   - Default: 0.05\n  //!\n  //! `task_space_planning_params/orientation_target_zone_tolerance` [`double`]\n  //!   - A configuration has reached the task space pose target when task space orientation is\n  //!     within `orientation_target_zone_tolerance` radians and an L2 norm translation\n  //!     within `translation_target_zone_tolerance` of the target.\n  //!   - It is assumed that a valid configuration within the target tolerances can be moved\n  //!     directly to the target configuration using an inverse kinematics solver and linearly\n  //!     stepping towards the solution.\n  //!   - In general, it is recommended that the size of the orientation target zone be on the same\n  //!     order of magnitude as the rotational distance in task-space corresponding to moving the\n  //!     robot in configuration space by one step with an L2 norm of `step_size`.\n  //!   - Must be > 0.\n  //!   - Default: 0.09\n  //!\n  //! `task_space_planning_params/translation_target_final_tolerance` [`double`]\n  //!   - Once a path is found that terminates within `translation_target_zone_tolerance`, an IK\n  //!     solver is used to find a configuration space solution corresponding to the task space\n  //!     target. This solver terminates when the L2-norm of the corresponding task space position\n  //!     is within `translation_target_final_tolerance` of the target.\n  //!   - Note: This solver assumes that if a c-space configuration within\n  //!     `translation_target_zone_tolerance` is found then this c-space configuration can be\n  //!     moved linearly in c-space to the IK solution. If this assumption is NOT met, the returned\n  //!     path will not reach the task space target within the `translation_target_final_tolerance`\n  //!     and an error is logged.\n  //!   - The recommended default value is 1e-4, but in general this value should be set to a\n  //!     positive value that is considered \"good enough\" precision for the specific system.\n  //!   - Default: 1e-4\n  //!\n  //! `task_space_planning_params/orientation_target_final_tolerance` [`double`]\n  //!   - For pose targets, once a path is found that terminates within\n  //!     `orientation_target_zone_tolerance` and `translation_target_zone_tolerance` of the target,\n  //!     an IK solver is used to find a configuration space solution corresponding to the task\n  //!     space target. This solver terminates when the L2-norm of the corresponding task space\n  //!     position is within `orientation_target_final_tolerance` and\n  //!     `translation_target_final_tolerance` of the target.\n  //!   - Note: This solver assumes that if a c-space configuration within the target zone\n  //!     tolerances is found then this c-space configuration can be moved linearly in c-space to\n  //!     the IK solution. If this assumption is NOT met, the returned path will not reach the task\n  //!     space target within the final target tolerances and an error is logged.\n  //!   - The recommended default value is 1e-4, but in general this value should be set to a\n  //!     positive value that is considered \"good enough\" precision for the specific system.\n  //!   - Default: 0.005\n  //!\n  //! `task_space_planning_params/translation_gradient_weight` [`double`]\n  //!   - For pose targets, computed translation and orientation gradients are linearly weighted by\n  //!     `translation_gradient_weight` and `orientation_gradient_weight` to compute a combined\n  //!     gradient step when using the Jacobian Transpose method to guide tree expansion\n  //!     towards a task space target.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Must be > 0.\n  //!   - Default: 1.0\n  //!\n  //! `task_space_planning_params/orientation_gradient_weight` [`double`]\n  //!   - For pose targets, computed translation and orientation gradients are linearly weighted by\n  //!     `translation_gradient_weight` and `orientation_gradient_weight` to compute a combined\n  //!     gradient step when using the Jacobian Transpose method to guide tree expansion\n  //!     towards a task space target.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Must be > 0.\n  //!   - Default: 0.125\n  //!\n  //! `task_space_planning_params/nn_translation_distance_weight` [`double`]\n  //!   - For pose targets, nearest neighbor distances are computed by linearly weighting\n  //!     translation and orientation distance by `nn_translation_distance_weight` and\n  //!     `nn_orientation_distance_weight`.\n  //!   - Nearest neighbor search is used to select the node from which the tree of valid\n  //!     configurations will be expanded.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Must be > 0.\n  //!   - Default: 1.0\n  //!\n  //! `task_space_planning_params/nn_orientation_distance_weight` [`double`]\n  //!   - For pose targets, nearest neighbor distances are computed by linearly weighting\n  //!     translation and orientation distance by `nn_translation_distance_weight` and\n  //!     `nn_orientation_distance_weight`.\n  //!   - Nearest neighbor search is used to select the node from which the tree of valid\n  //!     configurations will be expanded.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Must be > 0.\n  //!   - Default: 0.125\n  //!\n  //! `task_space_planning_params/task_space_exploitation_fraction` [`double`]\n  //!   - Fraction of iterations for which tree is extended towards target position in task space.\n  //!   - Must be in range [0, 1]. Additionally, the sum of `task_space_exploitation_fraction` and\n  //!     `task_space_exploration_fraction` must be <= 1.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Default: 0.4\n  //!\n  //! `task_space_planning_params/task_space_exploration_fraction` [`double`]\n  //!   - Fraction of iterations for which tree is extended towards random position in task space.\n  //!   - Must be in range [0, 1]. Additionally, the sum of `task_space_exploitation_fraction` and\n  //!     `task_space_exploration_fraction` must be <= 1.\n  //!   - The default value is recommended as a starting value for initial testing with a given\n  //!     system.\n  //!   - Default: 0.1\n  //!\n  //!  NOTE: The remaining fraction beyond `task_space_exploitation_fraction` and\n  //!        `task_space_exploration_fraction` is a `cspace_exploration_fraction` that is\n  //!        implicitly defined as:\n  //!          1 - (`task_space_exploitation_fraction` + `task_space_exploration_fraction`\n  //!        In general, easier path searches will take less time with higher exploitation fraction\n  //!        while more difficult searches will waste time if the exploitation fraction is too high\n  //!        and benefit from greater combined exploration fraction.\n  //!\n  //! `task_space_planning_params/max_extension_substeps_away_from_target` [`int`]\n  //!  - Maximum number of Jacobian transpose gradient descent substeps that may be taken\n  //!    while the end effector is away from the task-space target.\n  //!  - The threshold for nearness is determined by the\n  //!    `extension_substep_target_region_scale_factor` parameter.\n  //!  - The default value is recommended as a starting value for initial testing with a given\n  //!    system.\n  //!  - Must be >= 0.\n  //!  - Default: 6\n  //!\n  //! `task_space_planning_params/max_extension_substeps_near_target` [`int`]\n  //!  - Maximum number of Jacobian transpose gradient descent substeps that may be taken\n  //!    while the end effector is near the task-space target.\n  //!  - The threshold for nearness is determined by the\n  //!    `extension_substep_target_region_scale_factor` parameter.\n  //!  - The default value is recommended as a starting value for initial testing with a given\n  //!    system.\n  //!  - Must be >= 0.\n  //!  - Default: 50\n  //!\n  //! `task_space_planning_params/extension_substep_target_region_scale_factor` : [`double`]\n  //!  - A scale factor used to determine whether the end effector is close enough to the target\n  //!    to change the amount of gradient descent substeps allowed when adding a node in RRT.\n  //!  - The `max_extension_substeps_near_target` parameter is used when the distance\n  //!    (i.e., L2 norm) between the end effector and target position is less than\n  //!    `extension_substep_target_region_scale_factor` * `x_zone_target_tolerance`.\n  //!  - Must be greater than or equal to 1.0; a value of 1.0 effectively disables the\n  //!   `max_extension_substeps_near_target` parameter.\n  //!  - The default value is recommended as a starting value for initial testing with a given\n  //!    system.\n  //!  - Default: 2.0\n  //!\n  //! `task_space_planning_params/unexploited_nodes_culling_scalar` [`double`]\n  //!  - Scalar controlling culling of unexploited nodes during task-space planning.\n  //!  - Must be >= 0.0.\n  //!  - Default: 1.0\n  //!\n  //! `task_space_planning_params/gradient_substep_size` [`double`]\n  //!  - Size of each gradient-descent substep when following the Jacobian Transpose direction.\n  //!  - Must be > 0.0.\n  //!  - Default: 0.025\n  virtual bool setParam(const std::string &param_name, ParamValue value) = 0;\n};\n\n//! Load a set of `MotionPlanner` configuration parameters from file.\n//!\n//! These parameters are combined with `robot_description`, `tool_frame_name`, and `world_view` to\n//! create a configuration for motion planning.\n//!\n//! A fatal error will be logged if `motion_planner_config_file` is missing any required parameters.\n//! If any provided parameter values are invalid:\n//! - When recoverable, they will be clipped to their expected value range with a warning on calling\n//!   `CreateMotionPlanner()`.\n//! - When not recoverable, a fatal error will be logged.\n//!\n//! A configuration will *NOT* be created if:\n//!   1. `robot_description` is invalid, *OR*\n//!   2. `tool_frame_name` is not a valid frame in `robot_description`.\n//! In the case of failure, a non-fatal error will be logged and a `nullptr` will be returned.\nCUMO_EXPORT std::unique_ptr<MotionPlannerConfig> CreateMotionPlannerConfigFromFile(\n    const std::filesystem::path &motion_planner_config_file,\n    const RobotDescription &robot_description,\n    const std::string &tool_frame_name,\n    const WorldViewHandle &world_view);\n\n//! Use default parameters to create a configuration for motion planning.\n//!\n//! See `MotionPlannerConfig::setParam()` for the default parameter values.\n//!\n//! A configuration will *NOT* be created if:\n//!   1. `robot_description` is invalid, *OR*\n//!   2. `tool_frame_name` is not a valid frame in `robot_description`.\n//!\n//! In the case of failure, a non-fatal error will be logged and a `nullptr` will be returned.\nCUMO_EXPORT std::unique_ptr<MotionPlannerConfig> CreateDefaultMotionPlannerConfig(\n    const RobotDescription &robot_description,\n    const std::string &tool_frame_name,\n    const WorldViewHandle &world_view);\n\n//! Interface class for planning collision-free paths for robotic manipulators.\n//! Supports both configuration space (i.e., joint position) targets and task space (e.g., end\n//! effector position) targets.\nclass CUMO_EXPORT MotionPlanner {\n public:\n  virtual ~MotionPlanner() = default;\n\n  //! Results from a path search.\n  struct CUMO_EXPORT Results {\n    //! Indicate whether a collision-free path was found.\n    //! If `false`, `path` and `interpolated_path` will be empty.\n    bool path_found;\n\n    //! Minimal representation of collision-free path.\n    //! Each vector represents a knot in configuration space, where successive knots can be linearly\n    //! interpolated in configuration space to generate a collision-free path.\n    std::vector<Eigen::VectorXd> path;\n\n    //! Interpolation of `path` such that successive knots are (in general) one `step_size` apart\n    //! from each other (where distance is defined as L2-norm in c-space).\n    //! NOTE: Each straight segment from `path` above are interpolated individually, where the\n    //!       distance between the last two returned knots for each segment will be less than one\n    //!       `step_size` from each other.\n    //! The `interpolated_path` will only be populated if `generate_interpolated_path` is set to\n    //! `true` when generating a path plan.\n    std::vector<Eigen::VectorXd> interpolated_path;\n  };\n\n  //! Attempt to find a path from initial configuration space position (`q_initial`) to a\n  //! configuration space target (`q_target`).\n  //! `generate_interpolated_path` determines whether `interpolated_path` will be populated in\n  //! `Results`.\n  virtual Results planToCSpaceTarget(const Eigen::VectorXd &q_initial,\n                                     const Eigen::VectorXd &q_target,\n                                     bool generate_interpolated_path = false) = 0;\n\n  //! Attempt to find a path from initial configuration space position (`q_initial`) to a task\n  //! space translation target (`translation_target`).\n  //! `generate_interpolated_path` determines whether `interpolated_path` will be populated in\n  //! `Results`.\n  virtual Results planToTranslationTarget(const Eigen::VectorXd &q_initial,\n                                          const Eigen::Vector3d &translation_target,\n                                          bool generate_interpolated_path = false) = 0;\n\n  //! Attempt to find a path from initial configuration space position (`q_initial`) to a task\n  //! space pose target (`pose_target`).\n  //! `generate_interpolated_path` determines whether `interpolated_path` will be populated in\n  //! `Results`.\n  virtual Results planToPoseTarget(const Eigen::VectorXd &q_initial,\n                                   const Pose3 &pose_target,\n                                   bool generate_interpolated_path = false) = 0;\n\n  //! Reset all internal state so that subsequent planning calls produce the same results as they\n  //! did immediately after construction, given the same inputs.\n  virtual void reset() = 0;\n};\n\n//! Create a `MotionPlanner` with the given `config`.\nCUMO_EXPORT std::unique_ptr<MotionPlanner> CreateMotionPlanner(const MotionPlannerConfig &config);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/obstacle.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Obstacles represent 3d geometries that can be added to `cumotion::World`.\n//!\n//! See `cumotion/world.h` for usage.\nclass CUMO_EXPORT Obstacle {\n public:\n  //! Indicates what geometric primitive the obstacle represents.\n  //!\n  //! Each `Obstacle::Type` has one or more attributes that can be set via `setAttribute()`,\n  //! detailed in the documentation for the corresponding enum value.\n  enum class Type {\n    //! A cylinder with rounded ends.\n    //!\n    //! The capsule is oriented along the z-axis.\n    //!\n    //! Origin: Geometric center, with the radius in the x-y plane, and spheres placed equidistant\n    //!         along the z-axis.\n    //!\n    //! Attributes:\n    //! - RADIUS: Radius of the capsule.\n    //!           Default: 0.5\n    //! - HEIGHT: Height of the capsule, defined to be the distance between the centers of the two\n    //!           spheres defining the capsule.\n    //!           Default: 1.0\n    CAPSULE,\n\n    //! An axis-aligned cuboid (i.e., rectangular prism).\n    //!\n    //! The cuboid is constructed to be aligned with the axes of its local coordinate frame.\n    //!\n    //! Origin: Geometric center.\n    //!\n    //! Attributes:\n    //! - SIDE_LENGTHS: Dimensions along each axis (in the local obstacle frame). Each value\n    //!                 represents the full extent (i.e., \"length\") of the cuboid, not the\n    //!                 half-length.\n    //!                 Default: {1.0, 1.0, 1.0}\n    CUBOID,\n\n    //! A sphere.\n    //!\n    //! Origin: The center of the sphere.\n    //!\n    //! Attributes:\n    //! - RADIUS: The radius of the sphere.\n    //!           Default: 0.5\n    SPHERE,\n\n    //! An axis-aligned uniform grid of signed distances.\n    //!\n    //! Origin: Center of the grid.\n    //!\n    //! Attributes:\n    //! - GRID: Defines the dimensions and resolution of the SDF.\n    //!         A grid precision of `Grid::Precision::DOUBLE` is unsupported and will result in a\n    //!         fatal error if specified.\n    SDF\n  };\n\n  //! `Attribute`s are used to modify obstacles from their default geometry.\n  //!\n  //! Each `Attribute` may be applicable to one or more `Obstacle::Type`s. Details about which\n  //! `Attribute` can be used with which `Obstacle::Type` are included in documentation for\n  //! `Obstacle::Type`.\n  //!\n  //! Each `Attribute` has a required type for the `AttributeValue` used in conjunction with\n  //! `setAttribute()`, detailed in the documentation for each enum value below.\n  enum class Attribute {\n    //! The height of an obstacle (typically aligned with the z-axis).\n    //!\n    //! Data Type: `double`\n    HEIGHT,\n\n    //! The radius of an obstacle.\n    //!\n    //! Data Type: `double`\n    RADIUS,\n\n    //! The dimensions of the obstacle along the cardinal axes.\n    //!\n    //! Data Type: `Eigen::Vector3d`\n    SIDE_LENGTHS,\n\n    //! An axis-aligned regular grid of points.\n    //!\n    //! Data Type: `Obstacle::Grid`\n    GRID\n  };\n\n  //! Used to specify the defining attributes for a grid of voxels that covers an axis-aligned\n  //! rectangular region of the workspace.\n  //!\n  //! `Grid` fully describes how the grid values passed to `World::setGridValuesFromHost()` and\n  //! `World::setGridValuesFromDevice()` will be interpreted.  An `Attribute::GRID` specifies a grid\n  //! of voxels that each contain a scalar value.  Each voxel has a fixed workspace position\n  //! associated with its value that is implicitly defined by the parameters in `Grid`.\n  struct CUMO_EXPORT Grid {\n    //! Floating-point precision of grid data.\n    //!\n    //! Underlying integer values correspond to the size of each floating-point type in bytes.\n    enum class Precision {\n      HALF = 2,\n      FLOAT = 4,\n      DOUBLE = 8\n    };\n\n    //! A `Grid` covers a rectangular region in the workspace whose minimal coordinate is\n    //! the origin and whose voxel positions are determined by `num_voxels_x`, `num_voxels_y`,\n    //! `num_voxels_z`, and `voxel_size`.\n    //!\n    //! The values associated with a `Grid` correspond to the center of each voxel.  The minimal\n    //! corner of the minimal voxel in a `Grid` rests at the origin.  I.e., the origin is not at\n    //! a voxel center; it is at a voxel corner.\n    //!\n    //! Voxels in a `Grid` have length `voxel_size` along each axis.\n    //!\n    //! Grid contains `num_voxels_x` voxels along the X dimension, `num_voxels_y` voxels along the Y\n    //! dimension, and `num_voxels_z` voxels along the Z dimension.  This implies that the maximal\n    //! position in the region of the workspace that contains a `Grid` is\n    //! `voxel_size * [num_voxels_x, num_voxels_y, num_voxels_z]`.\n    //!\n    //! When grid values are set for an obstacle, up to two copies of the data will be maintained\n    //! at separately specified levels of precision.  For example, it is possible to pass a set of\n    //! grid values of type `double` and to specify that a set of grid values of type `double`\n    //! should be kept in host (CPU) memory for use in distance queries made on the host, along with\n    //! a set of grid values of type `float` kept resident in device (GPU) memory for use in\n    //! distance queries made on the device.\n    Grid(int num_voxels_x,\n         int num_voxels_y,\n         int num_voxels_z,\n         double voxel_size,\n         Precision host_precision,\n         Precision device_precision)\n        : num_voxels_x(num_voxels_x),\n          num_voxels_y(num_voxels_y),\n          num_voxels_z(num_voxels_z),\n          voxel_size(voxel_size),\n          host_precision(host_precision),\n          device_precision(device_precision) {}\n\n    //! The number of voxels along the X dimension of `Grid`.\n    int num_voxels_x;\n\n    //! The number of voxels along the Y dimension of `Grid`.\n    int num_voxels_y;\n\n    //! The number of voxels along the Z dimension of `Grid`.\n    int num_voxels_z;\n\n    //! The size of the voxels along each dimension.\n    double voxel_size;\n\n    //! The type of data for storing a set of grid values on CPU.\n    Precision host_precision;\n\n    //! The type of data for storing a set of grid values on GPU.\n    Precision device_precision;\n  };\n\n  //! Used to specify the value for a given `Attribute`.\n  //!\n  //! The required `AttributeValue` constructor for each `Attribute` is detailed in the\n  //! documentation for `Attribute`.\n  struct CUMO_EXPORT AttributeValue {\n    //! Create `AttributeValue` from `double`.\n    AttributeValue(double value);  // NOLINT Allow implicit conversion\n\n    //! Create `AttributeValue` from `Eigen::Vector3d`.\n    AttributeValue(const Eigen::Vector3d &value);  // NOLINT Allow implicit conversion\n\n    //! Create `AttributeValue` from `Grid`.\n    AttributeValue(const Grid &value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  virtual ~Obstacle() = default;\n\n  //! Set attribute for obstacle.\n  //!\n  //! Available attributes are based on the `Obstacle::Type` and detailed in documentation for\n  //! `Obstacle::Type`.\n  //!\n  //! Example usage:\n  //!     my_sphere.setAttribute(Obstacle::Attribute::RADIUS, 3.0);\n  //!     my_cuboid.setAttribute(Obstacle::Attribute::SIDE_LENGTHS, Eigen::Vector3d(2.0, 3.0, 1.0));\n  virtual void setAttribute(Attribute attribute, const AttributeValue &value) = 0;\n\n  //! Return the `Obstacle::Type` of this obstacle.\n  [[nodiscard]] virtual Type type() const = 0;\n};\n\n//! Create an obstacle with the given `type`.\n//!\n//! Available attributes and default attribute values for the given `type` are included in the\n//! documentation for `Obstacle::Type`.\nCUMO_EXPORT std::unique_ptr<Obstacle> CreateObstacle(Obstacle::Type type);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/path_conversion.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n\n#include \"cumotion/composite_path_spec.h\"\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/ik_solver.h\"\n#include \"cumotion/kinematics.h\"\n#include \"cumotion/linear_cspace_path.h\"\n\nnamespace cumotion {\n\n//! Configuration parameters for converting a `TaskSpacePathSpec` into a series of c-space\n//! configurations.\nstruct CUMO_EXPORT TaskSpacePathConversionConfig {\n  //! Create default set of configuration parameters for converting a `TaskSpacePathSpec` into a\n  //! series of c-space configurations.\n  //!\n  //! Default parameters are expected to work well for converting most task space paths to c-space\n  //! paths.\n  //!\n  //! If a tighter tolerance for adherence to the task space path is desired, then the\n  //! `min_position_deviation` and/or `max_position_deviation` can be decreased (at the expense of\n  //! more computational cost and more c-space waypoints which will likely lead to trajectories with\n  //! longer time-spans). Conversely, tolerances can be loosened for faster path conversion\n  //! and/or (likely) faster trajectories.\n  //!\n  //! Beyond (optionally) adjusting position deviations, it is unlikely that other parameters will\n  //! need to be modified from the provided default values.\n  TaskSpacePathConversionConfig() = default;\n\n  //! For each c-space waypoint that is generated, the position deviation between the desired task\n  //! space path and a task space mapping of a straight-line interpolation in c-space is\n  //! approximated. The minimum position deviation places a lower bound of deviation required to add\n  //! a c-space waypoint.\n  //!\n  //! While it is somewhat unintuitive that the deviation could be *too* small, this\n  //! minimum deviation is used to control the minimum spacing between c-space configurations along\n  //! the task space path domain. A (relatively) sparse series of c-space waypoints is desirable for\n  //! trajectory generation; if the minimum deviation is arbitrarily small then the c-space points\n  //! will be (in general) too close together to generate a time-optimal trajectory. Generation of\n  //! excessive c-space waypoints will also be computationally expensive and is, in general, best\n  //! avoided.\n  //!\n  //! `min_position_deviation` must be positive and less than `max_position_deviation`.\n  //!\n  //! Default value is 0.001.\n  double min_position_deviation = 0.001;\n\n  //! For each c-space waypoint that is generated, the position deviation between the desired task\n  //! space path and a task space mapping of a straight-line interpolation in c-space is\n  //! approximated. The maximum position deviation places an upper bound of deviation allowed to add\n  //! a c-space waypoint.\n  //!\n  //! `max_position_deviation` must be positive and greater than `min_position_deviation`.\n  //!\n  //! Default value is 0.003.\n  double max_position_deviation = 0.003;\n\n  //! Initial step size in the domain value 's' used to sample poses from the task space path to be\n  //! converted to c-space waypoints.\n  //!\n  //! The 's' step size will be adaptively updated throughout the path conversion and the default\n  //! value for initialization is generally recommended.\n  //!\n  //! `initial_s_step_size` must be positive.\n  //!\n  //! Default value is 0.05.\n  double initial_s_step_size = 0.05;\n\n  //! Initial step size \"delta\" that is used to adaptively adjust the 's' step size; 's' is the\n  //! domain value 's' used to sample poses from the task space path to be converted to c-space\n  //! waypoints.\n  //!\n  //! The 's' step size \"delta\" will be adaptively updated throughout the path conversion and the\n  //! default value for initialization is generally recommended.\n  //!\n  //! `initial_s_step_size_delta` must be positive.\n  //!\n  //! Default value is 0.005.\n  double initial_s_step_size_delta = 0.005;\n\n  //! Minimum allowable interval in domain value 's' that can separate poses from the task space\n  //! path to be converted to c-space waypoints.\n  //!\n  //! The minimum 's' step size serves to limit the number of c-space configurations that can be\n  //! returned in the converted path. Specifically, the upper bound for the number of returned\n  //! c-space configurations is (\"span of the task space path domain\" / min_s_step_size) + 1.\n  //!\n  //! `min_s_step_size` must be positive.\n  //!\n  //! Default value is 1e-5.\n  double min_s_step_size = 1e-5;\n\n  //! Minimum allowable 's' step size \"delta\" used to adaptively update the 's' step size.\n  //!\n  //! The `min_s_step_size_delta` serves to limit wasted iterations when (minimal) progress is being\n  //! made towards path conversion. If `min_s_step_size_delta` is reached during the search for any\n  //! c-space waypoint, then path conversion will fail.\n  //!\n  //! The default value is generally recommended.\n  //!\n  //! `min_s_step_size_delta` must be positive.\n  //!\n  //! Default value is 1e-5.\n  double min_s_step_size_delta = 1e-5;\n\n  //! Maximum number of iterations to search for each c-space waypoint.\n  //!\n  //! If `max_iterations` is reached for any c-space waypoint, then path conversion will fail.\n  //!\n  //! `max_iterations` must be positive.\n  //!\n  //! Default value is 50.\n  int max_iterations = 50;\n\n  //! \"alpha\" is used to exponentially scale the 's' step size \"delta\" to speed convergence when the\n  //! 's' step size is being successively increased or successively decreased. When an increase is\n  //! followed by a decrease, or vice versa, \"alpha\" is used to decrease the 's' step size \"delta\"\n  //! to reduce overshoot.\n  //!\n  //! The default value is generally recommended.\n  //!\n  //! `alpha` must be greater than 1.\n  //!\n  //! Default value is 1.4.\n  double alpha = 1.4;\n};\n\n//! Convert a `composite_path_spec` into a linear c-space path.\n//!\n//! The mapping from c-space to task space is defined by `kinematics` for the given `control_frame`.\n//!\n//! If non-default configuration parameters for the path conversion process are desired, then\n//! `task_space_path_conversion_config` can (optionally) be specified.\n//!\n//! If non-default configuration parameters for the inverse kinematics (IK) solver are desired,\n//! then `ik_config` can optionally be specified.\nCUMO_EXPORT std::unique_ptr<LinearCSpacePath> ConvertCompositePathSpecToCSpace(\n    const CompositePathSpec &composite_path_spec,\n    const Kinematics &kinematics,\n    const Kinematics::FrameHandle &control_frame,\n    const TaskSpacePathConversionConfig &task_space_path_conversion_config =\n        TaskSpacePathConversionConfig(),\n    const IkConfig &ik_config = IkConfig());\n\n//! Convert a `task_space_path_spec` into a linear c-space path.\n//!\n//! Inverse kinematics will be used to convert the initial task space pose of `task_space_path_spec`\n//! to a c-space position. If a particular c-space solution is desired, this can be set in\n//! `ik_config.cspace_seeds`. If the specified c-space does *not* correspond to the initial task\n//! space pose, it will simply be used as a warm start and the IK solver will proceed to search for\n//! an appropriate c-space position.\n//!\n//! The mapping from c-space to task space is defined by `kinematics` for the given `control_frame`.\n//!\n//! If non-default configuration parameters for the path conversion process are desired, then\n//! `task_space_path_conversion_config` can (optionally) be specified.\n//!\n//! If non-default configuration parameters for the inverse kinematics (IK) solver are desired,\n//! then `ik_config` can optionally be specified.\nCUMO_EXPORT std::unique_ptr<LinearCSpacePath> ConvertTaskSpacePathSpecToCSpace(\n    const TaskSpacePathSpec &task_space_path_spec,\n    const Kinematics &kinematics,\n    const Kinematics::FrameHandle &control_frame,\n    const TaskSpacePathConversionConfig &task_space_path_conversion_config =\n        TaskSpacePathConversionConfig(),\n    const IkConfig &ik_config = IkConfig());\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/path_spec_yaml.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <filesystem>\n#include <memory>\n#include <string>\n\n#include \"cumotion/composite_path_spec.h\"\n#include \"cumotion/cspace_path_spec.h\"\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/task_space_path_spec.h\"\n\nnamespace cumotion {\n\n//! Load a `TaskSpacePathSpec` from file with absolute path `task_space_path_spec_file`.\n//!\n//! The `task_space_spec_file` is expected to correspond to a YAML file specifying a task space\n//! path. This path specification must include:\n//!   [1] an initial task space pose, and\n//!   [2] a series of path segments emanating from this initial pose.\n//!\n//! The initial pose uses the key \"initial pose\" and is expected to have the following format:\n//!\n//!   initial pose:\n//!     position: [#, #, #]\n//!     orientation: {w: #, xyz: [#, #, #]}\n//!\n//! where each \"#\" represents a floating point value. Position must be a 3d-vector and orientation\n//! is represented by a quaternion.\n//!\n//! NOTE: Standard YAML formatting is allowed, wherein:\n//!   * mapped keys can be included in any order (e.g., \"position\" and \"orientation\" can be\n//!     included in either order),\n//!   * mapped keys can be included on a single comma-separated line with curly braces, or\n//!     expanded to multiple lines. E.g., \"orientation\" can be formatted as:\n//!        orientation: {w: #, xyz: [#, #, #]}\n//!     or equivalently as:\n//!        orientation:\n//!          w: #\n//!          xyz: [#, #, #]\n//!   * sequences can be included in a single comma-separated line with square brackets, or\n//!     expanded into multiple lines with preceding dashes. E.g., \"position\" can be formatted as:\n//!        position: [#, #, #]\n//!     or equivalently as:\n//!        position:\n//!          - #\n//!          - #\n//!          - #\n//!\n//! The path segments are specified as a sequence under the key \"path specs\". Each path\n//! specification must specify both a \"position mode\" and a \"orientation mode\".\n//!\n//! The \"position mode\" must be:\n//!   \"linear\",\n//!   \"constant\",\n//!   \"three_point_arc\", or\n//!   \"tangent_arc\".\n//! The \"orientation mode\" must be:\n//!   \"slerp\",\n//!   \"constant\", or\n//!   \"tangent\".\n//!\n//! WARNING: Not all combinations of \"position mode\" and \"orientation mode\" are compatible.\n//! Specifically, a \"linear\" position mode is *NOT* compatible with a \"tangent\" orientation mode.\n//! A \"constant\" position mode is *ONLY* compatible with a \"slerp\" orientation mode. The two arc\n//! position modes (\"three_point_arc\" and \"tangent_arc\") are compatible with all orientation modes.\n//!\n//! Each (valid) combination of position and orientation mode requires specific input data:\n//!\n//! [1] Linear path:\n//!     - \"linear\" position mode with \"slerp\" orientation mode.\n//!     - Requires \"target pose\" and optional \"blend radius\".\n//!     - See `TaskSpacePathSpec::addLinearPath()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: linear\n//!             orientation mode: slerp\n//!             target pose:\n//!               position: [#, #, #]\n//!               orientation: {w: #, xyz: [#, #, #]}\n//!             blend radius: #\n//! [2] Translation path:\n//!     - \"linear\" position mode with \"constant\" orientation mode.\n//!     - Requires \"target position\" and optional \"blend radius\".\n//!     - See `TaskSpacePathSpec::addTranslation()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: linear\n//!             orientation mode: constant\n//!             target position: [#, #, #]\n//!             blend radius: #\n//! [3] Rotation path:\n//!     - \"constant\" position mode with \"slerp\" orientation mode.\n//!     - Requires \"target orientation\" .\n//!     - See `TaskSpacePathSpec::addRotation()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: constant\n//!             orientation mode: slerp\n//!             target orientation: {w: #, xyz: [#, #, #]}\n//! [4] Three-point arc with constant orientation:\n//!     - \"three_point_arc\" position mode with \"constant\" orientation mode.\n//!     - Requires \"target position\" and \"intermediate position\".\n//!     - See `TaskSpacePathSpec::addThreePointArc()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: three_point_arc\n//!             orientation mode: constant\n//!             target position: [#, #, #]\n//!             intermediate position: [#, #, #]\n//! [5] Three-point arc with tangent orientation:\n//!     - \"three_point_arc\" position mode with \"tangent\" orientation mode.\n//!     - Requires \"target position\" and \"intermediate position\".\n//!     - See `TaskSpacePathSpec::addThreePointArc()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: three_point_arc\n//!             orientation mode: tangent\n//!             target position: [#, #, #]\n//!             intermediate position: [#, #, #]\n//! [6] Three-point arc with orientation target:\n//!     - \"three_point_arc\" position mode with \"slerp\" orientation mode.\n//!     - Requires \"target pose\" and \"intermediate position\".\n//!     - See `TaskSpacePathSpec::addThreePointArcWithOrientationTarget()` in\n//!       `task_space_path_spec.h` for more documentation.\n//!     - Format:\n//!           - position mode: three_point_arc\n//!             orientation mode: slerp\n//!             target pose:\n//!               position: [#, #, #]\n//!               orientation: {w: #, xyz: [#, #, #]}\n//!             intermediate position: [#, #, #]\n//! [7] Tangent arc with constant orientation:\n//!     - \"tangent_arc\" position mode with \"constant\" orientation mode.\n//!     - Requires \"target position\".\n//!     - See `TaskSpacePathSpec::addTangentArc()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: tangent_arc\n//!             orientation mode: constant\n//!             target position: [#, #, #]\n//! [8] Tangent arc with tangent orientation:\n//!     - \"tangent_arc\" position mode with \"tangent\" orientation mode.\n//!     - Requires \"target position\".\n//!     - See `TaskSpacePathSpec::addTangentArc()` in `task_space_path_spec.h` for more\n//!       documentation.\n//!     - Format:\n//!           - position mode: tangent_arc\n//!             orientation mode: tangent\n//!             target position: [#, #, #]\n//! [9] Tangent arc with orientation target:\n//!     - \"tangent_arc\" position mode with \"slerp\" orientation mode.\n//!     - Requires \"target pose\".\n//!     - See `TaskSpacePathSpec::addTangentArcWithOrientationTarget()` in\n//!       `task_space_path_spec.h` for more documentation.\n//!     - Format:\n//!           - position mode: tangent_arc\n//!             orientation mode: slerp\n//!             target pose:\n//!               position: [#, #, #]\n//!               orientation: {w: #, xyz: [#, #, #]}\n//!\n//! If \"initial pose\" or \"path specs\" are unable to be parsed, `nullptr` will be returned. If any\n//! \"path specs\" fail to be parsed, they will be discarded and a warning will be logged, but any\n//! other valid \"path specs\" will continue to be added to the returned `TaskSpacePathSpec`.\nCUMO_EXPORT std::unique_ptr<TaskSpacePathSpec> LoadTaskSpacePathSpecFromFile(\n    const std::filesystem::path &task_space_path_spec_file);\n\n//! Load a `TaskSpacePathSpec` from the contents of a YAML file (`task_space_path_spec_yaml`).\n//!\n//! See `LoadTaskSpacePathSpecFromFile()` documentation for detailed description of required YAML\n//! format.\nCUMO_EXPORT std::unique_ptr<TaskSpacePathSpec> LoadTaskSpacePathSpecFromMemory(\n    const std::string &task_space_path_spec_yaml);\n\n//! Export `task_space_path_spec` as a string.\n//!\n//! The returned string will be in YAML format as documented in `LoadTaskSpacePathSpecFromFile()`.\nCUMO_EXPORT std::string ExportTaskSpacePathSpecToMemory(\n    const TaskSpacePathSpec &task_space_path_spec);\n\n//! Load a `CSpacePathSpec` from file with absolute path `cspace_path_spec_file`.\n//!\n//! The `cspace_spec_file` is expected to correspond to a YAML file specifying a c-space path.\n//! This path specification must include:\n//!   [1] an initial c-space position, and\n//!   [2] a series of c-space waypoints following this initial c-space position.\n//!\n//! The initial c-space position uses the key \"initial c-space position\" and is expected to have the\n//! following format:\n//!\n//!   initial c-space position: [#, #, ... , #]\n//!\n//! where each \"#\" represents a floating point value. The number of elements in this vector sets the\n//! expected number of c-space coordinates for the `CSpacePathSpec`.\n//!\n//! The c-space waypoints are specified as a sequence under the key \"waypoints\", with the following\n//! format:\n//!\n//!   waypoints:\n//!     - [#, #, ... , #]\n//!     - [#, #, ... , #]\n//!     - [#, #, ... , #]\n//!\n//! where the number of waypoints is variable. Each waypoint must have the same number of\n//! c-space coordinates as the \"initial c-space position\", or it will be discarded and a warning\n//! will be logged.\n//!\n//! If \"initial c-space position\" or \"waypoints\" are unable to be parsed, `nullptr` will be\n//! returned. If any \"waypoints\" fail to be parsed, they will be discarded and a warning will be\n//! logged, but any other valid \"waypoints\" will continue to be added to the returned\n//! `CSpacePathSpec`.\nCUMO_EXPORT std::unique_ptr<CSpacePathSpec> LoadCSpacePathSpecFromFile(\n    const std::filesystem::path &cspace_path_spec_file);\n\n//! Load a `CSpacePathSpec` from the contents of a YAML file (`cspace_path_spec_yaml`).\n//!\n//! See `LoadCSpacePathSpecFromFile()` documentation for detailed description of required YAML\n//! format.\nCUMO_EXPORT std::unique_ptr<CSpacePathSpec> LoadCSpacePathSpecFromMemory(\n    const std::string &cspace_path_spec_yaml);\n\n//! Export `cspace_path_spec` as a string.\n//!\n//! The returned string will be in YAML format as documented in `LoadCSpacePathSpecFromFile()`.\nCUMO_EXPORT std::string ExportCSpacePathSpecToMemory(const CSpacePathSpec &cspace_path_spec);\n\n//! Load a `CompositePathSpec` from file with absolute path `composite_path_spec_file`.\n//!\n//! The `composite_path_spec_file` is expected to correspond to a YAML file specifying a path\n//! composed of task space and/or c-space segments. This path specification must include:\n//!   [1] an initial c-space position, and\n//!   [2] a series of task-space and/or c-space path specifications following this initial\n//!       c-space position.\n//!\n//! The initial c-space position uses the key \"initial c-space position\" and is expected to have the\n//! following format:\n//!\n//!   initial c-space position: [#, #, ... , #]\n//!\n//! where each \"#\" represents a floating point value. The number of elements in this vector sets the\n//! expected number of c-space coordinates for the `CSpacePathSpec`.\n//!\n//! The series of path specifications uses the key \"path specs\" and each spec is required to be\n//! labelled as either \"c-space\" or \"task-space\". In addition to including the full\n//! specification for the task space or c-space path (see `LoadTaskSpacePathSpecFromFile()` and\n//! `LoadCSpacePathSpecFromFile()` for details), each path specification must specify a\n//! \"transition mode\" which must be \"skip\", \"free\", or \"linear_task_space\". These modes correspond\n//! to the `CompositePathSpec::TransitionMode` defined in `composite_path_spec.h`.\n//!\n//! An example specification may look like:\n//!\n//! initial c-space position: [#, #, ... , #]\n//! path specs:\n//!   - c-space:\n//!       transition mode: skip\n//!       initial c-space position: [#, #, ... , #]\n//!       waypoints:\n//!         - [#, #, ... , #]\n//!         - [#, #, ... , #]\n//!         - ...\n//!         - [#, #, ... , #]\n//!   - task-space:\n//!       transition mode: linear_task_space\n//!       initial pose:\n//!         position: [#, #, #]\n//!         orientation: { w: #, xyz: [#, #, #] }\n//!       path specs:\n//!         - position mode: linear\n//!           orientation mode: constant\n//!           target position: [#, #, #]\n//!           blend radius: 0.0\n//!         - ...\n//!   - ...\n//!\n//! where any combination of task space and c-space path specifications may be included. The order\n//! of the path specifications in the YAML file dictates the order in which they are added to the\n//! `CompositePathSpec`.\n//!\n//! If \"initial c-space position\" or \"path specs\" are unable to be parsed, `nullptr` will be\n//! returned. If any \"path specs\" fail to be parsed, they will be discarded and a warning will be\n//! logged, but any other valid \"path specs\" will continue to be added to the returned\n//! `CompositePathSpec`.\nCUMO_EXPORT std::unique_ptr<CompositePathSpec> LoadCompositePathSpecFromFile(\n    const std::filesystem::path &composite_path_spec_file);\n\n//! Load a `CompositePathSpec` from the contents of a YAML file (`composite_path_spec_yaml`).\n//!\n//! See `LoadCompositePathSpecFromFile()` documentation for detailed description of required YAML\n//! format.\nCUMO_EXPORT std::unique_ptr<CompositePathSpec> LoadCompositePathSpecFromMemory(\n    const std::string &composite_path_spec_yaml);\n\n//! Export `composite_path_spec` as a string.\n//!\n//! The returned string will be in YAML format as documented in `LoadCompositePathSpecFromFile()`.\nCUMO_EXPORT std::string ExportCompositePathSpecToMemory(\n    const CompositePathSpec &composite_path_spec);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/pose3.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2020-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for representing a pose in 3d.\n\n#pragma once\n\n#include <vector>\n\n#include \"Eigen/Core\"\n#include \"Eigen/Geometry\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/rotation3.h\"\n\nnamespace cumotion {\n\n//! Class representing a 3d pose.\nstruct CUMO_EXPORT Pose3 {\n public:\n  //! Default constructor (set `rotation` identity and `translation` to zero).\n  Pose3();\n\n  //! Construct pose from a `rotation` and `translation`.\n  Pose3(const Rotation3 &rotation, const Eigen::Vector3d &translation);\n\n  //! Construct pose from a 4 x 4 homogeneous transform matrix.\n  //!\n  //! @note The bottom row of the input matrix is assumed to be `[0, 0, 0, 1]`, but is not\n  //! explicitly checked.\n  explicit Pose3(const Eigen::Matrix<double, 4, 4> &matrix);\n\n  //! Create a pure rotational pose.\n  static Pose3 FromRotation(const Rotation3 &rotation);\n\n  //! Create a pure translational pose.\n  static Pose3 FromTranslation(const Eigen::Vector3d &translation);\n\n  //! Create identity pose.\n  static Pose3 Identity();\n\n  //! Return matrix representation of the pose.\n  [[nodiscard]] Eigen::Matrix4d matrix() const;\n\n  //! Returns the inverse pose.\n  [[nodiscard]] Pose3 inverse() const;\n\n  //! Transforms a collection of 3d `vectors` by this pose.\n  [[nodiscard]]\n  std::vector<Eigen::Vector3d> transformVectors(const std::vector<Eigen::Vector3d> &vectors) const;\n\n  //! Transforms a collection of 3d `vectors` by this pose.\n  //!\n  //! Each column of `vectors` represents a 3d vector to be transformed.\n  [[nodiscard]] Eigen::Matrix<double, 3, Eigen::Dynamic> transformVectors(\n      const Eigen::Matrix<double, 3, Eigen::Dynamic> &vectors) const;\n\n  //! Compose poses via * operator.\n  friend CUMO_EXPORT Pose3 operator*(const Pose3 &lhs, const Pose3 &rhs);\n\n  //! Transforms a vector by the given pose.\n  friend CUMO_EXPORT Eigen::Vector3d operator*(const Pose3 &pose, const Eigen::Vector3d &vector);\n\n  //! Rotation component of pose.\n  Rotation3 rotation;\n\n  //! Translation component of pose.\n  Eigen::Vector3d translation;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/rmpflow.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2019-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface to cuMotion's RMPflow implementation\n\n#pragma once\n\n#include <filesystem>\n#include <memory>\n#include <optional>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/rotation3.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Interface class for loading and manipulating RMPflow parameters.\n//! WARNING: This interface may change in a future release.\nclass CUMO_EXPORT RmpFlowConfig {\n public:\n  virtual ~RmpFlowConfig() = default;\n\n  //! Get the value of a parameter, given a \"param_name\" string of the form\n  //! \"<rmp_name>/<parameter_name>\"\n  [[nodiscard]] virtual double getParam(const std::string &param_name) const = 0;\n\n  //! Set the value of the parameter.\n  virtual void setParam(const std::string &param_name, double value) = 0;\n\n  //! Get the names and values of all parameters.  The two vectors will be overwritten if not empty.\n  virtual void getAllParams(std::vector<std::string> &names, std::vector<double> &values) const = 0;\n\n  //! Set all parameters at once.  The vectors \"names\" and \"values\" must have the same size.\n  //! The parameter corresponding to names[i] will be set to the value given by values[i].\n  virtual void setAllParams(const std::vector<std::string> &names,\n                            const std::vector<double> &values) = 0;\n\n  //! Set the world view that will be used for obstacle avoidance. All enabled obstacles in\n  //! `world_view` will be avoided by the RMPflow policy.\n  virtual void setWorldView(const WorldViewHandle &world_view) = 0;\n};\n\n//! Load a set of RMPflow parameters from file, and combine with a robot description to create a\n//! configuration object for consumption by CreateRmpFlow().  The \"end_effector_frame\" should\n//! correspond to a link name as specified in the original URDF or added as a frame in the XRDF used\n//! to create the robot description. All enabled obstacles in `world_view` will be avoided by the\n//! RMPflow policy.\n//!\n//! DEPRECATED: This function is deprecated and will be removed in a future version.\n//!             Use the variant of `CreateRmpFlowConfigFromFile()` that does *not* include\n//!             `end_effector_frame` instead. The end effector frame can then be added using\n//!             `RmpFlow::addTargetFrame()`.\nCUMO_DEPRECATED_EXPORT std::unique_ptr<RmpFlowConfig> CreateRmpFlowConfigFromFile(\n    const std::filesystem::path &rmpflow_config_file,\n    const RobotDescription &robot_description,\n    const std::string &end_effector_frame,\n    const WorldViewHandle &world_view);\n\n//! Load a set of RMPflow parameters from file, and combine with a robot description to create a\n//! configuration object for consumption by CreateRmpFlow(). All enabled obstacles in `world_view`\n//! will be avoided by the RMPflow policy.\nCUMO_EXPORT std::unique_ptr<RmpFlowConfig> CreateRmpFlowConfigFromFile(\n    const std::filesystem::path &rmpflow_config_file,\n    const RobotDescription &robot_description,\n    const WorldViewHandle &world_view);\n\n//! Load a set of RMPflow parameters from string, and combine with a robot description to create a\n//! configuration object for consumption by CreateRmpFlow().  The \"end_effector_frame\" should\n//! correspond to a link name as specified in the original URDF or added as a frame in the XRDF used\n//! to create the robot description. All enabled obstacles in `world_view` will be avoided by the\n//! RMPflow policy.\n//!\n//! DEPRECATED: This function is deprecated and will be removed in a future version.\n//!             Use the variant of `CreateRmpFlowConfigFromMemory()` that does *not* include\n//!             `end_effector_frame` instead. The end effector frame can then be added using\n//!             `RmpFlow::addTargetFrame()`.\nCUMO_DEPRECATED_EXPORT std::unique_ptr<RmpFlowConfig> CreateRmpFlowConfigFromMemory(\n    const std::string &rmpflow_config,\n    const RobotDescription &robot_description,\n    const std::string &end_effector_frame,\n    const WorldViewHandle &world_view);\n\n//! Load a set of RMPflow parameters from string, and combine with a robot description to create a\n//! configuration object for consumption by CreateRmpFlow(). All enabled obstacles in `world_view`\n//! will be avoided by the RMPflow policy.\nCUMO_EXPORT std::unique_ptr<RmpFlowConfig> CreateRmpFlowConfigFromMemory(\n    const std::string &rmpflow_config,\n    const RobotDescription &robot_description,\n    const WorldViewHandle &world_view);\n\n//! Interface class for building and evaluating a motion policy in the RMPflow framework\nclass CUMO_EXPORT RmpFlow {\n public:\n  virtual ~RmpFlow() = default;\n\n  //! Parameters to configure task-space RMPs.\n  //!\n  //! For detailed description of all parameters, see RMPflow documentation\n  //! (https://nvidia-isaac.github.io/cumotion/concepts/rmpflow.html) and associated tuning guide\n  //! (https://nvidia-isaac.github.io/cumotion/concepts/rmpflow_tuning_guide.html). This\n  //! guide provides a mathematical framework for the target RMP and defines variables used\n  //! to document each of the parameters in `TargetRmpConfig`.\n  struct CUMO_EXPORT TargetRmpConfig {\n    //! Create a `TargetRmpConfig` using default parameters.\n    TargetRmpConfig();\n\n    //! Create a `TargetRmpConfig` using parameters from `rmpflow_config`.\n    explicit TargetRmpConfig(const RmpFlowConfig &rmpflow_config);\n\n    //! Parameters to configure a task-space position RMP.\n    struct CUMO_EXPORT PositionConfig {\n      //! Position gain.\n      //!\n      //! Units: m/s^2\n      //! Must be positive.\n      double accel_p_gain{80.0};\n\n      //! Damping gain.\n      //!\n      //! Units: s^-1\n      //! Must be positive.\n      double accel_d_gain{120.0};\n\n      //! Length scale controlling transition between constant acceleration region far from target\n      //! and linear region near target.\n      //!\n      //! Units: m\n      //! Must be positive.\n      double accel_norm_eps{0.075};\n\n      //! Length scale of the Gaussian controlling blending between `S` and `I`.\n      //!\n      //! Units: m\n      //! Must be positive.\n      double metric_alpha_length_scale{0.05};\n\n      //! Controls the minimum contribution of the isotropic `M_near` term to the metric (inertia\n      //! matrix).\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double min_metric_alpha{0.01};\n\n      //! Metric scalar for the isotropic `M_near` contribution to the metric (inertia matrix).\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double max_metric_scalar{10000.0};\n\n      //! Metric scalar for the directional `M_far` contribution to the metric (inertia matrix).\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double min_metric_scalar{2500.0};\n\n      //! Scale factor controlling the strength of boosting near the target.\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double proximity_metric_boost_scalar{20.0};\n\n      //! Length scale of the Gaussian controlling boosting near the target.\n      //!\n      //! Units: m\n      //! Must be positive.\n      double proximity_metric_boost_length_scale{0.02};\n    };\n\n    //! Parameters to configure a task-space orientation RMP.\n    struct CUMO_EXPORT OrientationConfig {\n      //! Position gain.\n      //!\n      //! Units: s^-2\n      //! Must be positive.\n      double accel_p_gain{200.0};\n\n      //! Damping gain.\n      //!\n      //! Units: s^-1\n      //! Must be positive.\n      double accel_d_gain{40.0};\n\n      //! Priority weight relative to other RMPs.\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double metric_scalar{10.0};\n\n      //! Scale factor controlling the strength of boosting near the position target.\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double proximity_metric_boost_scalar{3000.0};\n\n      //! Length scale of the Gaussian controlling boosting near the position target.\n      //!\n      //! Units: m\n      //! Must be positive.\n      double proximity_metric_boost_length_scale{0.05};\n    };\n\n    //! Parameters to configure damping for task-space RMPs.\n    struct CUMO_EXPORT DampingConfig {\n      //! Nonlinear damping gain.\n      //!\n      //! Units: m^-1\n      //! Must be positive.\n      double accel_d_gain{30.0};\n\n      //! Priority weight relative to other RMPs.\n      //!\n      //! Units: (m/s)^-1\n      //! Must be positive.\n      double metric_scalar{50.0};\n\n      //! Additional inertia.\n      //!\n      //! Units: dimensionless\n      //! Must be positive.\n      double inertia{100.0};\n    };\n\n    //! Parameters to configure a task-space position RMP.\n    PositionConfig position_config;\n\n    //! Parameters to configure a task-space orientation RMP.\n    OrientationConfig orientation_config;\n\n    //! Parameters to configure damping for task-space RMPs.\n    DampingConfig damping_config;\n  };\n\n  //! Return the number of task-space target frames.\n  //!\n  //! This count includes both \"active\" target frames (i.e., frames with position and/or orientation\n  //! targets set) as well as \"inactive\" target frames (i.e., frames for which no position or\n  //! orientation targets have been set). This count does *not* include any target frames that have\n  //! been removed.\n  [[nodiscard]] virtual int numTargetFrames() const = 0;\n\n  //! Return the names of all frames with task-space targets.\n  //!\n  //! These names include both \"active\" target frames (i.e., frames with position and/or orientation\n  //! targets set) as well as \"inactive\" target frames (i.e., frames for which no position or\n  //! orientation targets have been set). These names do *not* include any target frames that have\n  //! been removed.\n  [[nodiscard]] virtual std::vector<std::string> targetFrameNames() const = 0;\n\n  //! Enable a task-space target for the frame corresponding to `frame_name`.\n  //!\n  //! The target may be configured using the optional `config`. If `config` is not provided, then\n  //! configuration parameters are taken from the `RmpFlowConfig` originally used to create the\n  //! `RmpFlow` object.\n  //!\n  //! NOTE: The task-space target will not be active until a position and/or orientation target\n  //!       is set.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` is not a valid frame in the `RobotDescription` used to construct `RmpFlow`,\n  //!   2. `frame_name` is already in use as a target frame (i.e., this frame has been added and\n  //!      not removed). This includes the `end_effector_frame` (optionally) used to construct the\n  //!      `RmpflowConfig` used create this `RmpFlow` instance, *OR*\n  //!   3. Any parameter in `config` is invalid.\n  virtual void addTargetFrame(const std::string &frame_name,\n                              std::optional<TargetRmpConfig> config = std::nullopt) = 0;\n\n  //! Remove the task-space target for the frame corresponding to `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void removeTargetFrame(const std::string &frame_name) = 0;\n\n  //! Set both a position and orientation target for `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void setPoseTarget(const std::string &frame_name, const Pose3 &pose) = 0;\n\n  //! Clear both the position and orientation target for `frame_name`.\n  //!\n  //! This function will be a no-op if neither a position nor an orientation target is active for\n  //! `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void clearPoseTarget(const std::string &frame_name) = 0;\n\n  //! Set a position target for `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void setPositionTarget(const std::string &frame_name,\n                                 const Eigen::Vector3d &position) = 0;\n\n  //! Clear the position target for `frame_name`.\n  //!\n  //! This function will be a no-op if a position target is not active for `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void clearPositionTarget(const std::string &frame_name) = 0;\n\n  //! Set an orientation target for `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void setOrientationTarget(const std::string &frame_name,\n                                    const Rotation3 &orientation) = 0;\n\n  //! Clear the orientation target for `frame_name`.\n  //!\n  //! This function will be a no-op if an orientation target is not active for `frame_name`.\n  //!\n  //! A fatal error will be logged if:\n  //!   1. `frame_name` does not correspond to a task-space target frame.\n  virtual void clearOrientationTarget(const std::string &frame_name) = 0;\n\n  //! Set an end-effector position attractor.\n  //!\n  //! The origin of the end effector frame will be driven towards the specified position.\n  //!\n  //! The \"end-effector\" is defined as the first task-space target (i.e., the first frame name\n  //! returned from `targetFrameNames()`). A fatal error will be logged if there are no target\n  //! frames (i.e., `numTargetFrames()` == 0).\n  //!\n  //! DEPRECATED: This function is deprecated and will be removed in a future release.\n  //!             Use `setPositionTarget()` instead.\n  CUMO_DEPRECATED virtual void setEndEffectorPositionAttractor(const Eigen::Vector3d &position) = 0;\n\n  //! Clear end-effector position attractor.\n  //!\n  //! The RMP driving the origin of the end effector frame towards a particular position will be\n  //! deactivated.\n  //!\n  //! The \"end-effector\" is defined as the first task-space target (i.e., the first frame name\n  //! returned from `targetFrameNames()`). A fatal error will be logged if there are no target\n  //! frames (i.e., `numTargetFrames()` == 0).\n  //!\n  //! DEPRECATED: This function is deprecated and will be removed in a future release.\n  //!             Use `clearPositionTarget()` instead.\n  CUMO_DEPRECATED virtual void clearEndEffectorPositionAttractor() = 0;\n\n  //! Set an end-effector orientation attractor.\n  //!\n  //! The orientation of the end effector frame will be driven towards the specified orientation.\n  //!\n  //! The \"end-effector\" is defined as the first task-space target (i.e., the first frame name\n  //! returned from `targetFrameNames()`). A fatal error will be logged if there are no target\n  //! frames (i.e., `numTargetFrames()` == 0).\n  //!\n  //! DEPRECATED: This function is deprecated and will be removed in a future release.\n  //!             Use `setOrientationTarget()` instead.\n  CUMO_DEPRECATED virtual void setEndEffectorOrientationAttractor(const Rotation3 &orientation) = 0;\n\n  //! Clear end-effector orientation attractor.\n  //!\n  //! The RMPs driving the orientation of the end effector frame towards a particular orientation\n  //! will be deactivated.\n  //!\n  //! The \"end-effector\" is defined as the first task-space target (i.e., the first frame name\n  //! returned from `targetFrameNames()`). A fatal error will be logged if there are no target\n  //! frames (i.e., `numTargetFrames()` == 0).\n  //!\n  //! DEPRECATED: This function is deprecated and will be removed in a future release.\n  //!             Use `clearOrientationTarget()` instead.\n  CUMO_DEPRECATED virtual void clearEndEffectorOrientationAttractor() = 0;\n\n  //! Set an attractor in generalized coordinates (configuration space).\n  //!\n  //! The c-space coordinates will be biased towards the specified configuration.\n  //!\n  //! NOTE:  Unlike the end effector attractors, there is always an active c-space attractor (either\n  //!        set using `setCSpaceAttractor()` or using the default value loaded from the robot\n  //!        description).\n  virtual void setCSpaceAttractor(const Eigen::VectorXd &cspace_position) = 0;\n\n  //! Compute configuration-space acceleration from motion policy, given input state. This takes\n  //! into account the current c-space and/or task-space targets, as well as any\n  //! currently-enabled obstacles.\n  virtual void evalAccel(const Eigen::VectorXd &cspace_position,\n                         const Eigen::VectorXd &cspace_velocity,\n                         Eigen::Ref<Eigen::VectorXd> cspace_accel) const = 0;\n\n  //! Compute configuration-space force and metric from motion policy, given input state. This takes\n  //! into account the current c-space and/or task-space targets, as well as any\n  //! currently-enabled obstacles.\n  virtual void evalForceAndMetric(const Eigen::VectorXd &cspace_position,\n                                  const Eigen::VectorXd &cspace_velocity,\n                                  Eigen::Ref<Eigen::VectorXd> cspace_force,\n                                  Eigen::Ref<Eigen::MatrixXd> cspace_metric) const = 0;\n};\n\n//! Create an instance of the RmpFlow interface from an RMPflow configuration.\nCUMO_EXPORT std::unique_ptr<RmpFlow> CreateRmpFlow(const RmpFlowConfig &config);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/robot_description.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2020-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for loading and accessing a robot description\n\n#pragma once\n\n#include <filesystem>\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Forward declaration of Kinematics from `cumotion/kinematics.h`.\nclass Kinematics;\n\n//! Class encapsulating all semantic and kinematic properties of a robot\nclass CUMO_EXPORT RobotDescription {\n public:\n  virtual ~RobotDescription() = default;\n\n  //! Return the number of actuated joints for the robot.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Return the name of a given joint of the robot.\n  [[nodiscard]] virtual std::string cSpaceCoordName(int coord_index) const = 0;\n\n  //! Return default joint positions for the robot.\n  //!\n  //! Returned vector will have length equal to `numCSpaceCoords()`.\n  [[nodiscard]] virtual Eigen::VectorXd defaultCSpaceConfiguration() const = 0;\n\n  //! Return a copy of robot kinematics.\n  [[nodiscard]] virtual std::unique_ptr<Kinematics> kinematics() const = 0;\n\n  //! Return the names of all tool frames (if any) specified in the robot description.\n  [[nodiscard]] virtual std::vector<std::string> toolFrameNames() const = 0;\n};\n\n//! Load a robot description from an XRDF (`robot_xrdf`) and a URDF (`robot_urdf`).\n//!\n//! It is recommended that `robot_xrdf` and `robot_urdf` be specified as absolute filepaths.\n//! Relative paths will be resolved using the same rules as the `std::filesystem::path` type.\n//!\n//! The \"Extended Robot Description Format\" (XRDF) is documented at:\n//! https://nvidia-isaac-ros.github.io/v/release-3.2/concepts/manipulation/xrdf.html\n//!\n//! A fatal error will be logged if:\n//! - `robot_xrdf` is not a valid file path,\n//! - `robot_urdf` is not a valid file path,\n//! - `robot_xrdf` cannot be successfully parsed, *OR*\n//! - `robot_urdf` cannot be successfully parsed.\n[[nodiscard]] CUMO_EXPORT std::unique_ptr<RobotDescription> LoadRobotFromFile(\n    const std::filesystem::path &robot_xrdf,\n    const std::filesystem::path &robot_urdf);\n\n//! Load a robot description from the contents of an XRDF (\"robot_xrdf\") and the contents\n//! of a URDF (\"robot_urdf\").\n//!\n//! The \"Extended Robot Description Format\" (XRDF) is documented at:\n//! https://nvidia-isaac-ros.github.io/v/release-3.2/concepts/manipulation/xrdf.html\n[[nodiscard]] CUMO_EXPORT std::unique_ptr<RobotDescription> LoadRobotFromMemory(\n    const std::string &robot_xrdf,\n    const std::string &robot_urdf);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/robot_segmenter.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2024-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for segmenting out the contribution of a robot from a depth image\n//!\n//! @note This interface is experimental and may evolve in a future release.  It also lacks\n//!       a corresponding Python API.\n\n#pragma once\n\n#include <limits>\n#include <memory>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/pose3.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/vision.h\"\n\nnamespace cumotion {\n\n//! Interface class for segmenting (masking) out the contribution of a robot from a depth image.\nclass CUMO_EXPORT RobotSegmenter {\n public:\n  enum class RobotGeometryKind {\n    SELF_COLLISION_SPHERES,   //!< Use self-collision spheres for robot segmentation.\n    WORLD_COLLISION_SPHERES,  //!< Use world-collision spheres for robot segmentation.\n  };\n\n  virtual ~RobotSegmenter() {}\n\n  //! For the given `camera_pose_in_robot_frame`, `cspace_position` of the robot, and\n  //! `input_depth_image`, compute a filtered `output_depth_image` and/or `output_depth_mask`.\n  //!\n  //! Both `output_depth_image` and `output_mask` are optional, and passing in `nullptr` for either\n  //! will skip the corresponding computation.  A warning will be logged if `nullptr` is passed in\n  //! for both.\n  //!\n  //! If not null, `output_depth_image` will be populated with the same depth values as\n  //! `input_depth_image` for all pixels where the corresponding point (in three dimensions) is not\n  //! contained within the robot geometry, taking into account the `additional_buffer_distance`\n  //! specified during construction of the `RobotSegmenter`.  Depth values for pixels corresponding\n  //! to points contained in the robot geometry will be set to zero.\n  //!\n  //! If not null, `output_mask` will contain a zero for all pixels where the corresponding point\n  //! (in three dimensions) is contained within the robot geometry and a value larger than zero for\n  //! all other pixels.  The mask is pixel-aligned with `input_depth_image`.  The particular\n  //! nonzero value used depends on the scalar type of the `DepthImage`:\n  //!\n  //! * For a scalar type of `float`, the nonzero value is 1.0.  Note that `meters_per_unit` for\n  //!   the `DepthImage` has no effect; it's the \"raw\" value that's set to 1.0.\n  //! * For a scalar type of `uint16_t`, the nonzero value is 65,535 (the maximum representable\n  //!   integer).  Again, `meters_per_unit` for the `DepthImage` has no effect.\n  //!\n  //! `min_depth` and `max_depth` are given in meters and influence the interpretation of depth\n  //! values read from the `input_depth_image`.  A depth value outside the range\n  //! (`min_depth`, `max_depth`) is assumed to correspond to a point that is not contained within\n  //! the robot geometry, and such a depth value will be written to the `output_depth_image`\n  //! unchanged.\n  //!\n  //! The current implementation requires that all depth images have `HOST` or `MANAGED` residency.\n  //!\n  //! A fatal error will be logged if:\n  //!\n  //! 1. `output_depth_image` is not null, and its width or height differs from the width or height\n  //!    (respectively) of `input_depth_image`.\n  //! 2. `output_mask` is not null, and its width or height differs from the width or height\n  //!    (respectively) of `input_depth_image`.\n  //! 3. `output_depth_image` or `output_mask` is provided as a `DepthImage` with a `const` scalar\n  //!    type.\n  //! 4. `input_depth_image`, `output_depth_image`, or `output_mask` have `DEVICE` residency.\n  virtual void segmentDepthImage(const Pose3 &camera_pose_in_robot_frame,\n                                 const Eigen::VectorXd &cspace_position,\n                                 const DepthImageBase &input_depth_image,\n                                 DepthImageBase *output_depth_image,\n                                 DepthImageBase *output_mask,\n                                 double min_depth = 0.01,\n                                 double max_depth = std::numeric_limits<double>::max()) = 0;\n};\n\n//! Create a `RobotSegmenter` from a `robot_description` and `camera_intrinsics`.\n//!\n//! `camera_intrinsics` must be in the four-parameter \"pinhole\" format with zero skew.\n//!\n//! If provided, `additional_buffer_distance` will inflate the effective size of the robot geometry\n//! (e.g., collision spheres) such that a point within that distance is considered to be contained\n//! within the geometry.  This `additional_buffer_distance` augments rather than replaces any\n//! per-link buffer distance specified in the XRDF for the robot.\n//!\n//! `additional_buffer_distance` may be negative, shrinking the effective size of the robot\n//! geometry.  A given part of the geometry (e.g., a collision sphere) will be ignored altogether\n//! once `additional_buffer_distance` is sufficiently negative (e.g., negative with a magnitude\n//! that exceeds the radius, for the case of a sphere).\n//!\n//! The currently-supported values for `robot_geometry_kind` are `SELF_COLLISION_SPHERES` and\n//! `WORLD_COLLISION_SPHERES`, mapping to the corresponding collision geometry types in XRDF.\n//!\n//! A fatal error will be logged if the `camera_intrinsics` are not in the four-parameter \"pinhole\"\n//! format (with zero skew).\nCUMO_EXPORT std::unique_ptr<RobotSegmenter>\nCreateRobotSegmenter(const RobotDescription &robot_description,\n                     const CameraIntrinsics &camera_intrinsics,\n                     double additional_buffer_distance = 0.0,\n                     RobotSegmenter::RobotGeometryKind robot_geometry_kind =\n                         RobotSegmenter::RobotGeometryKind::WORLD_COLLISION_SPHERES);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/robot_world_inspector.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2023-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for querying spatial relationships between a robot and a world.\n\n#pragma once\n\n#include <memory>\n#include <optional>\n#include <string>\n#include <utility>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Interface for querying spatial relationships between a robot and a world.\nclass CUMO_EXPORT RobotWorldInspector {\n public:\n  virtual ~RobotWorldInspector() = default;\n\n  //! Clear the existing world view from `RobotWorldInspector`.\n  //!\n  //! All `ObstacleHandle`s will be invalid from the perspective of `RobotWorldInspector`.\n  //!\n  //! `inCollisionWithObstacle()` will always return `false`.\n  virtual void clearWorldView() = 0;\n\n  //! Set the internal world view used by `RobotWorldInspector`.\n  virtual void setWorldView(const WorldViewHandle &world_view) = 0;\n\n  //! Return the number of world-collision spheres in the robot representation.\n  [[nodiscard]] virtual int numWorldCollisionSpheres() const = 0;\n\n  //! Return the number of self-collision spheres in the robot representation.\n  [[nodiscard]] virtual int numSelfCollisionSpheres() const = 0;\n\n  //! Return the name of the frame associated with the world-collision sphere corresponding to\n  //! `world_collision_sphere_index`.\n  //!\n  //! A fatal error is logged if:\n  //!  1. `world_collision_sphere_index` is greater than or equal to the number of world-collision\n  //!     spheres attached to the robot (i.e., `numWorldCollisionSpheres()`), *OR*\n  //!  2. `world_collision_sphere_index` is negative.\n  [[nodiscard]]\n  virtual std::string worldCollisionSphereFrameName(int world_collision_sphere_index) const = 0;\n\n  //! Return the name of the frame associated with the self-collision sphere corresponding to\n  //! `self_collision_sphere_index`.\n  //!\n  //! A fatal error is logged if:\n  //!  1. `self_collision_sphere_index` is greater than or equal to the number of self-collision\n  //!     spheres attached to the robot (i.e., `numSelfCollisionSpheres()`), *OR*\n  //!  2. `self_collision_sphere_index` is negative.\n  [[nodiscard]]\n  virtual std::string selfCollisionSphereFrameName(int self_collision_sphere_index) const = 0;\n\n  //! Return the radii of all world-collision spheres on the robot.\n  //!\n  //! Each radius is the \"effective radius\", representing the sum of the \"base radius\" of the sphere\n  //! and any specified world-collision buffer distance.\n  //!\n  //! The order of `sphere_radii` corresponds to the order of `sphere_positions` returned by\n  //! `worldCollisionSpherePositions()`.\n  //!\n  //! The length of `sphere_radii` will be set to the number of world-collision spheres specified in\n  //! the `RobotDescription`.\n  virtual void worldCollisionSphereRadii(std::vector<double> &sphere_radii) const = 0;\n\n  //! Return the radii of all self-collision spheres on the robot.\n  //!\n  //! Each radius is the \"effective radius\", representing the sum of the \"base radius\" of the sphere\n  //! and any specified self-collision buffer distance.\n  //!\n  //! The order of `sphere_radii` corresponds to the order of `sphere_positions` returned by\n  //! `selfCollisionSpherePositions()`.\n  //!\n  //! The length of `sphere_radii` will be set to the number of self-collision spheres specified in\n  //! the `RobotDescription`.\n  virtual void selfCollisionSphereRadii(std::vector<double> &sphere_radii) const = 0;\n\n  //! Compute positions of all world-collision spheres on the robot at the specified\n  //! `cspace_position`.\n  //!\n  //! The order of `sphere_positions` corresponds to order of `sphere_radii` returned by\n  //! `worldCollisionSphereRadii()`.\n  //!\n  //! If the length of `sphere_positions` is less than the number of world-collision spheres\n  //! specified in the `RobotDescription`, `sphere_positions` will be resized to the number of\n  //! world-collision spheres. If the length of `sphere_positions` is larger than the number of\n  //! world-collision spheres, the vector will NOT be resized. Only the first n elements will be\n  //! written to, where n is the number of world-collision spheres.\n  virtual void worldCollisionSpherePositions(\n      const Eigen::VectorXd &cspace_position,\n      std::vector<Eigen::Vector3d> &sphere_positions) const = 0;\n\n  //! Compute positions of all self-collision spheres on the robot at the specified\n  //! `cspace_position`.\n  //!\n  //! The order of `sphere_positions` corresponds to order of `sphere_radii` returned by\n  //! `selfCollisionSphereRadii()`.\n  //!\n  //! If the length of `sphere_positions` is less than the number of self-collision spheres\n  //! specified in the `RobotDescription`, `sphere_positions` will be resized to the number of\n  //! self-collision spheres. If the length of `sphere_positions` is larger than the number of\n  //! self-collision spheres, the vector will NOT be resized. Only the first n elements will be\n  //! written to, where n is the number of self-collision spheres.\n  virtual void selfCollisionSpherePositions(\n      const Eigen::VectorXd &cspace_position,\n      std::vector<Eigen::Vector3d> &sphere_positions) const = 0;\n\n  //! Determine whether a specified `cspace_position` puts the robot into collision\n  //! with an obstacle.\n  //!\n  //! If no world view is specified, `false` is returned for any `cspace_position`.\n  virtual bool inCollisionWithObstacle(const Eigen::VectorXd &cspace_position) const = 0;\n\n  //! Compute the minimum pair-wise signed distance between the set of robot spheres and the set of\n  //! obstacles for the provided position in configuration space.  A positive distance implies that\n  //! the obstacle and the robot are *NOT* in collision.\n  //!\n  //! A fatal error is logged if the `RobotWorldInspector` does not have a world view set.\n  //!\n  //! NOTE: A distance of zero implies that the closest robot sphere to any obstacle intersects the\n  //!       obstacle at a point.\n  virtual double minDistanceToObstacle(const Eigen::VectorXd &cspace_position) const = 0;\n\n  //! Compute the signed distance between a given obstacle and collision sphere for the provided\n  //! position in configuration space.  A positive distance implies that the obstacle and the sphere\n  //! are *NOT* in collision.\n  //!\n  //! A fatal error is logged if:\n  //!  - `world_collision_sphere_index` is greater than or equal to the number of world-collision\n  //!    spheres attached to the robot (i.e., `numWorldCollisionSpheres()`), or it is negative.\n  //!  - The `RobotWorldInspector` does not have a world view set.\n  //!\n  //! NOTE: If the world-collision sphere intersects the `obstacle` a distance of zero is returned.\n  virtual double distanceToObstacle(const World::ObstacleHandle &obstacle,\n                                    int world_collision_sphere_index,\n                                    const Eigen::VectorXd &cspace_position) const = 0;\n\n  //! Indicate whether the robot is in self-collision at the given `cspace_position`.\n  //!\n  //! The robot is considered to be in self-collision if any collision sphere on any frame\n  //! intersects a collision sphere from another frame for which collisions have not been masked.\n  virtual bool inSelfCollision(const Eigen::VectorXd &cspace_position) const = 0;\n\n  //! Return pairs of frames for which self-collision occurs at the given `cspace_position`.\n  //!\n  //! Each pair of self-collision frames will be returned once in arbitrary order (e.g., the pair\n  //! {\"linkA\", \"linkB\"} may be returned *OR* the pair {\"linkB\", \"linkA\"} may be returned, but\n  //! not both).\n  virtual std::vector<std::pair<std::string, std::string>> framesInSelfCollision(\n      const Eigen::VectorXd &cspace_position) const = 0;\n};\n\n//! Create a `RobotWorldInspector` for a given `robot_description` and `world_view`.\n//!\n//! The `world_view` is optional. If not provided, queries related to obstacle collisions\n//! (e.g., `distanceToObstacle()`) will not be functional unless a world view is set by\n//! `setWorldView()` after construction.\nCUMO_EXPORT std::unique_ptr<RobotWorldInspector> CreateRobotWorldInspector(\n    const RobotDescription &robot_description,\n    std::optional<WorldViewHandle> world_view);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/rotation3.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2020-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for representing a rotation in 3d.\n\n#pragma once\n\n#include \"Eigen/Core\"\n#include \"Eigen/Geometry\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Class representing a 3d rotation.\nclass CUMO_EXPORT Rotation3 {\n public:\n  //! Construct rotation from a quaternion.\n  //!\n  //! By default, the `quaternion` will be normalized, but the argument `skip_normalization` can be\n  //! set to `true` to skip this normalization. This should only be done if the `quaternion` is\n  //! known to be normalized; otherwise, `Rotation3` operations will not work as expected.\n  explicit Rotation3(const Eigen::Quaterniond &quaternion, bool skip_normalization = false);\n  Rotation3(double w, double x, double y, double z);\n\n  //! Return a rotation that rotates by an angle (in radians) around a given axis.\n  //!\n  //! By default, the `axis` will be normalized (and if the axis norm is near zero, an identity\n  //! rotation will be constructed). The argument `skip_normalization` can set to `true` to skip\n  //! this normalization. This should only be done if the `axis` is known to be normalized;\n  //! otherwise, `Rotation3` may represent an invalid rotation.\n  static Rotation3 FromAxisAngle(const Eigen::Vector3d &axis,\n                                 double angle,\n                                 bool skip_normalization = false);\n\n  //! Return a rotation converted from a scaled axis.\n  //!\n  //! The magnitude of `scaled_axis` specifies the rotation angle in radians and the direction\n  //! of `scaled_axis` specifies the axis of rotation.\n  static Rotation3 FromScaledAxis(const Eigen::Vector3d &scaled_axis);\n\n  //! Return a rotation converted from a 3 x 3 rotation matrix.\n  //!\n  //! Internally, the `rotation_matrix` will be converted to a `Eigen::Quaterniond`. By default,\n  //! this quaternion will be normalized, but the argument `skip_normalization` can be\n  //! set to `true` to skip this normalization. This should only be done if the `rotation_matrix` is\n  //! known to be SO3; otherwise, `Rotation3` operations will not work as expected.\n  static Rotation3 FromMatrix(const Eigen::Matrix3d &rotation_matrix,\n                              bool skip_normalization = false);\n\n  //! Create identity rotation.\n  static Rotation3 Identity();\n\n  //! Return quaternion representation of the rotation. Returned quaternion is always normalized.\n  inline Eigen::Quaterniond quaternion() const { return quaternion_; }\n\n  //! Return `w` component of the quaternion representation of the rotation.\n  [[nodiscard]] double w() const { return quaternion_.w(); }\n\n  //! Return `x` component of the quaternion representation of the rotation.\n  [[nodiscard]] double x() const { return quaternion_.x(); }\n\n  //! Return `y` component of the quaternion representation of the rotation.\n  [[nodiscard]] double y() const { return quaternion_.y(); }\n\n  //! Return `z` component of the quaternion representation of the rotation.\n  [[nodiscard]] double z() const { return quaternion_.z(); }\n\n  //! Return matrix representation of the rotation.\n  [[nodiscard]] Eigen::Matrix3d matrix() const;\n\n  //! Return scaled axis representation of the rotation where magnitude of the returned vector\n  //! represents the rotation angle in radians.\n  [[nodiscard]] Eigen::Vector3d scaledAxis() const;\n\n  //! Returns the inverse rotation.\n  [[nodiscard]] Rotation3 inverse() const;\n\n  //! Compute the minimum angular distance (in radians) between two rotations.\n  static double Distance(const Rotation3 &rotation0, const Rotation3 &rotation1);\n\n  //! Smoothly interpolate between two rotations using spherical linear interpolation (\"slerp\").\n  //!\n  //! Intermediate rotations will follow a geodesic between `rotation0` and `rotation1` when\n  //! represented as quaternions on a unit sphere.\n  //!\n  //! The parameter `t` must be in range [0, 1], with 0 corresponding to `rotation0` and 1\n  //! corresponding to `rotation1`.\n  static Rotation3 Slerp(const Rotation3 &rotation0, const Rotation3 &rotation1, double t);\n\n  //! Compose rotations via * operator.\n  friend CUMO_EXPORT Rotation3 operator*(const Rotation3 &lhs, const Rotation3 &rhs);\n\n  //! Rotates a vector by the given rotation.\n  friend CUMO_EXPORT Eigen::Vector3d operator*(const Rotation3 &rotation,\n                                               const Eigen::Vector3d &vector);\n\n  //! See https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html\n  EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n private:\n  Eigen::Quaterniond quaternion_;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/task_space_path.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/pose3.h\"\n\nnamespace cumotion {\n\n//! Represent a path through task space (i.e., SE(3) group representing 6-dof poses).\n//!\n//! This path is parameterized by independent variable `s` and is generally expected to be\n//! continuous, but not necessarily smooth.\nclass CUMO_EXPORT TaskSpacePath {\n public:\n  virtual ~TaskSpacePath() = default;\n\n  //! Indicates continuous range of independent values, `s`, for which the path is defined.\n  struct CUMO_EXPORT Domain {\n    //! Minimum value of `s` defining domain.\n    double lower;\n\n    //! Maximum value of `s` defining domain.\n    double upper;\n\n    //! Convenience function to return the span of `s` values included in domain.\n    [[nodiscard]] double span() const { return upper - lower; }\n  };\n\n  //! Return the domain for the path.\n  [[nodiscard]] virtual Domain domain() const = 0;\n\n  //! Evaluate the path at the given `s`.\n  //!\n  //! A fatal error is logged if `s` is outside of the path `domain()`.\n  [[nodiscard]] virtual Pose3 eval(double s) const = 0;\n\n  //! Return the total translation distance accumulated along the path.\n  //!\n  //! This length is not, in general, equal to the translation distance between the poses at the\n  //! lower and upper bounds of the domain.\n  //!\n  //! E.g., if a path moved linearly from (0,0,0) to (0,1,0) to (1,1,0) to (1,0,0), then the path\n  //! length would be 3 while the net translation between the positions at the lower (0,0,0) and\n  //! upper (1,0,0) bounds of the domain would be 1.\n  [[nodiscard]] virtual double pathLength() const = 0;\n\n  //! Return the total angular distance (in radians) accumulated throughout the path.\n  //!\n  //! Similar to `pathLength()`, this value is not, in general, equal to the minimum angle between\n  //! the rotations at the lower and upper bounds of the domain.\n  //!\n  //! E.g., if a path rotates 2 pi around a single axis followed by pi around another axis, then the\n  //! accumulated rotation would be 3 pi, while the net rotation between the rotations at the lower\n  //! and upper bounds of the domain would be pi.\n  [[nodiscard]] virtual double accumulatedRotation() const = 0;\n\n  //! Return the minimum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These minimum position values are not, in general, synchronous. Instead, they represent\n  //!       the minimum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::Vector3d minPosition() const = 0;\n\n  //! Return the maximum position of the path within the defined `domain()`.\n  //!\n  //! NOTE: These maximum position values are not, in general, synchronous. Instead, they represent\n  //!       the maximum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::Vector3d maxPosition() const = 0;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/task_space_path_spec.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/task_space_path.h\"\n\nnamespace cumotion {\n\n//! The `TaskSpacePathSpec` is used to procedurally specify a `TaskSpacePath` from a series of\n//! continuous task space segments.\n//!\n//! Each segment can have position that is:\n//!   [1] constant,\n//!   [2] linearly interpolated,\n//!   [3] or defined by a circular arc.\n//! For the case where position follows an arc, the arc can be defined either by a series of three\n//! points, or by two endpoints and a constraint that the arc be tangent to the previous segment.\n//!\n//! Each segment may have orientation that is:\n//!   [1] constant (w.r.t. to the fixed global frame),\n//!   [2] smoothly blended via spherical linear interpolation (i.e., \"slerp\"), or\n//!   [3] constant w.r.t. the tangent direction of the position path (i.e., \"tangent orientation\").\n//! The \"tangent orientation\" specification is only relevant for path segments for which position is\n//! defined by an arc (since for linear position paths, the \"tangent orientation\" specification\n//! would reduce to a constant orientation w.r.t. the fixed global frame).\nclass CUMO_EXPORT TaskSpacePathSpec {\n public:\n  virtual ~TaskSpacePathSpec() = default;\n\n  //! Add a path to linearly connect the previous pose to the `target_pose`.\n  //!\n  //! Position will use linear interpolation and orientation will use \"slerp\".\n  //!\n  //! An optional `blend_radius` can be used to add arc segments between successive segments.\n  //! This arc segment will be added *only* if the successive segments both represent a path for\n  //! which position is linearly interpolated. Moreover, the `blend_radius` will be capped such that\n  //! no more than half of either linear segment is replaced by the intermediate arc segment. If\n  //! `blend_radius` is <= 0, then no blending will be performed.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addLinearPath(const Pose3 &target_pose, double blend_radius = 0.0) = 0;\n\n  //! Add a translation-only path to linearly connect the previous pose to the `target_position`.\n  //!\n  //! Position will use linear interpolation, and orientation will be constant.\n  //!\n  //! An optional `blend_radius` can be used to add arc segments between successive segments.\n  //! This arc segment will be added *only* if the successive segments both represent a path for\n  //! which position is linearly interpolated. Moreover, the `blend_radius` will be capped such that\n  //! no more than half of either linear segment is replaced by the intermediate arc segment. If\n  //! `blend_radius` is <= 0, then no blending will be performed.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addTranslation(const Eigen::Vector3d &target_position,\n                              double blend_radius = 0.0) = 0;\n\n  //! Add a rotation-only path connecting the previous pose to the `target_rotation`.\n  //!\n  //! Orientation will use \"slerp\" for interpolation, and position will be constant.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addRotation(const Rotation3 &target_rotation) = 0;\n\n  //! Add a path to connect the previous pose to the `target_position` along a circular arc that\n  //! passes through `intermediate_position`.\n  //!\n  //! Position will follow a \"three-point arc\" where the previous position and `target_position` are\n  //! endpoints and `intermediate_position` is an intermediate point on the arc.\n  //!\n  //! If `constant_orientation` is set to `true`, the orientation will be constant throughout the\n  //! arc.\n  //!\n  //! If `constant_orientation` is set to `false`, the orientation will remain fixed relative to the\n  //! tangent direction of the arc (i.e., \"tangent orientation\"; this tangent orientation is defined\n  //! such that if the angular distance of the arc is N radians, then the change in orientation\n  //! throughout the arc will be N radians about the normal axis of the arc).\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addThreePointArc(const Eigen::Vector3d &target_position,\n                                const Eigen::Vector3d &intermediate_position,\n                                bool constant_orientation = true) = 0;\n\n  //! Add a path to connect the previous pose to the `target_pose` along a circular arc that\n  //! passes through `intermediate_position`.\n  //!\n  //! Position will follow a \"three-point arc\" where the previous position and\n  //! `target_pose.translation` are endpoints and `intermediate_position` is an intermediate point\n  //! on the arc.\n  //!\n  //! Orientation will use \"slerp\" for interpolation between the previous orientation and\n  //! `target_pose.rotation`.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addThreePointArcWithOrientationTarget(\n      const Pose3 &target_pose,\n      const Eigen::Vector3d &intermediate_position) = 0;\n\n  //! Add a path to connect the previous pose to the `target_position` along a circular arc that\n  //! is tangent to the previous segment\n  //!\n  //! Position will follow a \"tangent arc\" where the previous position and `target_position` are\n  //! endpoints and the arc is tangent to the previous segment.\n  //!\n  //! It is required that at least one previous segment on the path defines a change in position for\n  //! a \"tangent arc\" to be able to be added. If no segments have been added or only rotation\n  //! segments have been added, then an error will be logged, no \"tangent arc\" segment will be\n  //! added, and `false` will be returned.\n  //!\n  //! If `constant_orientation` is set to `true`, the orientation will be constant throughout the\n  //! arc.\n  //!\n  //! If `constant_orientation` is set to `false`, the orientation will remain fixed relative to the\n  //! tangent direction of the arc (i.e., \"tangent orientation\"; this tangent orientation is defined\n  //! such that if the angular distance of the arc is N radians, then the change in orientation\n  //! throughout the arc will be N radians about the normal axis of the arc).\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addTangentArc(const Eigen::Vector3d &target_position,\n                             bool constant_orientation = true) = 0;\n\n  //! Add a path to connect the previous pose to the `target_pose` along a circular arc that\n  //! is tangent to the previous segment.\n  //!\n  //! Position will follow a \"tangent arc\" where the previous position and `target_pose.translation`\n  //! are endpoints and the arc is tangent to the previous segment.\n  //!\n  //! It is required that at least one previous segment on the path defines a change in position for\n  //! a \"tangent arc\" to be able to be added. If no segments have been added or only rotation\n  //! segments have been added, then an error will be logged, no \"tangent arc\" segment will be\n  //! added, and `false` will be returned.\n  //!\n  //! Orientation will use \"slerp\" for interpolation between the previous orientation and\n  //! `target_pose.rotation`.\n  //!\n  //! If path segment is successfully added, `true` is returned. Else, `false` is returned and an\n  //! error is logged.\n  virtual bool addTangentArcWithOrientationTarget(const Pose3 &target_pose) = 0;\n\n  //! Generate a continuous path between all of the procedurally added task space path segments.\n  //!\n  //! The lower bound of the domain of the generated path will be zero and the upper bound will be\n  //! determined by task space distance.\n  [[nodiscard]] virtual std::unique_ptr<TaskSpacePath> generatePath() const = 0;\n};\n\n//! Create a `TaskSpacePathSpec` with the specified `initial_pose`.\nCUMO_EXPORT std::unique_ptr<TaskSpacePathSpec> CreateTaskSpacePathSpec(const Pose3 &initial_pose);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/text_style.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief ANSI escape sequence definitions for formatting console text\n//!\n//! See, for example,\n//! https://en.wikipedia.org/wiki/ANSI_escape_code#SGR_(Select_Graphic_Rendition)_parameters\n\n#pragma once\n\nnamespace cumotion {\nnamespace text_style {\n\n// Reset all attributes.\nstatic constexpr char RESET[] = \"\\x1B[0m\";\n\n// Formatting\nstatic constexpr char BOLD[] = \"\\x1B[1m\";\nstatic constexpr char DIM[] = \"\\x1B[2m\";\nstatic constexpr char UNDERLINE[] = \"\\x1B[4m\";\nstatic constexpr char BLINK[] = \"\\x1B[5m\";\nstatic constexpr char REVERSE[] = \"\\x1B[7m\";\n\n// Foreground colors\nstatic constexpr char DEFAULT[] = \"\\x1B[39m\";\nstatic constexpr char BLACK[] = \"\\x1B[30m\";\nstatic constexpr char RED[] = \"\\x1B[31m\";\nstatic constexpr char GREEN[] = \"\\x1B[32m\";\nstatic constexpr char YELLOW[] = \"\\x1B[33m\";\nstatic constexpr char BLUE[] = \"\\x1B[34m\";\nstatic constexpr char MAGENTA[] = \"\\x1B[35m\";\nstatic constexpr char CYAN[] = \"\\x1B[36m\";\nstatic constexpr char LIGHT_GRAY[] = \"\\x1B[37m\";\nstatic constexpr char DARK_GRAY[] = \"\\x1B[90m\";\nstatic constexpr char BRIGHT_RED[] = \"\\x1B[91m\";\nstatic constexpr char BRIGHT_GREEN[] = \"\\x1B[92m\";\nstatic constexpr char BRIGHT_YELLOW[] = \"\\x1B[93m\";\nstatic constexpr char BRIGHT_BLUE[] = \"\\x1B[94m\";\nstatic constexpr char BRIGHT_MAGENTA[] = \"\\x1B[95m\";\nstatic constexpr char BRIGHT_CYAN[] = \"\\x1B[96m\";\nstatic constexpr char WHITE[] = \"\\x1B[97m\";\n\n// Background colors\nstatic constexpr char BG_DEFAULT[] = \"\\x1B[49m\";\nstatic constexpr char BG_BLACK[] = \"\\x1B[40m\";\nstatic constexpr char BG_RED[] = \"\\x1B[41m\";\nstatic constexpr char BG_GREEN[] = \"\\x1B[42m\";\nstatic constexpr char BG_YELLOW[] = \"\\x1B[43m\";\nstatic constexpr char BG_BLUE[] = \"\\x1B[44m\";\nstatic constexpr char BG_MAGENTA[] = \"\\x1B[45m\";\nstatic constexpr char BG_CYAN[] = \"\\x1B[46m\";\nstatic constexpr char BG_LIGHT_GRAY[] = \"\\x1B[47m\";\nstatic constexpr char BG_DARK_GRAY[] = \"\\x1B[100m\";\nstatic constexpr char BG_BRIGHT_RED[] = \"\\x1B[101m\";\nstatic constexpr char BG_BRIGHT_GREEN[] = \"\\x1B[102m\";\nstatic constexpr char BG_BRIGHT_YELLOW[] = \"\\x1B[103m\";\nstatic constexpr char BG_BRIGHT_BLUE[] = \"\\x1B[104m\";\nstatic constexpr char BG_BRIGHT_MAGENTA[] = \"\\x1B[105m\";\nstatic constexpr char BG_BRIGHT_CYAN[] = \"\\x1B[106m\";\nstatic constexpr char BG_WHITE[] = \"\\x1B[107m\";\n\n}  // namespace text_style\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/trajectory.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Represent a path through configuration space (i.e., \"c-space\") parameterized by time.\nclass CUMO_EXPORT Trajectory {\n public:\n  virtual ~Trajectory() = default;\n\n  //! Return the number of configuration space coordinates for the kinematic structure.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Indicates the continuous range of time values, `t`, for which the trajectory and its\n  //! derivatives are defined.\n  struct CUMO_EXPORT Domain {\n    //! Minimum value of time defining domain.\n    double lower;\n\n    //! Maximum value of time defining domain.\n    double upper;\n\n    //! Convenience function to return the span of time values included in domain.\n    [[nodiscard]] double span() const { return upper - lower; }\n  };\n\n  //! Return the domain for the trajectory.\n  [[nodiscard]] virtual Domain domain() const = 0;\n\n  //! Evaluate the trajectory at the given `time`.\n  virtual void eval(double time,\n                    Eigen::VectorXd *cspace_position,\n                    Eigen::VectorXd *cspace_velocity = nullptr,\n                    Eigen::VectorXd *cspace_acceleration = nullptr,\n                    Eigen::VectorXd *cspace_jerk = nullptr) const = 0;\n\n  //! Evaluate specified trajectory derivative at the given `time` and return value.\n  //!\n  //! The default `derivative_order` is the \"zeroth derivative\" (which is simply the c-space\n  //! position of the trajectory). Setting `derivative_order` to 1 will output the c-space velocity,\n  //! with higher `derivative_order`s corresponding to higher derivatives.\n  //!\n  //! Trajectories are expected to support at least the third derivative (i.e., \"jerk\").\n  //!\n  //! The default implementation internally calls the above `eval()` function and will log a fatal\n  //! error if the `derivative_order` is not in range [0, 3].\n  [[nodiscard]] virtual Eigen::VectorXd eval(double time, int derivative_order = 0) const;\n\n  //! Return the minimum position for each c-space coordinate within the defined `domain()`.\n  //!\n  //! NOTE: These minimum position values are not, in general, synchronous. Instead, they represent\n  //!       the minimum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd minPosition() const = 0;\n\n  //! Return the maximum position for each c-space coordinate within the defined `domain()`.\n  //!\n  //! NOTE: These maximum position values are not, in general, synchronous. Instead, they represent\n  //!       the maximum position independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd maxPosition() const = 0;\n\n  //! Return the maximum magnitude of velocity for each c-space coordinate within the defined\n  //! `domain()`.\n  //!\n  //! NOTE: These maximum magnitude values are not, in general, synchronous. Instead, they represent\n  //!       the maximum magnitudes independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd maxVelocityMagnitude() const = 0;\n\n  //! Return the maximum magnitude of acceleration for each c-space coordinate within the defined\n  //! `domain()`.\n  //!\n  //! NOTE: These maximum magnitude values are not, in general, synchronous. Instead, they represent\n  //!       the maximum magnitudes independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd maxAccelerationMagnitude() const = 0;\n\n  //! Return the maximum magnitude of jerk for each c-space coordinate within the defined\n  //! `domain()`.\n  //!\n  //! NOTE: These maximum magnitude values are not, in general, synchronous. Instead, they represent\n  //!       the maximum magnitudes independently achieved by each coordinate.\n  [[nodiscard]] virtual Eigen::VectorXd maxJerkMagnitude() const = 0;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/trajectory_generator.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/kinematics.h\"\n#include \"cumotion/trajectory.h\"\n\nnamespace cumotion {\n\n//! Configure a trajectory generator that can compute smooth* trajectories. A trajectory generation\n//! problem is specified by:\n//!\n//!   1. Bound constraints (position, velocity, and acceleration),\n//!   2. Intermediate position waypoints, and\n//!   3. Limits on some combination of position, velocity, acceleration and jerk.\n//!\n//! Times for domain bounds and intermediate waypoints are *not* specified. It is assumed that a\n//! time-optimal trajectory that respects the constraints (with the definition of time-optimality\n//! being implementation-specific) will be generated.\n//!\n//! * The definition of \"smooth\" is implementation-specific. Currently, `CSpaceTrajectoryGenerator`\n//!   uses a single implementation that interpolates through waypoints using a series of cubic\n//!   splines. For this implementation, \"smooth\" is defined as smooth velocity, continuous\n//!   acceleration, and bounded jerk.\nclass CUMO_EXPORT CSpaceTrajectoryGenerator {\n public:\n  virtual ~CSpaceTrajectoryGenerator() = default;\n\n  //! Return the number of configuration space coordinates for the trajectory generator.\n  [[nodiscard]] virtual int numCSpaceCoords() const = 0;\n\n  //! Set position limits.\n  //!\n  //! A fatal error is logged if either `min_position` or `max_position` does not have a length\n  //! matching the expected number of c-space coordinates, or if any coordinate of\n  //! `max_position` is not greater than the corresponding coordinate of `min_position`.\n  virtual void setPositionLimits(const Eigen::VectorXd &min_position,\n                                 const Eigen::VectorXd &max_position) = 0;\n\n  //! Set velocity magnitude limits.\n  //!\n  //! A fatal error is logged if `max_velocity` does not have a length matching the expected number\n  //! of c-space coordinates, or if any coordinate of `max_velocity` is negative.\n  //!\n  //! WARNING: The current implementation using a series of cubic splines requires velocity limits\n  //!          to be specified. This restriction may be lifted in future versions of the\n  //!          `CSpaceTrajectoryGenerator`.\n  virtual void setVelocityLimits(const Eigen::VectorXd &max_velocity) = 0;\n\n  //! Set acceleration magnitude limits.\n  //!\n  //! A fatal error is logged if `max_acceleration` does not have a length matching the expected\n  //! number of c-space coordinates, or if any coordinate of `max_acceleration` is negative.\n  virtual void setAccelerationLimits(const Eigen::VectorXd &max_acceleration) = 0;\n\n  //! Set jerk magnitude limits.\n  //!\n  //! A fatal error is logged if `max_jerk` does not have a length matching the expected\n  //! number of c-space coordinates, or if any coordinate of `max_jerk` is negative.\n  virtual void setJerkLimits(const Eigen::VectorXd &max_jerk) = 0;\n\n  //! Attempt to generate a time-optimal trajectory passing through the specified `waypoints` with\n  //! the specified constraints.\n  //!\n  //! If a trajectory cannot be generated, `nullptr` is returned.\n  [[nodiscard]] virtual std::unique_ptr<Trajectory> generateTrajectory(\n      const std::vector<Eigen::VectorXd> &waypoints) const = 0;\n\n  //! Interpolation modes used by `interpolateTrajectory()`.\n  enum class InterpolationMode {\n    //! Linear interpolation between c-space positions.\n    //!\n    //! The resulting trajectory will have continuous (but not smooth) position and discontinuous\n    //! segments of constant velocity. Acceleration and jerk will be returned as zero (including at\n    //! velocity discontinuities where values are technically undefined).\n    LINEAR,\n    //! Piecewise solution with a cubic spline between adjacent c-space positions.\n    //!\n    //! The resulting trajectory will have smooth position and velocity, continuous (but not smooth)\n    //! acceleration, and bounded (but discontinuous) jerk.\n    CUBIC_SPLINE\n  };\n\n  //! Attempt to interpolate a trajectory passing through the specified `waypoints` at the specified\n  //! `times`.\n  //!\n  //! Interpolation will fail if:\n  //! 1. The number of specified `positions` does not match the number of specified `times`;\n  //! 2. There is not at least one position specified for `LINEAR` interpolation mode or at least\n  //!    two positions specified for `CUBIC_SPLINE` interpolation mode;\n  //! 3. Not all `positions` have the same number of c-space coordinates;\n  //! 4. Not all `times` are strictly increasing; *OR*\n  //! 5. The interpolated trajectory does not satisfy specified limits on c-space position,\n  //!    velocity, acceleration, or jerk.\n  //!\n  //! If a trajectory cannot be generated due to invalid input (items 1-4 above), a fatal error is\n  //! logged. If failure is due to c-space limits (item 5 above), then `nullptr` is returned.\n  [[nodiscard]] virtual std::unique_ptr<Trajectory> generateTimeStampedTrajectory(\n      const std::vector<Eigen::VectorXd> &waypoints,\n      const std::vector<double> &times,\n      InterpolationMode interpolation_mode = InterpolationMode::CUBIC_SPLINE) const = 0;\n\n  //================================================================================================\n  // Interface for setting implementation-specific solver parameters.\n  // NOTE: Default values are likely to be sufficient for most use-cases.\n\n  //! Specify the value for a given parameter.\n  //!\n  //! The required `SolverParamValue` constructor for each parameter is detailed in the\n  //! documentation for `setSolverParam()`.\n  struct CUMO_EXPORT SolverParamValue {\n    //! Create `SolverParamValue` from `int`.\n    SolverParamValue(int value);  // NOLINT Allow implicit conversion\n    //! Create `SolverValue` from `double`.\n    SolverParamValue(double value);  // NOLINT Allow implicit conversion\n    //! Create `SolverParamValue` from `const char*`.\n    SolverParamValue(const char *value);  // NOLINT Allow implicit conversion\n    //! Create `SolverParamValue` from `std::string`.\n    SolverParamValue(const std::string &value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Set the value of the solver parameter.\n  //!\n  //! Currently, `CSpaceTrajectoryGenerator` uses a single solver implementation based on computing\n  //! a series of cubic splines. The following parameters can be set for this piecewise cubic spline\n  //! solver:\n  //!\n  //! `max_segment_iterations` [`int`]\n  //!   - The first step towards finding a time-optimal trajectory using a series of cubic\n  //!     splines is to iteratively compute an optimal span for each spline segment.\n  //!   - Setting a relatively high `max_segment_iterations` will, in general, result in shorter\n  //!     trajectory time spans, but will tend to take longer to converge.\n  //!   - This step can be skipped completely by setting `max_segment_iterations` to zero.\n  //!   - `max_segment_iterations` must be non-negative. Additionally at least one of\n  //!     `max_segment_iterations` and `max_aggregate_iterations` must be non-zero.\n  //!   - Default value is 5.\n  //!\n  //! `max_aggregate_iterations` [`int`]\n  //!   - The second step towards finding a time-optimal trajectory using a series of cubic splines\n  //!     is to iteratively compute an optimal span for the entire trajectory.\n  //!   - Relying more heavily on this second step (i.e., setting `max_segment_iterations` to a\n  //!     a relatively low value) will, in general, require less iterations (and thus less time) to\n  //!     converge, but the generated trajectories will tend to have longer time spans.\n  //!   - This step can be skipped completely by setting `max_aggregate_iterations` to zero.\n  //!   - `max_aggregate_iterations` must be non-negative. Additionally at least one of\n  //!     `max_segment_iterations` and `max_aggregate_iterations` must be non-zero.\n  //!   - Default value is 50.\n  //!\n  //! `convergence_dt` [`double`]\n  //!   - The search for optimal time values will terminate if the maximum change to any time value\n  //!     during a given iteration is less than the `convergence_dt`.\n  //!   - `convergence_dt` must be positive.\n  //!   - Default value is 0.001.\n  //!\n  //! `max_dilation_iterations` [`int`]\n  //!   - After the segment-wise and/or aggregate time-optimal search has converged or reached\n  //!     maximum iterations, the resulting set of splines will be tested to see if any derivative\n  //!     limits are exceeded.\n  //!   - If any derivative limits are exceeded, the splines will be iteratively scaled in time to\n  //!     reduce the maximum achieved derivative. This process will repeat until no derivative\n  //!     limits are exceeded (success) or `max_dilation_iterations` are reached (failure).\n  //!   - `max_dilation_iterations` must be non-negative.\n  //!   - Default value is 100.\n  //!\n  //! `dilation_dt` [`double`]\n  //!   - For the iterative dilation step described in `max_dilation_iterations` documentation, the\n  //!     `dilation_dt` is the \"epsilon\" value added to the span of the trajectory that exceeds\n  //!     derivative limits.\n  //!   - `dilation_dt` must be positive.\n  //!   - Default value is 0.001.\n  //!\n  //! `min_time_span` [`double`]\n  //!   - Specify the minimum allowable time span between adjacent waypoints/endpoints.\n  //!   - `min_time_span` must be positive.\n  //!   - Default value is 0.01.\n  //!\n  //! `time_split_method` [`string`]\n  //!   - Specify the `TimeSplitMethod` for the initial distribution of time values that will be\n  //!     used to iteratively search for time-optimal values.\n  //!   - Valid settings are `uniform`, `chord_length` and `centripetal`.\n  [[nodiscard]]\n  virtual bool setSolverParam(const std::string &param_name, SolverParamValue value) = 0;\n};\n\n//! Create a `CSpaceTrajectoryGenerator` with the specified number of configuration space\n//! coordinates.\n//!\n//! Position and derivative limits, bound constraints, and intermediate waypoints can be added\n//! after construction.\nCUMO_EXPORT std::unique_ptr<CSpaceTrajectoryGenerator> CreateCSpaceTrajectoryGenerator(\n    int num_cspace_coords);\n\n//! Create a `CSpaceTrajectoryGenerator` with the specified `kinematics`.\n//!\n//! The `kinematics` will be used to specify the number of configuration space coordinates, position\n//! limits, and any available derivative limits.\n//!\n//! Position and derivative limits may be added or overwritten after construction. Bound\n//! constraints and intermediate waypoints can be added after construction.\nCUMO_EXPORT std::unique_ptr<CSpaceTrajectoryGenerator> CreateCSpaceTrajectoryGenerator(\n    const Kinematics &kinematics);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/trajectory_optimizer.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <filesystem>\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/robot_description.h\"\n#include \"cumotion/rotation3.h\"\n#include \"cumotion/trajectory.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Configuration parameters for a `TrajectoryOptimizer`.\nclass CUMO_EXPORT TrajectoryOptimizerConfig {\n public:\n  virtual ~TrajectoryOptimizerConfig() = default;\n\n  //! Specify the value for a given parameter.\n  //!\n  //! The required `ParamValue` constructor for each param is detailed in the\n  //! documentation for `setParam()`.\n  struct CUMO_EXPORT ParamValue {\n    //! Create `ParamValue` from `int`.\n    ParamValue(int value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `double`.\n    ParamValue(double value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `bool`.\n    ParamValue(bool value);  // NOLINT Allow implicit conversion\n    //! Create `ParamValue` from `std::vector<double>`.\n    ParamValue(const std::vector<double> &value);  // NOLINT Allow implicit conversion\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Set the value of the parameter.\n  //!\n  //! `setParam()` returns `true` if the parameter has been successfully updated and `false` if an\n  //! error has occurred (e.g., invalid parameter).\n  //!\n  //! The following parameters can be set for `TrajectoryOptimizer`:\n  //!\n  //! `enable_self_collision` [`bool`]\n  //!   - Enable or disable self-collision avoidance between robot spheres during optimization.\n  //!   - When `true`, both inverse kinematics (IK) and trajectory optimization will include\n  //!     collision costs to prevent robot spheres from colliding with each other.\n  //!   - When `false`, self-collision costs are disabled, which may improve solve time but allows\n  //!     self-intersecting trajectories.\n  //!   - Default value is `true` (self-collision avoidance enabled).\n  //!\n  //! `enable_world_collision` [`bool`]\n  //!   - Enable or disable collision avoidance with world obstacles during optimization.\n  //!   - When `true`, both IK and trajectory optimization will include collision costs to avoid\n  //!     obstacles in the environment.\n  //!   - When `false`, world collision costs are disabled, which may improve performance but allows\n  //!     trajectories that intersect with obstacles.\n  //!   - Default value is `true` (world collision avoidance enabled).\n  //!\n  //! `task_space_terminal_position_tolerance` [`double`]\n  //!   - Maximum allowable violation of the user-specified constraint on the position of the tool\n  //!     frame.\n  //!   - Used both for IK and trajectory optimization problems.\n  //!     It determines when a solution is considered to have satisfied position targets.\n  //!     Smaller values require more precise solutions but may be harder to achieve.\n  //!   - Units are in meters.\n  //!   - A default value of 1e-3 (1mm) is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `task_space_terminal_orientation_tolerance` [`double`]\n  //!   - Maximum allowable violation of the user-specified constraint on the orientation of the\n  //!     tool frame.\n  //!   - Used both for IK and trajectory optimization problems.\n  //!     It determines when a solution is considered to have satisfied orientation targets.\n  //!     Smaller values require more precise solutions but may be harder to achieve.\n  //!   - Units are in radians.\n  //!   - A default value of 5e-3 (approximately 0.29 degrees) is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `ik/num_seeds` [`int`]\n  //!   - Number of seeds used to solve the inverse kinematics (IK) problem.\n  //!   - The trajectory optimizer generates multiple pseudo-random c-space configurations and\n  //!     optimizes them to find diverse collision-free configurations for the desired tool pose.\n  //!     Higher values increase the likelihood of finding valid solutions but increase\n  //!     computational cost.\n  //!   - A default value of 32 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `ik/max_reattempts` [`int`]\n  //!   - Maximum number of times to restart the IK problem with different random seeds, in case of\n  //!     failure, before giving up.\n  //!   - Higher values increase the likelihood of finding valid IK solutions but increase\n  //!     memory usage and the maximum possible time to find a solution. A value of 0 means no\n  //!     retries (i.e. only perform the initial attempt).\n  //!   - A default value of 10 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/desired_cspace_position_weight` [`double`]\n  //!   - Weight used on the distance to a desired c-space configuration when ranking IK solutions.\n  //!   - When this weight is zero, the valid IK solutions are ranked purely by their tool\n  //!     constraint violations.\n  //!   - A default value of 1.0 provides balanced behavior for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `ik/pbo/cost/tool_frame_position_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space position error violations at tool-frame constraints\n  //!     during PBO IK optimization.\n  //!   - Higher values more strongly enforce exact position matching at goal targets.\n  //!   - A default value of `10000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/tool_frame_position_error_penalty/activation_distance` [`double`]\n  //!   - Distance from the deviation limit at which task-space position error penalties activate.\n  //!     This value is subtracted from the deviation limit specified when creating the position\n  //!     constraint (e.g., `TranslationConstraint::Target()`) during PBO IK\n  //!     optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero position errors will be penalized, encouraging solutions that have as little\n  //!     error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final error of `deviation_limit - activation_distance`, instead of an error\n  //!     near zero.\n  //!   - Units are in meters.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/tool_frame_orientation_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space orientation error violations at tool-frame\n  //!     constraints during PBO IK optimization.\n  //!   - Higher values more strongly enforce exact orientation matching at goal targets.\n  //!   - A default value of `5000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/tool_frame_orientation_error_penalty/activation_distance` [`double`]\n  //!   - Angular distance from the deviation limit at which task-space orientation error penalties\n  //!     activate. This value is subtracted from the deviation limit specified when creating the\n  //!     orientation constraint (e.g., `OrientationConstraint::TerminalTarget()`) during\n  //!     PBO IK optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero orientation errors will be penalized, encouraging solutions that have as little\n  //!     angular error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final angular error of `deviation_limit - activation_distance`, instead of an\n  //!     error near zero.\n  //!   - Units are in radians.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/cspace_position_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied when IK solutions violate c-space position limits, minus the\n  //!     activation distance, during Particle-Based Optimizer (PBO) IK optimization.\n  //!   - Higher values more strongly discourage solutions near joint limits.\n  //!   - A default value of 5000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/cspace_position_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from c-space position limits at which the position limit penalty\n  //!     activates, during PBO IK optimization.\n  //!   - Units correspond to rad for revolute joints, m for prismatic joints.\n  //!   - A default value of 0.05 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/enable_cspace_position_limit` [`bool`]\n  //!   - Enable or disable c-space position limit penalties during PBO IK optimization.\n  //!   - When `true`, PBO IK will penalize solutions that violate or approach c-space\n  //!     position limits.\n  //!   - When `false`, position limit costs are ignored (not recommended).\n  //!   - Default value is `true` (position limit penalties enabled).\n  //!\n  //! `ik/pbo/cost/self_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in self-colliding robot configurations during PBO IK optimization.\n  //!   - Higher values more strongly discourage self-colliding configurations.\n  //!   - This parameter is only used when `enable_self_collision` is `true`.\n  //!   - A default value of 500.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/self_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the self-collision penalty activates during PBO IK\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     each other.\n  //!   - Units are in meters.\n  //!   - A default value of 0.01 m (1 cm) is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/world_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in world-colliding robot configurations during PBO IK optimization.\n  //!   - Higher values more strongly discourage configurations that collide with obstacles.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 500.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/pbo/cost/world_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the world collision penalty activates during PBO IK\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     obstacles.\n  //!   - Units are in meters.\n  //!   - A default value of 0.035 m is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/tool_frame_position_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space position error violations at tool-frame constraints\n  //!     during L-BFGS IK optimization.\n  //!   - Higher values more strongly enforce exact position matching at goal targets.\n  //!   - A default value of `10000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/tool_frame_position_error_penalty/activation_distance` [`double`]\n  //!   - Distance from the deviation limit at which task-space position error penalties activate.\n  //!     This value is subtracted from the deviation limit specified when creating the position\n  //!     constraint (e.g., `TranslationConstraint::Target()`) during L-BFGS IK\n  //!     optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero position errors will be penalized, encouraging solutions that have as little\n  //!     error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final error of `deviation_limit - activation_distance`, instead of an error\n  //!     near zero.\n  //!   - Units are in meters.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/tool_frame_orientation_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space orientation error violations at tool-frame\n  //!     constraints during L-BFGS IK optimization.\n  //!   - Higher values more strongly enforce exact orientation matching at goal targets.\n  //!   - A default value of `5000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/tool_frame_orientation_error_penalty/activation_distance` [`double`]\n  //!   - Angular distance from the deviation limit at which task-space orientation error penalties\n  //!     activate. This value is subtracted from the deviation limit specified when creating the\n  //!     orientation constraint (e.g., ``OrientationConstraint::TerminalTarget()`) during\n  //!     L-BFGS IK optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero orientation errors will be penalized, encouraging solutions that have as little\n  //!     angular error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final angular error of `deviation_limit - activation_distance`, instead of an\n  //!     error near zero.\n  //!   - Units are in radians.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/cspace_position_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied when IK solutions violate c-space position limits, minus the\n  //!     activation distance, during L-BFGS IK optimization.\n  //!   - Higher values more strongly discourage solutions near joint limits.\n  //!   - A default value of 100.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/cspace_position_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from c-space position limits at which the position limit penalty\n  //!     activates, during L-BFGS IK optimization.\n  //!   - Units correspond to rad for revolute joints, m for prismatic joints.\n  //!   - A default value equivalent to 5 degrees is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/enable_cspace_position_limit` [`bool`]\n  //!   - Enable or disable c-space position limit penalties during L-BFGS IK optimization.\n  //!   - When `true`, L-BFGS IK will penalize solutions that violate or approach c-space\n  //!     position limits.\n  //!   - When `false`, position limit costs are ignored (not recommended).\n  //!   - Default value is `true` (position limit penalties enabled).\n  //!\n  //! `ik/lbfgs/cost/self_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in self-colliding robot configurations during L-BFGS IK\n  //!     optimization.\n  //!   - Higher values more strongly discourage self-colliding configurations.\n  //!   - This parameter is only used when `enable_self_collision` is `true`.\n  //!   - A default value of 1000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/self_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the self-collision penalty activates during L-BFGS IK\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     each other.\n  //!   - Units are in meters.\n  //!   - A default value of 0.01 m (1 cm) is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/world_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in world-colliding robot configurations during L-BFGS IK\n  //!     optimization.\n  //!   - Higher values more strongly discourage configurations that collide with obstacles.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 1000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/cost/world_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the world collision penalty activates during L-BFGS IK\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     obstacles.\n  //!   - Units are in meters.\n  //!   - A default value of 0.001 m (1 mm) is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `ik/lbfgs/max_iterations` [`int`]\n  //!   - Maximum number of L-BFGS iterations for the inverse kinematics solver.\n  //!   - Higher values allow the IK solver to converge more precisely but increase computational\n  //!     cost per IK attempt.\n  //!   - A default value of 100 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `trajopt/num_seeds` [`int`]\n  //!   - Number of seeds used to solve the trajectory optimization problem.\n  //!   - The trajectory optimizer generates interpolated trajectories between the initial\n  //!     configuration and each of the IK solutions, and then optimizes each trajectory.\n  //!     Higher values provide more trajectory candidates but increase computational cost.\n  //!   - A default value of 4 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `trajopt/num_knots_per_trajectory` [`int`]\n  //!   - Number of knot points per trajectory used for trajectory optimization.\n  //!   - This parameter controls the temporal resolution of the optimized trajectory, where higher\n  //!     values provide finer temporal resolution but increase computational cost.\n  //!   - A default value of 32 is recommended for most use-cases.\n  //!   - Must be greater than or equal to 4.\n  //!\n  //! `trajopt/pbo/enabled` [`bool`]\n  //!   - Enable or disable Particle-Based Optimizer (PBO) for the trajectory optimization problem.\n  //!   - PBO is an algorithm that uses a set of \"particles\" to explore a non-linear design space.\n  //!     If enabled, PBO is used to find collision-free trajectories before starting the\n  //!     gradient-based L-BFGS optimization algorithm.  PBO is helpful for complex scenarios where\n  //!     a linearly-interpolated path between the initial configuration and the IK solutions is not\n  //!     sufficient to find a collision-free trajectory.\n  //!   - When set to `false`, PBO is disabled, and L-BFGS is initialized with a\n  //!     linearly-interpolated path.\n  //!   - When set to `true`, PBO is enabled and the `trajopt/pbo` parameters control its behavior.\n  //!   - Default value is `true` (PBO enabled).\n  //!\n  //! `trajopt/pbo/num_particles_per_problem` [`int`]\n  //!   - Number of particles used by PBO in the trajectory optimization problem, when enabled.\n  //!   - This parameter controls the population size of particles in the PBO algorithm. Higher\n  //!     values increase the likelihood of finding better solutions but increase solve time and\n  //!     memory usage.\n  //!   - This parameter is only used when `trajopt/pbo/enabled` is set to `true`.\n  //!   - A default value of 25 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `trajopt/pbo/num_iterations` [`int`]\n  //!   - Number of iterations to run PBO in the trajectory optimization problem, when enabled.\n  //!   - This parameter controls how many optimization iterations PBO performs.\n  //!     Higher values allow for more thorough optimization but increase computational cost.\n  //!   - This parameter is only used when `trajopt/pbo/enabled` is set to `true`.\n  //!   - A default value of 5 is recommended for most use-cases.\n  //!   - Must be positive.\n  //!\n  //! `trajopt/pbo/cost/path_position_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space position error violations at tool-frame path\n  //!     constraints during PBO trajectory optimization for non-terminal knot points.\n  //!   - Higher values more strongly enforce exact position matching at goal targets.\n  //!   - A default value of `600.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/path_position_error_penalty/activation_distance` [`double`]\n  //!   - Distance from the deviation limit at which task-space path position error penalties\n  //!     activate.  This value is subtracted from the deviation limit specified when creating the\n  //!     path position constraint (e.g., `TranslationConstraint::LinearPathConstraint()`) during\n  //!     PBO trajectory optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero position errors will be penalized, encouraging solutions that have as little\n  //!     error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final error of `deviation_limit - activation_distance`, instead of an error\n  //!     near zero.\n  //!   - Units are in meters.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/path_orientation_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space path orientation error violations at tool-frame path\n  //!     constraints during PBO trajectory optimization.\n  //!   - Higher values more strongly enforce exact orientation matching at goal targets.\n  //!   - A default value of `100.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/path_orientation_error_penalty/activation_distance` [`double`]\n  //!   - Angular distance from the deviation limit at which task-space path orientation error\n  //!     penalties activate. This value is subtracted from the deviation limit specified when\n  //!     creating the path orientation constraint (e.g.,\n  //!     `OrientationConstraint::TerminalAndPathTarget()`) during PBO trajectory optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero orientation errors will be penalized, encouraging solutions that have as little\n  //!     angular error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final angular error of `deviation_limit - activation_distance`, instead of an\n  //!     error near zero.\n  //!   - Units are in radians.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_position_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space position limit violations during PBO trajectory\n  //!     optimization.\n  //!   - It discourages trajectories that violate joint limits.\n  //!   - A default value of 10.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_position_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from c-space position limits for PBO trajectory optimization penalties.\n  //!   - Units correspond to rad/s for revolute joints, m/s for prismatic joints.\n  //!   - A default value of 0.0 (no activation distance) is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_velocity_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space velocity limit violations during PBO trajectory\n  //!     optimization.\n  //!   - It discourages trajectories with velocities that exceed joint limits.\n  //!   - A default value of 0.0 (disabled) is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_velocity_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from velocity limits for PBO trajectory optimization penalties.\n  //!   - Units correspond to rad/s for revolute joints, m/s for prismatic joints.\n  //!   - A default value of 0.0 is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_acceleration_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space acceleration limit violations during PBO trajectory\n  //!     optimization.\n  //!   - It discourages trajectories with accelerations that exceed joint limits.\n  //!   - A default value of 0.0 (disabled) is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_acceleration_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from acceleration limits for PBO trajectory optimization penalties.\n  //!   - Units correspond to rad/s^2 for revolute joints, m/s^2 for prismatic joints.\n  //!   - A default value of 0.0 is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_jerk_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space jerk limit violations during PBO trajectory\n  //!     optimization.\n  //!   - It discourages trajectories with jerks that exceed joint limits.\n  //!   - A default value of 0.0 (disabled) is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_jerk_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from jerk limits for PBO trajectory optimization penalties.\n  //!   - Units correspond to rad/s^3 for revolute joints, m/s^3 for prismatic joints.\n  //!   - A default value of 0.0 is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_acceleration_smoothing_cost` [`double`]\n  //!   - Weight used to encourage smooth trajectories during PBO trajectory optimization by\n  //!     penalizing c-space acceleration.\n  //!   - Higher values encourage smoother trajectories by penalizing non-zero accelerations.\n  //!   - A default value of 10.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/cspace_jerk_smoothing_cost` [`double`]\n  //!   - Weight applied to minimize c-space jerk (smoothness regularization) during PBO trajectory\n  //!     optimization.\n  //!   - Higher values encourage smoother trajectories by penalizing non-zero jerk values.\n  //!   - A default value of 0.0 (disabled) is used.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/self_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in self-colliding robot configurations during PBO trajectory\n  //!     optimization.\n  //!   - This parameter is only used when `enable_self_collision` is `true`.\n  //!   - A default value of 100000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/self_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the self-collision penalty activates during PBO trajectory\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     each other.\n  //!   - Units are in meters.\n  //!   - A default value of 0.01 m (1 cm) is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/world_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in world-colliding robot configurations during PBO trajectory\n  //!     optimization.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 5000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/world_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the world collision penalty activates during PBO trajectory\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     obstacles.\n  //!   - Units are in meters.\n  //!   - A default value of 0.025 m is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/pbo/cost/world_collision_max_sweep_steps` [`int`]\n  //!   - Maximum number of discrete sampling steps when \"sweeping\" collision spheres between\n  //!     trajectory knot points during PBO optimization. \"Sweeping\" samples sphere positions at\n  //!     intervals along the path between knot points to detect collisions that might be missed\n  //!     if only checking at the knot points themselves.\n  //!   - Higher values provide more accurate collision detection but increase computational cost.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 5 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/path_position_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space position error violations at tool-frame path\n  //!     constraints during L-BFGS trajectory optimization for non-terminal knot points.\n  //!   - Higher values more strongly enforce exact position matching at goal targets.\n  //!   - A default value of `50000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/path_position_error_penalty/activation_distance` [`double`]\n  //!   - Distance from the deviation limit at which task-space path position error penalties\n  //!     activate.  This value is subtracted from the deviation limit specified when creating the\n  //!     path position constraint (e.g., `TranslationConstraint::LinearPathConstraint()`) during\n  //!     L-BFGS trajectory optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero position errors will be penalized, encouraging solutions that have as little\n  //!     error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final error of `deviation_limit - activation_distance`, instead of an error\n  //!     near zero.\n  //!   - Units are in meters.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/path_orientation_error_penalty/weight` [`double`]\n  //!   - Penalty weight applied in task-space path orientation error violations at tool-frame path\n  //!     constraints during L-BFGS trajectory optimization.\n  //!   - Higher values more strongly enforce exact orientation matching at goal targets.\n  //!   - A default value of `10000.0` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/path_orientation_error_penalty/activation_distance` [`double`]\n  //!   - Angular distance from the deviation limit at which task-space path orientation error\n  //!     penalties activate. This value is subtracted from the deviation limit specified when\n  //!     creating the path orientation constraint (e.g.,\n  //!     `OrientationConstraint::TerminalAndPathTarget()`) during L-BFGS trajectory optimization.\n  //!   - If the activation distance is greater than the deviation limit (which is often zero), all\n  //!     non-zero orientation errors will be penalized, encouraging solutions that have as little\n  //!     angular error as possible.\n  //!   - If the activation distance is less than the deviation limit, any solution with an error\n  //!     less than `deviation_limit - activation_distance` will have equal cost. Most solutions\n  //!     will have a final angular error of `deviation_limit - activation_distance`, instead of an\n  //!     error near zero.\n  //!   - Units are in radians.\n  //!   - A default value of `0.01` is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_position_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space position limit violations during L-BFGS trajectory\n  //!     optimization.\n  //!   - Higher values more strongly discourage trajectories that violate joint limits.\n  //!   - A default value of 50000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_position_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from c-space position limits for L-BFGS trajectory optimization\n  //!     penalties.\n  //!   - Units correspond to rad/s for revolute joints, m/s for prismatic joints.\n  //!   - A default value equivalent to 5 degrees is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_velocity_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space velocity limit violations during L-BFGS trajectory\n  //!     optimization.\n  //!   - Higher values more strongly discourage trajectories with excessive joint velocities.\n  //!   - A default value of 1000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_velocity_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from velocity limits for L-BFGS trajectory optimization penalties.\n  //!   - Units correspond to rad/s for revolute joints and m/s for prismatic joints.\n  //!   - A default value equivalent to 5 degrees/s is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_acceleration_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space acceleration limit violations during L-BFGS trajectory\n  //!     optimization.\n  //!   - Higher values more strongly discourage trajectories with excessive joint accelerations.\n  //!   - A default value of 1000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_acceleration_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from acceleration limits for L-BFGS trajectory optimization penalties.\n  //!   - Units correspond to rad/s^2 for revolute joints and m/s^2 for prismatic joints.\n  //!   - A default value equivalent to 5 degrees/s^2 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_jerk_limit_penalty/weight` [`double`]\n  //!   - Penalty weight applied in c-space jerk limit violations during L-BFGS trajectory\n  //!     optimization.\n  //!   - Higher values more strongly discourage trajectories with excessive joint jerk.\n  //!   - A default value of 10.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_jerk_limit_penalty/activation_distance` [`double`]\n  //!   - Distance threshold from jerk limits for L-BFGS trajectory optimization penalties.\n  //!   - Units correspond to rad/s^3 for revolute joints and m/s^3 for prismatic joints.\n  //!   - A default value equivalent to 5 degrees/s^3 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_acceleration_smoothing_cost` [`double`]\n  //!   - Weight applied to minimize c-space acceleration (smoothness regularization) during L-BFGS\n  //!     trajectory optimization.\n  //!   - Higher values encourage smoother trajectories by penalizing large accelerations.\n  //!   - A default value of 10.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/cspace_jerk_smoothing_cost` [`double`]\n  //!   - Weight applied to minimize c-space jerk (smoothness regularization) during L-BFGS\n  //!     trajectory optimization.\n  //!   - Higher values encourage smoother trajectories by penalizing large jerk values.\n  //!   - A default value of 0.01 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/self_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in self-colliding robot configurations during L-BFGS trajectory\n  //!     optimization.\n  //!   - This parameter is only used when `enable_self_collision` is `true`.\n  //!   - A default value of 100000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/self_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the self-collision penalty activates during L-BFGS trajectory\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     each other.\n  //!   - Units are in meters.\n  //!   - A default value of 0.01 m (1 cm) is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/world_collision_penalty/weight` [`double`]\n  //!   - Penalty weight applied in world-colliding robot configurations during L-BFGS trajectory\n  //!     optimization.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 1000000.0 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/world_collision_penalty/activation_distance` [`double`]\n  //!   - Distance threshold at which the world-collision penalty activates during L-BFGS trajectory\n  //!     optimization.\n  //!   - The penalty becomes non-zero when robot collision geometries are within this distance of\n  //!     obstacles.\n  //!   - Units are in meters.\n  //!   - A default value of 0.015 m is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/cost/world_collision_max_sweep_steps` [`int`]\n  //!   - Maximum number of discrete sampling steps when \"sweeping\" collision spheres between\n  //!     trajectory knot points during L-BFGS optimization. \"Sweeping\" samples sphere positions at\n  //!     intervals along the path between knot points to detect collisions that might be missed\n  //!     if only checking at the knot points themselves.\n  //!   - Higher values provide more accurate collision detection but increase computational cost.\n  //!   - This parameter is only used when `enable_world_collision` is `true`.\n  //!   - A default value of 5 is recommended for most use-cases.\n  //!   - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/history_length` [`int`]\n  //!   - Number of past iterations stored by the L-BFGS algorithm to approximate the inverse\n  //!     Hessian during trajectory optimization.\n  //!   - Larger values may improve convergence quality but increase memory usage and per-iteration\n  //!     computational cost.\n  //!   - A default value of 5 is recommended for most use-cases.\n  //!   - Must be less than or equal to the number of optimization variables in the trajectory\n  //!     optimization problem, calculated as\n  //!     `RobotDescription::numCSpaceCoords() * ('trajopt/num_knots_per_trajectory' - 2)`.\n  //!     This value is automatically clamped to not exceed the number of optimization variables, if\n  //!     setting any other parameter (e.g., 'trajopt/num_knots_per_trajectory') reduces the upper\n  //!     bound.\n  //!   - Must be positive.\n  //!\n  //! `trajopt/lbfgs/initial_iterations` [`int`]\n  //! - Number of L-BFGS iterations used to solve the initial trajectory optimization problem.\n  //! - A smaller number of iterations will result in a more coarse solution used when retiming\n  //!   the trajectory.\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - A value of 0 will result in the initial retiming attempt using the output of the\n  //!   particle-based optimizer.\n  //! - Default value: 300\n  //! - Must be non-negative.\n  //!\n  //! `trajopt/lbfgs/retiming_initial_iterations` [`int`]\n  //! - Number of L-BFGS iterations used to solve the 2nd trajectory optimization problem, after\n  //!   the initial solutions are retimed using `trajopt/timestep_scaling_factor` to minimize the\n  //!   trajectory time by saturating the kinematic limits.\n  //! - A smaller number of iterations will result in a more coarse solution used when retiming\n  //!   solution used when retiming the trajectory.\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - Default value: 400\n  //! - Must be positive.\n  //!\n  //! `trajopt/lbfgs/retiming_iterations` [`int`]\n  //! - Number of L-BFGS iterations used to solve the trajectory optimization problem after the\n  //!   initial solve and first retiming attempt, after the previous solution is scaled by\n  //!   `trajopt/timestep_scaling_factor` to minimize trajectory time by saturating the\n  //!   kinematic limits.\n  //! - A smaller number of iterations will result in a more coarse final solution, including the\n  //!   possibility of failing to find a valid solution where one would have been found with a\n  //!   greater number of iterations.\n  //! - A larger number of iterations will require more compute time, but may provide a\n  //!   higher-quality final trajectory (typically meaning smoother kinematic derivatives).\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - Default value: 100\n  //! - Must be positive.\n  //!\n  //! `trajopt/num_geometric_planning_attempts` [`int`]\n  //!  - Number of attempts that will be made to use a geometric planner to generate a feasible\n  //!    path to the target as a fallback when standard trajectory optimization fails.  Geometric\n  //!    planning is, in general, computationally expensive relative to trajectory optimization, but\n  //!    it can be necessary in complex environments.\n  //!  - A default value of 4 is used.\n  //!  - Must be non-negative.\n  //!\n  //! `trajopt/geometric_planning/max_iterations` [`int`]\n  //!   - Maximum number of iterations for the geometric planner per attempt.\n  //!   - Higher values increase the likelihood of finding a solution in complex environments but\n  //!     increase the maximum runtime of each geometric planning attempt.\n  //!   - Default value: 10000\n  //!   - Must be positive.\n  //!\n  //! `trajopt/initial_timestep_scaling_factor` [`double`]\n  //! - Scales the saturating timestep calculated on the initial trajectory seeds.\n  //! - Smaller values result in shorter initial trajectories where the derivative limits are\n  //!   violated for a greater percentage of the trajectory.\n  //! - A value of 1.0 or more will produce an initial trajectory that does not violate any\n  //!   kinematic derivatives along the trajectory.\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - Should typically be less than 1.0, since values greater than 1.0 will undersaturate the\n  //!   derivative limits, leading to longer trajectory times.\n  //! - Default value: 0.85\n  //! - Must be positive.\n  //!\n  //! `trajopt/timestep_scaling_factor` [`double`]\n  //! - Scales the saturating timestep calculated on the retimed trajectory seeds (after the initial\n  //!   solve).\n  //! - Smaller values result in shorter retimed trajectories where the derivative limits are\n  //!   violated for a greater percentage of the trajectory.\n  //! - A value of 1.0 or more will produce a retimed trajectory that does not violate any\n  //!   kinematic derivatives along the trajectory.\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - Should typically be less than 1.0, since values greater than 1.0 will undersaturate the\n  //!   derivative limits, leading to longer trajectory times.\n  //! - Default value: 0.85\n  //! - Must be positive.\n  //!\n  //! `trajopt/num_retiming_iterations` [`int`]\n  //! - Number of times the trajectory is retimed after the initial solve.\n  //! - A smaller number of iterations may result in a shorter planning time and a longer final\n  //!   trajectory time. A higher number of iterations may result in a shorter final trajectory\n  //!   time.\n  //! - The optimizer may exit early if no trajectories are valid after retiming.\n  //! - The default value is a reasonable starting point, but performance improvements may be\n  //!   found by tuning this parameter.\n  //! - A value of 0 will disable retiming and the trajectory will use the timestep calculated\n  //!   by saturating the kinematic limits of the initial solution.\n  //! - Default value: 5\n  //! - Must be non-negative.\n  [[nodiscard]] virtual bool setParam(const std::string &param_name, ParamValue value) = 0;\n};\n\n//! Load a set of `TrajectoryOptimizer` configuration parameters from file.\n//!\n//! These parameters are combined with `robot_description`, `tool_frame_name`, and `world_view`\n//! to create a configuration for trajectory optimization.\n//!\n//! Default values will be used for any parameters not specified in\n//! `trajectory_optimizer_config_file`.\n//!\n//! A configuration will *NOT* be created if:\n//!   1. `trajectory_optimizer_config_file` path is invalid or empty,\n//!   2. `trajectory_optimizer_config_file` points to an invalid YAML file,\n//!   3. `trajectory_optimizer_config_file` contains invalid contents (e.g., parameters, version),\n//!   4. `robot_description` is invalid, *OR*\n//!   5. `tool_frame_name` is not a valid frame in `robot_description`.\n//!\n//! In the case of failure, a non-fatal error will be logged and a `nullptr` will be returned.\n[[nodiscard]] CUMO_EXPORT std::unique_ptr<TrajectoryOptimizerConfig>\nCreateTrajectoryOptimizerConfigFromFile(\n    const std::filesystem::path &trajectory_optimizer_config_file,\n    const RobotDescription &robot_description,\n    const std::string &tool_frame_name,\n    const WorldViewHandle &world_view);\n\n//! Use default parameters to create a configuration for trajectory optimization.\n//!\n//! These default parameters are combined with `robot_description`, `tool_frame_name`, and\n//! `world_view`.\n//!\n//! A configuration will *NOT* be created if:\n//!   1. `robot_description` is invalid, *OR*\n//!   2. `tool_frame_name` is not a valid frame in `robot_description`.\n//!\n//! In the case of failure, a non-fatal error will be logged and a `nullptr` will be returned.\n[[nodiscard]] CUMO_EXPORT std::unique_ptr<TrajectoryOptimizerConfig>\nCreateDefaultTrajectoryOptimizerConfig(const RobotDescription &robot_description,\n                                       const std::string &tool_frame_name,\n                                       const WorldViewHandle &world_view);\n\n//! Interface for using numerical optimization to generate collision-free trajectories for a robot.\n//!\n//! \\rst\n//! See documentation for corresponding :py:class:`Python class <cumotion.TrajectoryOptimizer>`.\n//! \\endrst\nclass CUMO_EXPORT TrajectoryOptimizer {\n public:\n  virtual ~TrajectoryOptimizer() = default;\n\n  //! Translation constraints restrict the position of the origin of a tool frame.\n  //!\n  //! These constraints are always active at the terminal point of the trajectory; partial\n  //! constraints may be active along the path.\n  class CUMO_EXPORT TranslationConstraint {\n   public:\n    //! Create a `TranslationConstraint` such that a `translation_target` is specified at\n    //! termination, but no translation constraints are active along the path.\n    //!\n    //! The optional parameter `terminal_deviation_limit` can be used to allow deviation from the\n    //! `translation_target` at termination. This limit specifies the maximum allowable deviation\n    //! from the desired position.\n    //!\n    //! If `terminal_deviation_limit` is not input, then the deviation limit is assumed to be zero\n    //! (i.e., it is desired that terminal translation be exactly `translation_target`).\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `terminal_deviation_limit` is negative.\n    //!\n    //! NOTE: The `translation_target` is specified in world frame coordinates (i.e., relative to\n    //! the base of the robot).\n    static TranslationConstraint Target(const Eigen::Vector3d &translation_target,\n                                        const double *terminal_deviation_limit = nullptr);\n\n    //! Create a `TranslationConstraint` such that a `translation_target` is specified at\n    //! termination *AND* a linear translation constraint is active along the path.\n    //!\n    //! The linear path constraint is defined by the line **segment** between `translation_target`\n    //! and the origin of the tool frame at the initial c-space position. This path constraint is\n    //! satisfied if the origin of the tool frame is on this line segment between the initial and\n    //! final tool positions at all points in the trajectory. It is considered a constraint\n    //! violation if the origin of the tool frame \"overshoots\" either the initial or final\n    //! positions.\n    //!\n    //! If the distance between the initial and final tool frames are nearly the same, the path\n    //! constraint becomes a constraint on the distance from the initial and final positions in any\n    //! direction.\n    //!\n    //! The optional parameters `path_deviation_limit` and `terminal_deviation_limit` can be used\n    //! to allow deviations from the specified line along the path and at the end of the path,\n    //! respectively.\n    //!\n    //! If both limits are input, the `path_deviation_limit` must be greater than or equal to\n    //! (i.e., no more restrictive than) the `terminal_deviation_limit`.\n    //!\n    //! If neither is input, then both deviation limits are assumed to be zero, indicating that\n    //! translation should always be exactly coincident with the specified line.\n    //!\n    //! It is valid to input a `path_deviation_limit` without a `terminal_deviation_limit`\n    //! (implicitly setting the latter to zero) but not to input a `terminal_deviation_limit`\n    //! without a `path_deviation_limit`. This ensures that the terminal constraint is more\n    //! restrictive than the corresponding path constraint.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `path_deviation_limit` is negative,\n    //!   2. `terminal_deviation_limit` is negative,\n    //!   3. both `path_deviation_limit` and `terminal_deviation_limit` are defined, but\n    //!      `path_deviation_limit` < `terminal_deviation_limit`, *OR*\n    //!   4. `terminal_deviation_limit` is defined without defining a `path_deviation_limit`.\n    //!\n    //! NOTE: The `translation_target` is specified in world frame coordinates (i.e., relative to\n    //! the base of the robot).\n    static TranslationConstraint LinearPathConstraint(\n        const Eigen::Vector3d &translation_target,\n        const double *path_deviation_limit = nullptr,\n        const double *terminal_deviation_limit = nullptr);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `TranslationConstraint` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of `TranslationConstraint`s are considered concurrently. Each\n  //! `TranslationConstraint` in the goalset must have the same mode (e.g., \"terminal target\n  //! with linear path constraint\") but may have different data for each `TranslationConstraint`.\n  class CUMO_EXPORT TranslationConstraintGoalset {\n   public:\n    //! Create a `TranslationConstraintGoalset` such that `translation_targets` are specified at\n    //! termination, but no translation constraints are active along the path.\n    //!\n    //! See `TranslationConstraint::Target()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `TranslationConstraint::Target()` is not met, *OR*\n    //!   2. `translation_targets` is empty.\n    static TranslationConstraintGoalset Target(\n        const std::vector<Eigen::Vector3d> &translation_targets,\n        const double *terminal_deviation_limit = nullptr);\n\n    //! Create a `TranslationConstraintGoalset` such that `translation_targets` are specified at\n    //! termination *AND* linear translation constraints are active along the path.\n    //!\n    //! See `TranslationConstraint::LinearPathConstraint()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `TranslationConstraint::LinearPathConstraint()` is not met, *OR*\n    //!   2. `translation_targets` is empty.\n    static TranslationConstraintGoalset LinearPathConstraint(\n        const std::vector<Eigen::Vector3d> &translation_targets,\n        const double *path_deviation_limit = nullptr,\n        const double *terminal_deviation_limit = nullptr);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Orientation constraints restrict the orientation of a tool frame.\n  //!\n  //! These constraints may be active at the terminal point of the trajectory and/or along the path.\n  //! Each constraint may fully or partially constrain the orientation.\n  class CUMO_EXPORT OrientationConstraint {\n   public:\n    //! Create an `OrientationConstraint` such that no tool frame orientation constraints are\n    //! active along the path *OR* at termination.\n    static OrientationConstraint None();\n\n    //! Create an `OrientationConstraint` such that the tool frame orientation along the path *AND*\n    //! at termination are constrained to the tool frame orientation at the initial\n    //! c-space position.\n    //!\n    //! The optional parameters `path_deviation_limit` and `terminal_deviation_limit` can be used\n    //! to allow deviations from this constant orientation along the path and at termination\n    //! respectively. If input, the limits are expressed in radians. These limits specify the\n    //! maximum allowable deviation from the desired orientation.\n    //!\n    //! If both limits are input, the `path_deviation_limit` must be greater than or equal to\n    //! (i.e., no more restrictive than) the `terminal_deviation_limit`.\n    //!\n    //! If neither is input, then both deviation limits are assumed to be zero, indicating that\n    //! orientation should be exactly constant.\n    //!\n    //! It is valid to input a `path_deviation_limit` without a `terminal_deviation_limit`\n    //! (implicitly setting the latter to zero) but not to input a `terminal_deviation_limit`\n    //! without a `path_deviation_limit`. This ensures that the terminal constraint is more\n    //! restrictive than the corresponding path constraint.\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `path_deviation_limit` is near or greater than pi, *OR*\n    //!   2. `terminal_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `path_deviation_limit` is negative,\n    //!   2. `terminal_deviation_limit` is negative,\n    //!   3. both `path_deviation_limit` and `terminal_deviation_limit` are defined, but\n    //!      `path_deviation_limit` < `terminal_deviation_limit`, *OR*\n    //!   4. `terminal_deviation_limit` is defined without defining a `path_deviation_limit`.\n    static OrientationConstraint Constant(const double *path_deviation_limit = nullptr,\n                                          const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraint` such that a tool frame `orientation_target` is specified\n    //! at termination, but no orientation constraints are active along the path.\n    //!\n    //! The optional parameter `terminal_deviation_limit` can be used to allow deviation from the\n    //! `orientation_target` at termination. If input, `terminal_deviation_limit` is expressed in\n    //! radians. This limit specifies the maximum allowable deviation from the desired\n    //! orientation.\n    //!\n    //! If `terminal_deviation_limit` is not input, then the deviation limit is assumed to be zero\n    //! (i.e., it is desired that terminal orientation be exactly `orientation_target`).\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   - `terminal_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   - `terminal_deviation_limit` is negative.\n    //!\n    //! NOTE: The `orientation_target` is specified in world frame coordinates (i.e., relative to\n    //! the base of the robot).\n    static OrientationConstraint TerminalTarget(const Rotation3 &orientation_target,\n                                                const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraint` such that a tool frame `orientation_target` is specified\n    //! along the path *AND* at termination.\n    //!\n    //! The optional parameters `path_deviation_limit` and `terminal_deviation_limit` can be used\n    //! to allow deviations from the `orientation_target` along the path and at termination\n    //! respectively. If input, the limits are expressed in radians. These limits specify the\n    //! maximum allowable deviation from the desired orientation.\n    //!\n    //! If both limits are input, the `path_deviation_limit` must be greater than or equal to\n    //! (i.e., no more restrictive than) the `terminal_deviation_limit`.\n    //!\n    //! If the `path_deviation_limit` is *NOT* input, its value is set to the most restrictive\n    //! feasible value. This feasibility is determined by the tool frame orientation at the initial\n    //! c-space position. If the initial orientation exactly matches `orientation_target`, then\n    //! the effective `path_deviation_limit` will be zero. Otherwise, the `path_deviation_limit`\n    //! will be set to the angular distance between the initial orientation and\n    //! `orientation_target`. If the `path_deviation_limit` is input, it must be greater than or\n    //! equal to this angular distance.\n    //!\n    //! If the `terminal_deviation_limit` is *NOT* input, its value is set to zero (i.e., the most\n    //! restrictive feasible value). If `terminal_deviation_limit` *AND* `path_deviation_limit` are\n    //! input, then `terminal_deviation_limit` must be less than or equal to `path_deviation_limit`.\n    //! If `terminal_deviation_limit` is specified but `path_deviation_limit` is auto-computed, then\n    //! `terminal_deviation_limit` will be clamped to the minimum of the input value and the\n    //! auto-computed value of `path_deviation_limit`.\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `path_deviation_limit` is near or greater than pi, *OR*\n    //!   2. `terminal_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `path_deviation_limit` is negative,\n    //!   2. `path_deviation_limit` results in an infeasible initial c-space position,\n    //!   3. `terminal_deviation_limit` is negative, *OR*\n    //!   4. `terminal_deviation_limit` is greater than `path_deviation_limit`.\n    //!\n    //! NOTE: Condition [3] will only be validated when the resulting `OrientationConstraint` is\n    //!       used for trajectory optimization (i.e., it is used as input to\n    //!       `planToTaskSpaceTarget()`).\n    //!\n    //! NOTE: The `orientation_target` is specified in world frame coordinates (i.e., relative to\n    //!       the base of the robot).\n    static OrientationConstraint TerminalAndPathTarget(\n        const Rotation3 &orientation_target,\n        const double *path_deviation_limit = nullptr,\n        const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraint` such that the terminal tool frame orientation is\n    //! constrained to rotate about a \"free axis\", but no orientation constraints are active along\n    //! the path.\n    //!\n    //! The \"free axis\" for rotation is defined by a `tool_frame_axis` (specified in the tool frame\n    //! coordinates) and a corresponding `world_target_axis` (specified in world frame coordinates).\n    //!\n    //! The optional `terminal_axis_deviation_limit` can be used to allow deviation from the\n    //! desired axis alignment at termination. If input, `terminal_axis_deviation_limit` is\n    //! expressed in radians and the limit specifies the maximum allowable deviation of the\n    //! rotation axis at termination. If `axis_deviation_limit` is not input, then the deviation\n    //! limit is assumed to be zero (i.e., it is desired that the tool frame axis be exactly\n    //! aligned with `world_target_axis`).\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   - `terminal_axis_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `terminal_axis_deviation_limit` is negative,\n    //!   2. `tool_frame_axis` is (nearly) zero, *OR*\n    //!   3. `world_target_axis` is (nearly) zero.\n    //!\n    //! NOTE: `tool_frame_axis` and `world_target_axis` inputs will be normalized.\n    static OrientationConstraint TerminalAxis(\n        const Eigen::Vector3d &tool_frame_axis,\n        const Eigen::Vector3d &world_target_axis,\n        const double *terminal_axis_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraint` such that the tool frame orientation is constrained to\n    //! rotate about a \"free axis\" along the path *AND* at termination.\n    //!\n    //! The \"free axis\" for rotation is defined by a `tool_frame_axis` (specified in the tool frame\n    //! coordinates) and a corresponding `world_target_axis` (specified in world frame coordinates).\n    //!\n    //! The optional parameters `path_axis_deviation_limit` and `terminal_axis_deviation_limit`\n    //! can be used to allow deviation from the desired axis alignment along the path and at\n    //! termination respectively. If input, the limits are expressed in radians.\n    //!\n    //! If both limits are input, the `path_axis_deviation_limit` must be greater than or equal to\n    //! (i.e., no more restrictive than) the `terminal_axis_deviation_limit`.\n    //!\n    //! If the `path_axis_deviation_limit` is *NOT* input, its value is set to the most restrictive\n    //! feasible value. This feasibility is determined by the tool frame orientation at the initial\n    //! c-space position. If the initial orientation exactly satisfies the axis constraint,\n    //! then the effective `path_axis_deviation_limit` will be zero. Otherwise, the\n    //! `path_axis_deviation_limit` will be set to the angular distance between the initial tool\n    //! frame rotation axis and the desired tool frame rotation axis. If the\n    //! `path_axis_deviation_limit` is input, it must be greater than or equal to this angular\n    //! distance.\n    //!\n    //! If the `terminal_deviation_limit` is *NOT* input, its value is set to zero (i.e., the most\n    //! restrictive feasible value). If `terminal_deviation_limit` *AND* `path_deviation_limit` are\n    //! input, then `terminal_deviation_limit` must be less than or equal to `path_deviation_limit`.\n    //! If `terminal_deviation_limit` is specified but `path_deviation_limit` is auto-computed, then\n    //! `terminal_deviation_limit` will be clamped to the minimum of the input value and the\n    //! auto-computed value of `path_deviation_limit`.\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `path_axis_deviation_limit` is near or greater than pi, *OR*\n    //!   2. `terminal_axis_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `path_axis_deviation_limit` is negative,\n    //!   2. `path_axis_deviation_limit` results in an infeasible initial c-space position,\n    //!   3. `terminal_axis_deviation_limit` is negative,\n    //!   4. `terminal_axis_deviation_limit` is > to `path_deviation_limit`,\n    //!   5. `tool_frame_axis` is (nearly) zero, *OR*\n    //!   6. `world_target_axis` is (nearly) zero.\n    //!\n    //! NOTE: Condition [3] will only be validated when the resulting `OrientationConstraint` is\n    //!       used for trajectory optimization (i.e., it is used as input to\n    //!       `planToTaskSpaceTarget()`).\n    //!\n    //! NOTE: `tool_frame_axis` and `world_target_axis` inputs will be normalized.\n    static OrientationConstraint TerminalAndPathAxis(\n        const Eigen::Vector3d &tool_frame_axis,\n        const Eigen::Vector3d &world_target_axis,\n        const double *path_axis_deviation_limit = nullptr,\n        const double *terminal_axis_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraint` such that a tool frame `terminal_orientation_target` is\n    //! specified at termination, *AND* the tool frame orientation is constrained to rotate\n    //! about a \"free axis\" along the path.\n    //!\n    //! The optional parameter `terminal_deviation_limit` can be used to allow deviation from the\n    //! `orientation_target` at termination. If input, `terminal_deviation_limit` is expressed in\n    //! radians. This limit specifies the maximum allowable deviation from the desired\n    //! orientation.\n    //!\n    //! If `terminal_deviation_limit` is not input, then the deviation limit is assumed to be zero\n    //! (i.e., it is desired that terminal orientation be exactly `terminal_orientation_target`).\n    //!\n    //! The \"free axis\" for rotation along the path is defined by a `tool_frame_axis` (specified\n    //! in the tool frame coordinates) and a corresponding `world_target_axis` (specified in\n    //! world frame coordinates).\n    //!\n    //! The optional `path_axis_deviation_limit` can be used to allow deviation from the\n    //! desired axis alignment along the path. If input, `path_axis_deviation_limit` is\n    //! expressed in radians. This limit specifies the maximum allowable deviation of the\n    //! rotation axis along the path.\n    //!\n    //! If the `path_axis_deviation_limit` is *NOT* input, its value is set to the most restrictive\n    //! feasible value. This feasibility is determined by the tool frame orientation at the initial\n    //! c-space position. If the initial orientation exactly satisfies the axis constraint,\n    //! then the effective `path_axis_deviation_limit` will be zero. Otherwise, the\n    //! `path_axis_deviation_limit` will be set to the angular distance between the initial tool\n    //! frame rotation axis and the desired tool frame rotation axis. If the\n    //! `path_axis_deviation_limit` is input, it must be greater than or equal to this angular\n    //! distance.\n    //!\n    //! In general, it is suggested that angular deviation limits are set to values less than pi.\n    //! A limit that is near or greater than pi essentially disables the constraint (without culling\n    //! the computation). Non-fatal warnings will be logged if:\n    //!   1. `path_axis_deviation_limit` is near or greater than pi, *OR*\n    //!   2. `terminal_deviation_limit` is near or greater than pi.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. `terminal_deviation_limit` is negative,\n    //!   2. `path_axis_deviation_limit` is negative,\n    //!   3. `path_axis_deviation_limit` results in an infeasible initial c-space position,\n    //!   4. `tool_frame_axis` is (nearly) zero, *OR*\n    //!   5. `world_target_axis` is (nearly) zero.\n    //!\n    //! NOTE: Condition [5] will only be validated when the resulting `OrientationConstraint` is\n    //!       used for trajectory optimization (i.e., it is used as input to\n    //!       `planToTaskSpaceTarget()`).\n    //!\n    //! NOTE: `tool_frame_axis` and `world_target_axis` inputs will be normalized.\n    //!\n    //! NOTE: The `terminal_orientation_target` is specified in world frame coordinates\n    //!       (i.e., relative to the base of the robot).\n    static OrientationConstraint TerminalTargetAndPathAxis(\n        const Rotation3 &terminal_orientation_target,\n        const Eigen::Vector3d &tool_frame_axis,\n        const Eigen::Vector3d &world_target_axis,\n        const double *terminal_deviation_limit = nullptr,\n        const double *path_axis_deviation_limit = nullptr);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `OrientationConstraint` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of `OrientationConstraint`s are considered concurrently. Each\n  //! `OrientationConstraint` in the goalset must have the same mode (e.g., \"full terminal target\n  //! with free axis path constraint\"), but may have different data for each\n  //! `OrientationConstraint`.\n  class CUMO_EXPORT OrientationConstraintGoalset {\n   public:\n    //! Create an `OrientationConstraintGoalset` such that no tool frame orientation constraints\n    //! are active along the path *OR* at termination.\n    static OrientationConstraintGoalset None();\n\n    //! Create an `OrientationConstraintGoalset` such that the tool frame orientation along the\n    //! path *AND* at termination are constrained to the tool frame orientation at the initial\n    //! c-space position.\n    //!\n    //! See `OrientationConstraint::Constant()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::Constant()` is not met.\n    static OrientationConstraintGoalset Constant(const double *path_deviation_limit = nullptr,\n                                                 const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraintGoalset` such that tool frame `orientation_targets` are\n    //! specified at termination, but no orientation constraints are active along the path.\n    //!\n    //! See `OrientationConstraint::TerminalTarget()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::TerminalTarget()` is not met, *OR*\n    //!   2. `orientation_targets` is empty.\n    static OrientationConstraintGoalset TerminalTarget(\n        const std::vector<Rotation3> &orientation_targets,\n        const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraintGoalset` such that tool frame `orientation_targets` are\n    //! specified along the path *AND* at termination.\n    //!\n    //! See `OrientationConstraint::TerminalAndPathTarget()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::TerminalAndPathTarget()` is not met, *OR*\n    //!   2. `orientation_targets` is empty.\n    static OrientationConstraintGoalset TerminalAndPathTarget(\n        const std::vector<Rotation3> &orientation_targets,\n        const double *path_deviation_limit = nullptr,\n        const double *terminal_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraintGoalset` such that the terminal tool frame orientation is\n    //! constrained to rotate about a \"free axis\", but no orientation constraints are active along\n    //! the path.\n    //!\n    //! See `OrientationConstraint::TerminalAxis()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::TerminalAxis()` is not met,\n    //!   2. `tool_frame_axes` and `world_target_axes` do not have the same number of elements, *OR*\n    //!   3. `tool_frame_axes` and `world_target_axes` are empty.\n    static OrientationConstraintGoalset TerminalAxis(\n        const std::vector<Eigen::Vector3d> &tool_frame_axes,\n        const std::vector<Eigen::Vector3d> &world_target_axes,\n        const double *terminal_axis_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraintGoalset` such that the tool frame orientation is\n    //! constrained to rotate about a \"free axis\" along the path *AND* at termination.\n    //!\n    //! See `OrientationConstraint::TerminalAndPathAxis()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::TerminalAndPathAxis()` is not met,\n    //!   2. `tool_frame_axes` and `world_target_axes` do not have the same number of elements, *OR*\n    //!   3. `tool_frame_axes` and `world_target_axes` are empty.\n    static OrientationConstraintGoalset TerminalAndPathAxis(\n        const std::vector<Eigen::Vector3d> &tool_frame_axes,\n        const std::vector<Eigen::Vector3d> &world_target_axes,\n        const double *path_axis_deviation_limit = nullptr,\n        const double *terminal_axis_deviation_limit = nullptr);\n\n    //! Create an `OrientationConstraintGoalset` such that tool frame `terminal_orientation_targets`\n    //! are specified at termination, *AND* the tool frame orientation is constrained to rotate\n    //! about a \"free axis\" along the path.\n    //!\n    //! See `OrientationConstraint::TerminalTargetAndPathAxis()` for details.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. Any condition of `OrientationConstraint::TerminalTargetAndPathAxis()` is not met,\n    //!   2. `terminal_orientation_targets`, `tool_frame_axes`, and `world_target_axes` do not have\n    //!      the same number of elements, *OR*\n    //!   3. `terminal_orientation_targets`, `tool_frame_axes`, and `world_target_axes` are empty.\n    static OrientationConstraintGoalset TerminalTargetAndPathAxis(\n        const std::vector<Rotation3> &terminal_orientation_targets,\n        const std::vector<Eigen::Vector3d> &tool_frame_axes,\n        const std::vector<Eigen::Vector3d> &world_target_axes,\n        const double *terminal_deviation_limit = nullptr,\n        const double *path_axis_deviation_limit = nullptr);\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Task-space targets restrict the position and (optionally) orientation of the tool frame at the\n  //! termination of a trajectory and (optionally) along the path.\n  struct CUMO_EXPORT TaskSpaceTarget {\n    //! Create a task-space target.\n    explicit TaskSpaceTarget(\n        const TranslationConstraint &translation_constraint,\n        const OrientationConstraint &orientation_constraint = OrientationConstraint::None());\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Variant of `TaskSpaceTarget` for \"goalset\" planning.\n  //!\n  //! For goalset planning, a set of pose constraints are considered concurrently. Each pose\n  //! constraint in the goalset must have the same mode (e.g., \"terminal target\n  //! with linear path constraint and no orientation constraints\") but may have different data for\n  //! each constraint.\n  struct CUMO_EXPORT TaskSpaceTargetGoalset {\n    //! Create a task-space target goalset.\n    //!\n    //! A fatal error will be logged if:\n    //!   1. The number of `translation_constraints` does not match the number of\n    //!      `orientation_constraints`.\n    explicit TaskSpaceTargetGoalset(const TranslationConstraintGoalset &translation_constraints,\n                                    const OrientationConstraintGoalset &orientation_constraints =\n                                        OrientationConstraintGoalset::None());\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! C-space targets fully restrict the c-space configuration at the termination of the trajectory\n  //! while allowing (optional) task-space constraints along the path.\n  class CUMO_EXPORT CSpaceTarget {\n   public:\n    //! Translation path constraints restrict the position of the origin of a tool frame along the\n    //! path.\n    class CUMO_EXPORT TranslationPathConstraint {\n     public:\n      //! Create a `TranslationPathConstraint` such that the position of the tool frame is not\n      //! restricted along the path.\n      static TranslationPathConstraint None();\n\n      //! Create a `TranslationPathConstraint` such that a linear translation constraint is active\n      //! along the path.\n      //!\n      //! The linear path constraint is defined by the line **segment** between the origin of the\n      //! tool frame at the initial c-space position and the origin of the tool frame at the\n      //! terminal c-space target. This path constraint is satisfied if the origin of the tool frame\n      //! is on this line segment between the initial and final tool positions at all points in the\n      //! trajectory. It is considered a constraint violation if the origin of the tool frame\n      //! \"overshoots\" either the initial or final tool positions.\n      //!\n      //! If the distance between the initial and final tool frames are nearly the same, the path\n      //! constraint becomes a constraint on the distance from the initial and final positions in\n      //! any direction.\n      //!\n      //! The optional parameter `path_deviation_limit` can be used to allow deviation from the\n      //! nominal task-space line along the path. This limit specifies the maximum allowable\n      //! deviation from the desired linear path.\n      //!\n      //! If `path_deviation_limit` is not input, then the deviation limit is assumed to be zero\n      //! (i.e., it is desired that translation is always exactly coincident with the specified\n      //! line).\n      //!\n      //! A fatal error will be logged if:\n      //!   1. `path_deviation_limit` is negative.\n      static TranslationPathConstraint Linear(const double *path_deviation_limit = nullptr);\n\n      struct Impl;\n      std::shared_ptr<Impl> impl;\n    };\n\n    //! Orientation path constraints restrict the orientation of a tool frame along the path.\n    class CUMO_EXPORT OrientationPathConstraint {\n     public:\n      //! Create a `OrientationPathConstraint` such that the orientation of the tool frame is not\n      //! restricted along the path.\n      static OrientationPathConstraint None();\n\n      //! Create an `OrientationPathConstraint` such that the tool frame orientation along the path\n      //! is constant.\n      //!\n      //! If `use_terminal_orientation` is set to `true`, then the tool frame orientation\n      //! corresponding to the terminal c-space target will be used to define the orientation\n      //! target along the path. Otherwise, the tool frame orientation corresponding to the initial\n      //! c-space position will be used.\n      //!\n      //! The optional parameter `path_deviation_limit` can be used to allow deviations from this\n      //! constant orientation along the path. If input, the units are radians. These limits\n      //! specify the maximum allowable deviation from the desired orientation.\n      //!\n      //! If the `path_deviation_limit` is *NOT* input, its value is set to the most restrictive\n      //! feasible value. This feasible limit is determined by the angular distance between the tool\n      //! frame orientations at the initial and terminal c-space positions. If the\n      //! `path_deviation_limit` is input, it must be greater than or equal to this angular\n      //! distance.\n      //!\n      //! In general, it is suggested that angular deviation limits are set to values less than pi.\n      //! A limit that is near or greater than pi essentially disables the constraint (without\n      //! culling the computation). Non-fatal warnings will be logged if:\n      //!   - `path_deviation_limit` is near or greater than pi.\n      //!\n      //! A fatal error will be logged if:\n      //!   1. `path_deviation_limit` is negative, *OR*\n      //!   2. `path_deviation_limit` is infeasible for the given initial c-space position and\n      //!      terminal c-space target.\n      //!\n      //! NOTE: Condition [3] will only be validated when the resulting `OrientationPathConstraint`\n      //!       is used for trajectory optimization (i.e., it is used as input to\n      //!       `planToCSpaceTarget()`).\n      static OrientationPathConstraint Constant(const double *path_deviation_limit = nullptr,\n                                                bool use_terminal_orientation = true);\n\n      //! Create an `OrientationPathConstraint` such that the tool frame orientation is constrained\n      //! to rotate about a \"free axis\" along the path.\n      //!\n      //! The \"free axis\" for rotation is defined by a `tool_frame_axis` (specified in the tool\n      //! frame coordinates) and a corresponding `world_target_axis` (specified in world frame\n      //! coordinates).\n      //!\n      //! The optional parameter `path_axis_deviation_limit` can be used to allow deviation from\n      //! the desired axis alignment along the path. If input, the units are radians.\n      //!\n      //! If the `path_axis_deviation_limit` is *NOT* input, its value is set to the most\n      //! restrictive feasible value. This feasibility is determined by the tool frame orientations\n      //! at the initial and terminal c-space positions. For each orientation, the angular distance\n      //! (if any) from satisfying the constraint will be computed. The `path_axis_deviation_limit`\n      //! will be set to the maximum of these angular distances. If the `path_axis_deviation_limit`\n      //! is input, it must be greater than or equal to this minimum feasible deviation limit.\n      //!\n      //! In general, it is suggested that angular deviation limits are set to values less than pi.\n      //! A limit that is near or greater than pi essentially disables the constraint (without\n      //! culling the computation). Non-fatal warnings will be logged if:\n      //!   - `path_axis_deviation_limit` near or greater than pi.\n      //!\n      //! A fatal error will be logged if:\n      //!   1. `path_axis_deviation_limit` is negative,\n      //!   2. `path_axis_deviation_limit` is infeasible for the given initial c-space position and\n      //!      terminal c-space target,\n      //!   3. `tool_frame_axis` is (nearly) zero, *OR*\n      //!   4. `world_target_axis` is (nearly) zero.\n      //!\n      //! NOTE: Condition [3] will only be validated when the resulting `OrientationPathConstraint`\n      //!       is used for trajectory optimization (i.e., it is used as input to\n      //!       `planToCSpaceTarget()`).\n      //!\n      //! NOTE: `tool_frame_axis` and `world_target_axis` inputs will be normalized.\n      static OrientationPathConstraint Axis(const Eigen::Vector3d &tool_frame_axis,\n                                            const Eigen::Vector3d &world_target_axis,\n                                            const double *path_axis_deviation_limit = nullptr);\n\n      struct Impl;\n      std::shared_ptr<Impl> impl;\n    };\n\n    //! Create a c-space target.\n    explicit CSpaceTarget(const Eigen::VectorXd &cspace_position_terminal_target,\n                          const TranslationPathConstraint &translation_path_constraint =\n                              TranslationPathConstraint::None(),\n                          const OrientationPathConstraint &orientation_path_constraint =\n                              OrientationPathConstraint::None());\n\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  //! Results from a trajectory optimization.\n  class CUMO_EXPORT Results {\n   public:\n    //! Indicate the success or failure of the trajectory optimization.\n    enum class Status {\n      //! Valid trajectory found.\n      SUCCESS,\n\n      //! Invalid initial c-space position.\n      //!\n      //! NOTE: The `RobotWorldInspector` can be used to determine if this invalid state is\n      //!       due to world-collision, self-collision, c-space position limit violations, etc.\n      INVALID_INITIAL_CSPACE_POSITION,\n\n      //! Invalid target c-space position.\n      //!\n      //! Only applicable when planning to c-space targets.\n      //!\n      //! NOTE: The `RobotWorldInspector` can be used to determine if this invalid state is\n      //!       due to world-collision, self-collision, c-space position limit violations, etc.\n      INVALID_TARGET_CSPACE_POSITION,\n\n      //! The c-space or task-space target specification is invalid.\n      INVALID_TARGET_SPECIFICATION,\n\n      //! The inverse kinematics solver failed to find a valid solution.\n      INVERSE_KINEMATICS_FAILURE,\n\n      //! Initial trajectory optimization attempts failed and geometric planning was attempted as\n      //! a fallback, but this geometric planning routine also failed.\n      GEOMETRIC_PLANNING_FAILURE,\n\n      //! Initial trajectory optimization attempts failed and geometric planning was attempted as\n      //! a fallback (and was successful), but secondary trajectory optimization using the\n      //! geometrically planned path as a warm start failed to produce a valid trajectory.\n      TRAJECTORY_OPTIMIZATION_FAILURE\n    };\n\n    virtual ~Results() = default;\n\n    //! Return the `Status` from the trajectory optimization.\n    [[nodiscard]] virtual Status status() const = 0;\n\n    //! If `status()` returns `SUCCESS`, then `trajectory()` returns a valid `Trajectory`.\n    //!\n    //! If `status()` returns `INVALID_INITIAL_CSPACE_POSITION`,\n    //! `INVALID_TARGET_CSPACE_POSITION`, `INVALID_TARGET_SPECIFICATION`, or\n    //! `INVERSE_KINEMATICS_FAILURE`, then `nullptr` will be returned.\n    //!\n    //! If `status()` returns `GEOMETRIC_PLANNING_FAILURE` or `TRAJECTORY_OPTIMIZATION_FAILURE`,\n    //! then the lowest-cost (but *invalid*) `Trajectory` will be returned.\n    [[nodiscard]] virtual std::unique_ptr<Trajectory> trajectory() const = 0;\n\n    //! Return the index of the target selected for the terminal knot point.\n    //!\n    //! For goalset planning (e.g., `planToTaskSpaceGoalset()`) this represents an index into the\n    //! constraint vectors used to create the goalset (e.g., `TaskSpaceTargetGoalset`).\n    //!\n    //! For single target planning (e.g., `planToTaskSpaceTarget()` and `planToCSpaceTarget()`),\n    //! zero will be returned for successful trajectory optimization.\n    //!\n    //! In all cases, -1 will be returned if trajectory optimization is unsuccessful.\n    [[nodiscard]] virtual int targetIndex() const = 0;\n  };\n\n  //! Attempt to find a trajectory from `initial_cspace_position` to `task_space_target`.\n  [[nodiscard]] virtual std::unique_ptr<Results> planToTaskSpaceTarget(\n      const Eigen::VectorXd &initial_cspace_position,\n      const TaskSpaceTarget &task_space_target) const = 0;\n\n  //! Attempt to find a trajectory from `initial_cspace_position` to `task_space_target_goalset`.\n  [[nodiscard]] virtual std::unique_ptr<Results> planToTaskSpaceGoalset(\n      const Eigen::VectorXd &initial_cspace_position,\n      const TaskSpaceTargetGoalset &task_space_target_goalset) const = 0;\n\n  //! Attempt to find a trajectory from `initial_cspace_position` to `cspace_target`.\n  [[nodiscard]] virtual std::unique_ptr<Results> planToCSpaceTarget(\n      const Eigen::VectorXd &initial_cspace_position,\n      const CSpaceTarget &cspace_target) const = 0;\n};\n\n//! Create a `TrajectoryOptimizer` with the given `config`.\nCUMO_EXPORT std::unique_ptr<TrajectoryOptimizer> CreateTrajectoryOptimizer(\n    const TrajectoryOptimizerConfig &config);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/version.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2022-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#define CUMOTION_VERSION_MAJOR 1\n#define CUMOTION_VERSION_MINOR 1\n#define CUMOTION_VERSION_PATCH 0\n#define CUMOTION_VERSION_SUFFIX \"\"\n\n#define CUMOTION_VERSION_STRING \"1.1.0\"\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/vision.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2024-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for handling depth camera data\n//!\n//! @note This interface is experimental and may evolve in a future release.  It also lacks\n//!       a corresponding Python API.\n\n#pragma once\n\n#include <cstdint>\n#include <memory>\n#include <optional>\n#include <type_traits>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n\nnamespace cumotion {\n\n//! Camera intrinsic parameters.\n//!\n//! The intrinsics define the mapping from 3D camera coordinates to 2D image coordinates. The\n//! standard pinhole camera model uses four parameters (`fx`, `fy`, `cx`, `cy`) that form the\n//! intrinsic matrix:\n//!\n//! @code\n//!   [fx   0  cx]\n//!   [ 0  fy  cy]\n//!   [ 0   0   1]\n//! @endcode\n//!\n//! where `fx` and `fy` are the focal lengths in pixels, and `cx` and `cy` are the principal point\n//! coordinates (optical center) in pixels.\nclass CUMO_EXPORT CameraIntrinsics {\n public:\n  //! Construct from the four standard pinhole camera parameters.\n  //!\n  //! The parameters `fx` and `fy` represent the focal lengths in the x and y directions\n  //! (in pixels), while `cx` and `cy` represent the principal point (optical center) coordinates.\n  CameraIntrinsics(double fx, double fy, double cx, double cy);\n\n  //! Construct from a full 3x3 intrinsic matrix.\n  explicit CameraIntrinsics(const Eigen::Matrix3d &matrix);\n\n  //! Return the focal length in the x direction (in pixels).\n  [[nodiscard]] double fx() const;\n\n  //! Return the focal length in the y direction (in pixels).\n  [[nodiscard]] double fy() const;\n\n  //! Return the x coordinate of the principal point (optical center) in pixels.\n  [[nodiscard]] double cx() const;\n\n  //! Return the y coordinate of the principal point (optical center) in pixels.\n  [[nodiscard]] double cy() const;\n\n  //! Return the full 3x3 intrinsic matrix.\n  [[nodiscard]] Eigen::Matrix3d matrix() const;\n\n  //! Return whether the intrinsic matrix has the standard pinhole camera structure.\n  //!\n  //! Returns `true` if the matrix follows the canonical pinhole format with zero skew, zeros in the\n  //! lower triangular entries, and 1 in the bottom-right corner. This allows for optimized\n  //! operations that only use the four key parameters (`fx`, `fy`, `cx`, `cy`).\n  [[nodiscard]] bool isPinhole() const;\n\n  struct Impl;\n  std::shared_ptr<Impl> impl;\n};\n\n//! Memory residency for buffer data.\nenum class BufferResidency {\n  //! Host memory allocated via `malloc()`, `new`, `cudaMallocHost()`, etc.\n  HOST,\n\n  //! Device memory allocated via `cudaMalloc()` or similar CUDA device allocators.\n  DEVICE,\n\n  //! CUDA unified/managed memory allocated via `cudaMallocManaged()`.\n  //!\n  //! This memory is accessible from both host and device, with migration handled as needed by the\n  //! CUDA runtime.\n  MANAGED\n};\n\n//! Type-agnostic base class for depth images and (non-owning) views of depth images.\n//!\n//! See `DepthImage` for member function documentation.\nclass CUMO_EXPORT DepthImageBase {\n public:\n  //! Scalar type used to represent the depth values.\n  enum class ScalarType {\n    FLOAT,  //!< 32-bit floating point (corresponds to `DepthImage<float>`)\n    UINT16  //!< 16-bit unsigned integer (corresponds to `DepthImage<uint16_t>`)\n  };\n\n  virtual ~DepthImageBase() = default;\n\n  [[nodiscard]] virtual int width() const = 0;\n  [[nodiscard]] virtual int height() const = 0;\n  [[nodiscard]] virtual int stride() const = 0;\n  [[nodiscard]] virtual BufferResidency residency() const = 0;\n  [[nodiscard]] virtual double metersPerUnit() const = 0;\n  [[nodiscard]] virtual bool isView() const = 0;\n  [[nodiscard]] virtual ScalarType scalarType() const = 0;\n  [[nodiscard]] virtual bool isConst() const = 0;\n  [[nodiscard]] virtual double depthInMeters(int x, int y) const = 0;\n};\n\n//! A depth image or non-owning view of a depth image buffer.\n//!\n//! The data buffer is assumed to be contiguous with pixels in the conventional row-major order\n//! (with the x coordinate varying fastest) and with the origin at the top-left corner.\n//!\n//! Supported types are `float` and `uint16_t`.  The latter is often used for raw depth data where\n//! distance is measured in millimeters.  Const-qualified variants are also supported.\n//!\n//! The `stride()` will always be equal to or greater than `width()`.  The pixel at coordinates\n//! (x, y) is located at index `y * stride() + x` in the buffer, where `x` is the column index and\n//! `y` is the row index.\ntemplate<typename T = float>\nclass DepthImage : public DepthImageBase {\n  using BaseT = std::remove_const_t<T>;\n  static_assert(std::is_same_v<BaseT, float> || std::is_same_v<BaseT, uint16_t>,\n                \"'DepthImage' supports 'float' and 'uint16_t' types.\");\n\n public:\n  //! Return the default `meters_per_unit` value for the scalar type `T`:\n  //!\n  //! - `0.001` for `uint16_t`, corresponding to units of millimeters.\n  //! - `1.0` for `float`, corresponding to units of meters.\n  [[nodiscard]] static constexpr double DefaultMetersPerUnit() {\n    return std::is_same_v<const T, const uint16_t> ? 0.001 : 1.0;\n  }\n\n  //! Construct a `DepthImage` that owns its own buffer.\n  //!\n  //! This constructor is only available for non-const scalar types (`float` or `uint16_t`).\n  //!\n  //! The optional `stride` parameter specifies the number of elements between consecutive rows. If\n  //! not provided, the stride defaults to `width` (tightly-packed layout). The `residency`\n  //! parameter indicates where to allocate the buffer, and the `meters_per_unit` parameter\n  //! specifies the scaling factor to convert depth values to meters.\n  //!\n  //! A fatal error will be logged if:\n  //!\n  //! 1. `width` or `height` is zero or negative, or\n  //! 2. `stride` is smaller than `width`.\n  template<typename U = T, typename = std::enable_if_t<!std::is_const_v<U>>>\n  DepthImage(int width, int height,\n             std::optional<int> stride = std::nullopt,\n             BufferResidency residency = BufferResidency::HOST,\n             double meters_per_unit = DefaultMetersPerUnit());\n\n  ~DepthImage() override = default;\n\n  DepthImage(DepthImage&&) noexcept = default;\n  DepthImage& operator=(DepthImage&&) noexcept = default;\n\n  // Copying is disallowed.\n  DepthImage(const DepthImage&) = delete;\n  DepthImage& operator=(const DepthImage&) = delete;\n\n  //! Return pointer to the underlying depth data buffer.\n  [[nodiscard]] T *data();\n\n  //! Return const pointer to the underlying depth data buffer.\n  [[nodiscard]] const T *data() const;\n\n  //! Return the width of the depth image in pixels.\n  [[nodiscard]] int width() const override;\n\n  //! Return the height of the depth image in pixels.\n  [[nodiscard]] int height() const override;\n\n  //! Return the stride of the depth image in elements.\n  //!\n  //! The stride represents the number of elements between consecutive rows in the buffer. For\n  //! tightly-packed images, stride equals width. For padded or aligned images, stride may be\n  //! larger than width.\n  [[nodiscard]] int stride() const override;\n\n  //! Return the memory residency of the depth image buffer.\n  [[nodiscard]] BufferResidency residency() const override;\n\n  //! Return the scaling factor to convert depth values to meters.\n  //!\n  //! This value represents the number of meters per depth unit. Common values include 1.0 for\n  //! depth already in meters, 0.001 for millimeters, and 0.0001 for tenths of millimeters.\n  [[nodiscard]] double metersPerUnit() const override;\n\n  //! Return whether this is a (non-owning) view or a `DepthImage` that owns its memory.\n  //!\n  //! Views do not manage the lifetime of the underlying buffer. Owning instances allocate and\n  //! deallocate the buffer automatically.\n  [[nodiscard]] bool isView() const override;\n\n  //! Return the scalar type of the depth image data (needed for `DepthImageBase` interface).\n  [[nodiscard]] ScalarType scalarType() const override {\n    using BaseT = std::remove_const_t<T>;\n    return std::is_same_v<BaseT, float> ? ScalarType::FLOAT : ScalarType::UINT16;\n  }\n\n  //! Return whether the depth image data is const-qualified (needed for `DepthImageBase`\n  //! interface).\n  [[nodiscard]] bool isConst() const override {\n    return std::is_const_v<T>;\n  }\n\n  //! Return the depth value at pixel coordinates (x, y).\n  //!\n  //! The returned value is the raw depth measurement without unit conversion. The `x` coordinate\n  //! corresponds to the column index, and `y` corresponds to the row index, with the origin at\n  //! the top-left corner.\n  //!\n  //! @warning This accessor does not perform bounds checking and does not check that the data is\n  //!          resident on the host.\n  [[nodiscard]] T at(int x, int y) const;\n\n  //! Return a reference to the depth value at pixel coordinates (x, y).\n  //!\n  //! This function allows modification of the depth value and is only available for non-const\n  //! template instantiations. The `x` coordinate corresponds to the column index, and `y`\n  //! corresponds to the row index, with the origin at the top-left corner.\n  //!\n  //! @warning This accessor does not perform bounds checking and does not check that the data is\n  //!          resident on the host.\n  [[nodiscard]] T &at(int x, int y);\n\n  //! Return the depth value at pixel coordinates (`x`, `y`) converted to meters.\n  //!\n  //! This method returns the depth value scaled by `metersPerUnit()`. The `x` coordinate\n  //! corresponds to the column index, and `y` corresponds to the row index, with the origin at\n  //! the top-left corner.\n  //!\n  //! A fatal error will be logged if the data is not resident on the host or if either `x` or `y`\n  //! is out of bounds.\n  [[nodiscard]] double depthInMeters(int x, int y) const override;\n\n  struct Impl;\n\n  //! Constructor used by the internal implementation.\n  explicit DepthImage(std::shared_ptr<Impl> impl);\n\n private:\n  std::shared_ptr<Impl> impl_;\n};\n\nextern template class CUMO_EXTERN_TEMPLATE_EXPORT DepthImage<float>;\nextern template class CUMO_EXTERN_TEMPLATE_EXPORT DepthImage<const float>;\nextern template class CUMO_EXTERN_TEMPLATE_EXPORT DepthImage<uint16_t>;\nextern template class CUMO_EXTERN_TEMPLATE_EXPORT DepthImage<const uint16_t>;\n\n//! Create a non-owning `DepthImage` view from a depth data buffer.\n//!\n//! The template parameter `T` can be `float`, `uint16_t`, `const float`, or `const uint16_t`.\n//!\n//! The `data` parameter points to a contiguous buffer containing depth values arranged in\n//! row-major order. The buffer must remain valid for the lifetime of the returned `DepthImage`\n//! object, as the view does not take ownership of the memory.\n//!\n//! The optional `stride` parameter specifies the number of elements between consecutive rows. If\n//! not provided, the stride defaults to `width` (tightly-packed layout). The `residency`\n//! parameter indicates where the buffer is allocated, and the `meters_per_unit` parameter\n//! specifies the scaling factor to convert depth values to meters.\n//!\n//! A fatal error will be logged if:\n//!\n//! 1. `data` is null,\n//! 2. `width` or `height` is zero or negative, or\n//! 3. `stride` is smaller than `width`.\ntemplate<typename T>\nCUMO_EXPORT DepthImage<T> CreateDepthImageView(\n  T *data,\n  int width,\n  int height,\n  std::optional<int> stride = std::nullopt,\n  BufferResidency residency = BufferResidency::HOST,\n  double meters_per_unit = DepthImage<T>::DefaultMetersPerUnit());\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/world.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n#pragma once\n\n#include <memory>\n#include <optional>\n#include <vector>\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/obstacle.h\"\n#include \"cumotion/pose3.h\"\n\nnamespace cumotion {\n\n//! Forward declaration of `WorldViewHandle` for use by `World::addWorldView()`.\nstruct WorldViewHandle;\n\n//! World represents a collection of obstacles.\nclass CUMO_EXPORT World {\n public:\n  //! Opaque handle to an obstacle.\n  struct CUMO_EXPORT ObstacleHandle {\n    struct Impl;\n    std::shared_ptr<Impl> impl;\n  };\n\n  virtual ~World() = default;\n\n  //! Add `obstacle` to the world.\n  //!\n  //! All attributes of obstacle are copied to world and subsequent changes to `obstacle` will\n  //! not be reflected in the world.\n  //!\n  //! If a `pose` is not provided for the `obstacle`, `Pose3::Identity()` will be used.\n  //!\n  //! Obstacles are automatically enabled when added.\n  virtual ObstacleHandle addObstacle(const Obstacle &obstacle,\n                                     std::optional<Pose3> pose = std::nullopt) = 0;\n\n  //! Permanently remove obstacle, invalidating its handle.\n  virtual void removeObstacle(const ObstacleHandle &obstacle) = 0;\n\n  //! Enable an obstacle for the purpose of collision checks and distance evaluations.\n  virtual void enableObstacle(const ObstacleHandle &obstacle) = 0;\n\n  //! Disable an obstacle for the purpose of collision checks and distance evaluations.\n  virtual void disableObstacle(const ObstacleHandle &obstacle) = 0;\n\n  //! Set the pose of the given obstacle.\n  virtual void setPose(const ObstacleHandle &obstacle, const Pose3 &pose) = 0;\n\n  //! Set the grid values for an obstacle of type SDF using a host-resident `values` buffer.\n  //!\n  //! It is assumed that `values` is stored with the `z` index varying fastest and has dimensions\n  //! given by the `Obstacle::Grid` associated with `obstacle`.  For example, for an obstacle with\n  //! `Grid` parameters `num_voxels_x`, `num_voxels_y`, and `num_voxels_z`, the length of `values`\n  //! should be `num_voxels_x * num_voxels_y * num_voxels_z`, and in the provided coordinates,\n  //! adjacent elements in memory should correspond to voxels with adjacent Z coordinates, and\n  //! voxels with adjacent X coordinates should be separated by `num_voxels_y * num_voxels_z`\n  //! elements.\n  //!\n  //! `precision` specifies the floating-point type of `values`.  `Obstacle::Grid::Precision::HALF`\n  //! corresponds to the `__half` data type defined in the `cuda_fp16.h` header.\n  //!\n  //! If the type of `obstacle` is not `Obstacle::Type::SDF`, a fatal error will be logged.\n  virtual void setSdfGridValuesFromHost(const ObstacleHandle &obstacle,\n                                        const void *values,\n                                        Obstacle::Grid::Precision grid_precision) = 0;\n\n  //! Set the grid values for an obstacle of type SDF using a device-resident buffer `values`.\n  //!\n  //! It is assumed that `values` is stored with the `z` index varying fastest and has dimensions\n  //! given by the `Obstacle::Grid` associated with `obstacle`.  For example, for an obstacle with\n  //! `Grid` parameters `num_voxels_x`, `num_voxels_y`, and `num_voxels_z`, the length of `values`\n  //! should be `num_voxels_x * num_voxels_y * num_voxels_z`, and in the provided coordinates,\n  //! adjacent elements in memory should correspond to voxels with adjacent Z coordinates, and\n  //! voxels with adjacent X coordinates should be separated by `num_voxels_y * num_voxels_z`\n  //! elements.\n  //!\n  //! `precision` specifies the floating-point type of `values`.  `Obstacle::Grid::Precision::HALF`\n  //! corresponds to the `__half` data type defined in the `cuda_fp16.h` header.\n  //!\n  //! If the type of `obstacle` is not `Obstacle::Type::SDF`, a fatal error will be logged.\n  virtual void setSdfGridValuesFromDevice(const ObstacleHandle &obstacle,\n                                          const void *values,\n                                          Obstacle::Grid::Precision grid_precision) = 0;\n\n  //! Tolerances used when inspecting an SDF.\n  //!\n  //! Accumulation of numerical error from type-casting is handled independently of the tolerances\n  //! in `SdfInspectionTolerances`.  It is expected that any SDF computed from an analytically\n  //! correct distance function will pass `InspectCudaSdfAndSync()` with the default tolerances of\n  //! `0.0f`.\n  //!\n  //! Inspection tolerances are specified in single-precision (`float`) because only single- and\n  //! half- precision data for device-resident SDFs are supported.\n  struct CUMO_EXPORT SdfInspectionTolerances {\n    //! Numerical tolerance on checking whether an SDF voxel value is equal to its neighbor.\n    //!\n    //! The effective tolerance used is the combination of this tolerance and an internally computed\n    //! `numerical_tolerance`.  `numerical_tolerance` is a derived upper bound on accumulated error\n    //! under the assumption that SDF data was computed analytically with infinite precision, then\n    //! cast to the `SDF` data-type specified by `Obstacle::Grid::device_precision`.\n    //!\n    //! A voxel is said to be equal to its neighbor if:\n    //! `abs(voxel_value - neighbor_value) <= voxel_matches_neighbor_tolerance +\n    //!                                       numerical_tolerance`.\n    float voxel_matches_neighbor_tolerance = 0.0f;\n\n    //! Numerical tolerance on checking whether an SDF voxel value is too far from a neighboring\n    //! voxel.\n    //!\n    //! The effective tolerance used is the combination of this tolerance and an internally computed\n    //! `numerical_tolerance`.  `numerical_tolerance` is a derived upper bound on accumulated error\n    //! under the assumption that SDF data was computed analytically with infinite precision, then\n    //! cast to the `SDF` data-type specified by `Obstacle::Grid::device_precision`.\n    //!\n    //! A voxel is said to be too far from its neighbor if:\n    //! `abs(voxel_value - neighbor_value) > voxel_too_far_from_neighbor_tolerance +\n    //!                                      numerical_tolerance`.\n    float voxel_too_far_from_neighbor_tolerance = 0.0f;\n  };\n\n  //! Inspection results associated with an SDF.\n  //!\n  //! When `numErrors() == 0`, this does *NOT* guarantee that the SDF is globally valid.  It only\n  //! shows that none of the specific error cases covered by the inspector were present.\n  struct CUMO_EXPORT SdfInspectionResults {\n    //! This count will be set to the number of voxels whose values match the values of `6`\n    //! neighboring voxels within the specified `voxel_matches_neighbor_tolerance`.  This can only\n    //! be triggered by non-boundary voxels, as boundary voxels do not have `6` neighbors.\n    //!\n    //! There is no geometrically consistent way this can happen in a valid distance field without\n    //! voxel-scale obstacles.\n    int num_voxels_matching_all_neighbors = 0;\n\n    //! This count will be set to the number of voxels whose distance to a neighboring voxel is\n    //! greater than the `voxel_size` parameter of the SDF inflated by the specified\n    //! `voxel_too_far_from_neighbor_tolerance`.\n    //!\n    //! There is no geometrically consistent way this can happen in a valid distance field.\n    int num_voxels_too_far_from_neighbors = 0;\n\n    //! Returns the sum of all error conditions in `SdfInspectionResults`.\n    [[nodiscard]] int numErrors() const {\n      return num_voxels_matching_all_neighbors + num_voxels_too_far_from_neighbors;\n    }\n  };\n\n  //! Inspect the data for an obstacle of type `SDF`.\n  //!\n  //! The returned `SdfInspectionResults` reports error-cases in the SDF data that are likely to\n  //! cause issues such as undetected collision with the environment.\n  //!\n  //! The default `inspection_tolerances` are recommended, and only need to be replaced if there is\n  //! known systematic error in the generation of SDF data within known bounds.\n  //!\n  //! A fatal error will be logged if:\n  //!  - `obstacle` does not have type `SDF`.\n  //!  - `obstacle` has not been populated with data.\n  //!  - `obstacle` has been removed from the world.\n  [[nodiscard]] virtual SdfInspectionResults inspectSdf(\n      const ObstacleHandle &obstacle,\n      std::optional<SdfInspectionTolerances> inspection_tolerances = std::nullopt) const = 0;\n\n  //! Create a view into the world that can be used for collision checks and distance evaluations.\n  //!\n  //! Each world view will maintain a static view of the world until it is updated. When a\n  //! world view is updated, it will reflect any changes to the world since its last update.\n  virtual WorldViewHandle addWorldView() = 0;\n};\n\n//! Create an empty world with no obstacles.\nCUMO_EXPORT std::shared_ptr<World> CreateWorld();\n\n//! A handle to a view of a `cumotion::World`.\n//!\n//! This view can be independently updated to track updates made to a `cumotion::World` object.\n//! A `WorldViewHandle` may be copied, with all copies sharing the same underlying view.\n//!\n//! To query spatial relationships in a world view, use `cumotion::WorldInspector`.\nstruct CUMO_EXPORT WorldViewHandle {\n  //! Update world view such that any changes to the underlying world are reflected in this view.\n  void update();\n\n  struct Impl;\n  std::shared_ptr<Impl> impl;\n};\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/include/cumotion/world_inspector.h",
    "content": "// SPDX-FileCopyrightText: Copyright (c) 2021-2026 NVIDIA CORPORATION & AFFILIATES.\n//                         All rights reserved.\n// SPDX-License-Identifier: LicenseRef-NvidiaProprietary\n//\n// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual\n// property and proprietary rights in and to this material, related\n// documentation and any modifications thereto. Any use, reproduction,\n// disclosure or distribution of this material and related documentation\n// without an express license agreement from NVIDIA CORPORATION or\n// its affiliates is strictly prohibited.\n\n//! @file\n//! @brief Public interface for querying spatial relationships in a world.\n\n#pragma once\n\n#include <memory>\n#include <vector>\n\n#include \"Eigen/Core\"\n\n#include \"cumotion/cumotion_export.h\"\n#include \"cumotion/pose3.h\"\n#include \"cumotion/world.h\"\n\nnamespace cumotion {\n\n//! Interface for querying properties and spatial relationships within a world.\nclass CUMO_EXPORT WorldInspector {\n public:\n  virtual ~WorldInspector() = default;\n\n  //! Return whether the obstacle specified by `obstacle` is enabled in the current view of the\n  //! world.\n  [[nodiscard]] virtual bool isEnabled(const World::ObstacleHandle &obstacle) const = 0;\n\n  //! Return the pose of the obstacle specified by `obstacle`.\n  [[nodiscard]] virtual Pose3 pose(const World::ObstacleHandle &obstacle) const = 0;\n\n  //! Test whether a sphere defined by `center` and `radius` is in collision with ANY enabled\n  //! obstacle in the world.\n  [[nodiscard]] virtual bool inCollision(const Eigen::Vector3d &center, double radius) const = 0;\n\n  //! Test whether a sphere defined by `center` and `radius` is in collision with the obstacle\n  //! specified by `obstacle`.\n  [[nodiscard]] virtual bool inCollision(const World::ObstacleHandle &obstacle,\n                                         const Eigen::Vector3d &center,\n                                         double radius) const = 0;\n\n  //! Compute the distance from `point` to the obstacle specified by `obstacle`.\n  //!\n  //! Returns distance between the `point` and `obstacle`. If the `point` intersects `obstacle`, a\n  //! negative distance is returned.  The distance gradient is written to `gradient` if provided.\n  virtual double distanceTo(const World::ObstacleHandle &obstacle,\n                            const Eigen::Vector3d &point,\n                            Eigen::Vector3d *gradient = nullptr) const = 0;\n\n  //! Compute distances from `point` to all enabled obstacles.\n  //!\n  //! Distances between the `point` and each enabled obstacle are written to `distances`.\n  //! If the `point` intersects an obstacle, the resulting distance will be negative. The distance\n  //! gradients are written to `distance_gradients` if provided.\n  //!\n  //! The number of `distances` and/or `distance_gradients` that are written (i.e., the number of\n  //! enabled obstacles) is returned.\n  //!\n  //! If the length of `distances` is less than the number of enabled obstacles, it will be resized\n  //! to the number of enabled obstacles. The same applies to `distance_gradients` if provided. If\n  //! the vectors lengths are larger than the number of enabled obstacles, the vectors will NOT be\n  //! resized. Only the first N elements will be written to, where N is the number of enabled\n  //! obstacles. The values of extra elements at the end of the input vectors will NOT be changed.\n  virtual int distancesTo(const Eigen::Vector3d &point,\n                          std::vector<double> &distances,\n                          std::vector<Eigen::Vector3d> *distance_gradients = nullptr) const = 0;\n\n  //! Compute distances from `point` to all enabled obstacles.\n  //!\n  //! Distances between the `point` and each enabled obstacle are written to `distances`, which must\n  //! not be `nullptr`.\n  //! If the `point` intersects an obstacle, the resulting distance will be negative. The distance\n  //! gradients are written to `distance_gradients` if provided.\n  //!\n  //! The number of `distances` and/or `distance_gradients` that are written (i.e., the number of\n  //! enabled obstacles) is returned.\n  //!\n  //! If the length of `distances` is less than the number of enabled obstacles, it will be resized\n  //! to the number of enabled obstacles. The same applies to `distance_gradients` if provided. If\n  //! the vectors lengths are larger than the number of enabled obstacles, the vectors will NOT be\n  //! resized. Only the first N elements will be written to, where N is the number of enabled\n  //! obstacles. The values of extra elements at the end of the input vectors will NOT be changed.\n  //!\n  //! A fatal error will be logged if `distances` is `nullptr`.\n  //!\n  //! DEPRECATED: This function overload is deprecated and will be removed in a future version. Use\n  //!             the reference overload (`WorldInspector::distancesTo(const Eigen::Vector3d &point,\n  //!             std::vector<double> &distances, std::vector<Eigen::Vector3d> *distance_gradients =\n  //!             nullptr) const`) instead.\n  virtual CUMO_DEPRECATED int distancesTo(\n      const Eigen::Vector3d &point,\n      std::vector<double> *distances,\n      std::vector<Eigen::Vector3d> *distance_gradients = nullptr) const = 0;\n\n  //! Compute the minimum distance from `point` to ANY enabled obstacle in the current\n  //! view of the world.\n  //!\n  //! Returns distance between the `point` and the nearest obstacle. If the `point` is inside an\n  //! obstacle, a negative distance is returned. The distance gradient is written to `gradient` if\n  //! provided.\n  virtual double minDistance(const Eigen::Vector3d &point,\n                             Eigen::Vector3d *gradient = nullptr) const = 0;\n\n  //! Return the number of enabled obstacles in the current view of the world.\n  [[nodiscard]] virtual int numEnabledObstacles() const = 0;\n};\n\n//! Create a `WorldInspector` for a given `world_view`.\nCUMO_EXPORT std::unique_ptr<WorldInspector> CreateWorldInspector(const WorldViewHandle &world_view);\n\n}  // namespace cumotion\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/lib/cmake/cumotion/cumotionConfig.cmake",
    "content": "# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES.\n#                         All rights reserved.\n# SPDX-License-Identifier: Apache-2.0\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n\n\n####### Expanded from @PACKAGE_INIT@ by configure_package_config_file() #######\n####### Any changes to this file will be overwritten by the next CMake run ####\n####### The input file was CMakeConfig.cmake.in                            ########\n\nget_filename_component(PACKAGE_PREFIX_DIR \"${CMAKE_CURRENT_LIST_DIR}/../../../\" ABSOLUTE)\n\n####################################################################################\n\ninclude(CMakeFindDependencyMacro)\nfind_dependency(Eigen3 3.3)\nfind_dependency(CUDAToolkit)\n\nset(CUDA_PATH ${CUDAToolkit_LIBRARY_ROOT})\nif (${CUDAToolkit_VERSION} VERSION_GREATER_EQUAL 13.0)\n  if(${CMAKE_SYSTEM_PROCESSOR} MATCHES \"aarch64\")\n    set(CCCL_DIR \"${CUDA_PATH}/targets/sbsa-linux/lib/cmake/cccl\")  # for Jetpack 7.0\n  else()\n    set(CCCL_DIR \"${CUDA_PATH}/lib64/cmake/cccl\")\n  endif()\n  find_dependency(CCCL REQUIRED)\nendif()\n\ninclude(${CMAKE_CURRENT_LIST_DIR}/cumotionConfigVersion.cmake)\ninclude(${CMAKE_CURRENT_LIST_DIR}/cumotionTargets.cmake)\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/lib/cmake/cumotion/cumotionConfigVersion.cmake",
    "content": "# This is a basic version file for the Config-mode of find_package().\n# It is used by write_basic_package_version_file() as input file for configure_file()\n# to create a version-file which can be installed along a config.cmake file.\n#\n# The created file sets PACKAGE_VERSION_EXACT if the current version string and\n# the requested version string are exactly the same and it sets\n# PACKAGE_VERSION_COMPATIBLE if the current version is >= requested version,\n# but only if the requested major version is the same as the current one.\n# The variable CVF_VERSION must be set before calling configure_file().\n\n\nset(PACKAGE_VERSION \"1.1.0\")\n\nif(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION)\n  set(PACKAGE_VERSION_COMPATIBLE FALSE)\nelse()\n\n  if(\"1.1.0\" MATCHES \"^([0-9]+)\\\\.\")\n    set(CVF_VERSION_MAJOR \"${CMAKE_MATCH_1}\")\n    if(NOT CVF_VERSION_MAJOR VERSION_EQUAL 0)\n      string(REGEX REPLACE \"^0+\" \"\" CVF_VERSION_MAJOR \"${CVF_VERSION_MAJOR}\")\n    endif()\n  else()\n    set(CVF_VERSION_MAJOR \"1.1.0\")\n  endif()\n\n  if(PACKAGE_FIND_VERSION_RANGE)\n    # both endpoints of the range must have the expected major version\n    math (EXPR CVF_VERSION_MAJOR_NEXT \"${CVF_VERSION_MAJOR} + 1\")\n    if (NOT PACKAGE_FIND_VERSION_MIN_MAJOR STREQUAL CVF_VERSION_MAJOR\n        OR ((PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL \"INCLUDE\" AND NOT PACKAGE_FIND_VERSION_MAX_MAJOR STREQUAL CVF_VERSION_MAJOR)\n          OR (PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL \"EXCLUDE\" AND NOT PACKAGE_FIND_VERSION_MAX VERSION_LESS_EQUAL CVF_VERSION_MAJOR_NEXT)))\n      set(PACKAGE_VERSION_COMPATIBLE FALSE)\n    elseif(PACKAGE_FIND_VERSION_MIN_MAJOR STREQUAL CVF_VERSION_MAJOR\n        AND ((PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL \"INCLUDE\" AND PACKAGE_VERSION VERSION_LESS_EQUAL PACKAGE_FIND_VERSION_MAX)\n        OR (PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL \"EXCLUDE\" AND PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION_MAX)))\n      set(PACKAGE_VERSION_COMPATIBLE TRUE)\n    else()\n      set(PACKAGE_VERSION_COMPATIBLE FALSE)\n    endif()\n  else()\n    if(PACKAGE_FIND_VERSION_MAJOR STREQUAL CVF_VERSION_MAJOR)\n      set(PACKAGE_VERSION_COMPATIBLE TRUE)\n    else()\n      set(PACKAGE_VERSION_COMPATIBLE FALSE)\n    endif()\n\n    if(PACKAGE_FIND_VERSION STREQUAL PACKAGE_VERSION)\n      set(PACKAGE_VERSION_EXACT TRUE)\n    endif()\n  endif()\nendif()\n\n\n# if the installed project requested no architecture check, don't perform the check\nif(\"FALSE\")\n  return()\nendif()\n\n# if the installed or the using project don't have CMAKE_SIZEOF_VOID_P set, ignore it:\nif(\"${CMAKE_SIZEOF_VOID_P}\" STREQUAL \"\" OR \"8\" STREQUAL \"\")\n  return()\nendif()\n\n# check that the installed version has the same 32/64bit-ness as the one which is currently searching:\nif(NOT CMAKE_SIZEOF_VOID_P STREQUAL \"8\")\n  math(EXPR installedBits \"8 * 8\")\n  set(PACKAGE_VERSION \"${PACKAGE_VERSION} (${installedBits}bit)\")\n  set(PACKAGE_VERSION_UNSUITABLE TRUE)\nendif()\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/lib/cmake/cumotion/cumotionTargets-release.cmake",
    "content": "#----------------------------------------------------------------\n# Generated CMake target import file for configuration \"Release\".\n#----------------------------------------------------------------\n\n# Commands may need to know the format version.\nset(CMAKE_IMPORT_FILE_VERSION 1)\n\n# Import target \"cumotion::cumotion\" for configuration \"Release\"\nset_property(TARGET cumotion::cumotion APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE)\nset_target_properties(cumotion::cumotion PROPERTIES\n  IMPORTED_LOCATION_RELEASE \"${_IMPORT_PREFIX}/lib/libcumotion.so.1.1.0\"\n  IMPORTED_SONAME_RELEASE \"libcumotion.so.1\"\n  )\n\nlist(APPEND _cmake_import_check_targets cumotion::cumotion )\nlist(APPEND _cmake_import_check_files_for_cumotion::cumotion \"${_IMPORT_PREFIX}/lib/libcumotion.so.1.1.0\" )\n\n# Commands beyond this point should not need to know the version.\nset(CMAKE_IMPORT_FILE_VERSION)\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/lib/cmake/cumotion/cumotionTargets.cmake",
    "content": "# Generated by CMake\n\nif(\"${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}\" LESS 2.8)\n   message(FATAL_ERROR \"CMake >= 2.8.0 required\")\nendif()\nif(CMAKE_VERSION VERSION_LESS \"2.8.3\")\n   message(FATAL_ERROR \"CMake >= 2.8.3 required\")\nendif()\ncmake_policy(PUSH)\ncmake_policy(VERSION 2.8.3...3.22)\n#----------------------------------------------------------------\n# Generated CMake target import file.\n#----------------------------------------------------------------\n\n# Commands may need to know the format version.\nset(CMAKE_IMPORT_FILE_VERSION 1)\n\n# Protect against multiple inclusion, which would fail when already imported targets are added once more.\nset(_cmake_targets_defined \"\")\nset(_cmake_targets_not_defined \"\")\nset(_cmake_expected_targets \"\")\nforeach(_cmake_expected_target IN ITEMS cumotion::cumotion)\n  list(APPEND _cmake_expected_targets \"${_cmake_expected_target}\")\n  if(TARGET \"${_cmake_expected_target}\")\n    list(APPEND _cmake_targets_defined \"${_cmake_expected_target}\")\n  else()\n    list(APPEND _cmake_targets_not_defined \"${_cmake_expected_target}\")\n  endif()\nendforeach()\nunset(_cmake_expected_target)\nif(_cmake_targets_defined STREQUAL _cmake_expected_targets)\n  unset(_cmake_targets_defined)\n  unset(_cmake_targets_not_defined)\n  unset(_cmake_expected_targets)\n  unset(CMAKE_IMPORT_FILE_VERSION)\n  cmake_policy(POP)\n  return()\nendif()\nif(NOT _cmake_targets_defined STREQUAL \"\")\n  string(REPLACE \";\" \", \" _cmake_targets_defined_text \"${_cmake_targets_defined}\")\n  string(REPLACE \";\" \", \" _cmake_targets_not_defined_text \"${_cmake_targets_not_defined}\")\n  message(FATAL_ERROR \"Some (but not all) targets in this export set were already defined.\\nTargets Defined: ${_cmake_targets_defined_text}\\nTargets not yet defined: ${_cmake_targets_not_defined_text}\\n\")\nendif()\nunset(_cmake_targets_defined)\nunset(_cmake_targets_not_defined)\nunset(_cmake_expected_targets)\n\n\n# Compute the installation prefix relative to this file.\nget_filename_component(_IMPORT_PREFIX \"${CMAKE_CURRENT_LIST_FILE}\" PATH)\nget_filename_component(_IMPORT_PREFIX \"${_IMPORT_PREFIX}\" PATH)\nget_filename_component(_IMPORT_PREFIX \"${_IMPORT_PREFIX}\" PATH)\nget_filename_component(_IMPORT_PREFIX \"${_IMPORT_PREFIX}\" PATH)\nif(_IMPORT_PREFIX STREQUAL \"/\")\n  set(_IMPORT_PREFIX \"\")\nendif()\n\n# Create imported target cumotion::cumotion\nadd_library(cumotion::cumotion SHARED IMPORTED)\n\nset_target_properties(cumotion::cumotion PROPERTIES\n  INTERFACE_INCLUDE_DIRECTORIES \"${_IMPORT_PREFIX}/include\"\n  INTERFACE_LINK_LIBRARIES \"Eigen3::Eigen\"\n)\n\nif(CMAKE_VERSION VERSION_LESS 2.8.12)\n  message(FATAL_ERROR \"This file relies on consumers using CMake 2.8.12 or greater.\")\nendif()\n\n# Load information for each installed configuration.\nfile(GLOB _cmake_config_files \"${CMAKE_CURRENT_LIST_DIR}/cumotionTargets-*.cmake\")\nforeach(_cmake_config_file IN LISTS _cmake_config_files)\n  include(\"${_cmake_config_file}\")\nendforeach()\nunset(_cmake_config_file)\nunset(_cmake_config_files)\n\n# Cleanup temporary variables.\nset(_IMPORT_PREFIX)\n\n# Loop over all imported files and verify that they actually exist\nforeach(_cmake_target IN LISTS _cmake_import_check_targets)\n  foreach(_cmake_file IN LISTS \"_cmake_import_check_files_for_${_cmake_target}\")\n    if(NOT EXISTS \"${_cmake_file}\")\n      message(FATAL_ERROR \"The imported target \\\"${_cmake_target}\\\" references the file\n   \\\"${_cmake_file}\\\"\nbut this file does not exist.  Possible reasons include:\n* The file was deleted, renamed, or moved to another location.\n* An install or uninstall procedure did not complete successfully.\n* The installation package was faulty and contained\n   \\\"${CMAKE_CURRENT_LIST_FILE}\\\"\nbut not all the files it references.\n\")\n    endif()\n  endforeach()\n  unset(_cmake_file)\n  unset(\"_cmake_import_check_files_for_${_cmake_target}\")\nendforeach()\nunset(_cmake_target)\nunset(_cmake_import_check_targets)\n\n# This file does not depend on other imported targets which have\n# been exported from the same project but in a separate export set.\n\n# Commands beyond this point should not need to know the version.\nset(CMAKE_IMPORT_FILE_VERSION)\ncmake_policy(POP)\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/lib/libcumotion.so.1.1.0",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:fe13f8b2c1b6d908a72f087041a2385a1b4c2157323623bdf3e905f2852c9a2c\nsize 25997032\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/python_wheels/cumotion-1.1.0-cp312-cp312-linux_x86_64.whl",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:b3f3eb0699686f4c3b1ba218513040fe6cdcdf0e474f78d5a8fe06371819fec5\nsize 20054427\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cumotion/x86_64_cuda_13_0/python_wheels/cumotion_vis-1.1.0-py3-none-any.whl",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:ffebfff502ff599aa917503e2648255def6b815bf1092405bf732bb9bd056f3a\nsize 11885\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuvslam/.gitattributes",
    "content": "# Binaries\ncuvslam_api_launcher filter=lfs diff=lfs merge=lfs -text\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuvslam/include/cuvslam/cuvslam.h",
    "content": "/**\n * @file cuvslam.h\n\n * @copyright Copyright (c) 2025, NVIDIA CORPORATION.  All rights reserved.\\n\\n\n * NVIDIA CORPORATION and its licensors retain all intellectual property\n * and proprietary rights in and to this software, related documentation\n * and any modifications thereto.  Any use, reproduction, disclosure or\n * distribution of this software and related documentation without an express\n * license agreement from NVIDIA CORPORATION is strictly prohibited.\n */\n\n#pragma once\n\n/// @cond Doxygen_Suppress\n#ifdef __cplusplus\n#include <cstddef>\n#include <cstdint>\n\n#define CUVSLAM_DEFAULT(x) = (x)\n#else\n#include <stddef.h>\n#include <stdint.h>\n\n#define CUVSLAM_DEFAULT(x)\n#endif\n\n// TODO: duplicated definitions from cuvslam2.h with intent to remove this old API (cuvslam.h)\n#if !defined(CUVSLAM_API)\n\n#ifdef _WIN32\n#ifdef CUVSLAM_EXPORT\n#define CUVSLAM_API __declspec(dllexport)\n#else\n#define CUVSLAM_API __declspec(dllimport)\n#endif\n#else\n#define CUVSLAM_API __attribute__((visibility(\"default\")))\n#endif\n\n/// Major version. API is guaranteed to be compatible between the same major version numbers.\n#define CUVSLAM_API_VERSION_MAJOR 14\n\n/// Minor version\n#define CUVSLAM_API_VERSION_MINOR 1\n\n#endif  // !defined(CUVSLAM_API)\n\n#ifdef __cplusplus\nextern \"C\" {\n#endif\n/// @endcond\n\n/**\n * @name Error codes\n * Error codes are subject to change except for CUVSLAM_SUCCESS.\n * Under normal conditions you should only expect to see\n * ::CUVSLAM_SUCCESS and ::CUVSLAM_TRACKING_LOST.\n * All other error codes indicate unexpected errors.\n * In case of an unexpected error the tracker should be reinitialized.\n */\n///@{\n\n/// Success\n#define CUVSLAM_SUCCESS 0U\n\n/// Tracking lost\n#define CUVSLAM_TRACKING_LOST 1U\n\n/// Invalid argument\n#define CUVSLAM_INVALID_ARG 2U\n\n/// Can't localize\n#define CUVSLAM_CAN_NOT_LOCALIZE 3U\n\n/// Generic error\n#define CUVSLAM_GENERIC_ERROR 4U\n\n/// Unsupported number of cameras\n#define CUVSLAM_UNSUPPORTED_NUMBER_OF_CAMERAS 5U\n\n/// Slam is not initialized\n#define CUVSLAM_SLAM_IS_NOT_INITIALIZED 6U\n\n/// Not implemented\n#define CUVSLAM_NOT_IMPLEMENTED 7U\n\n/// Reading slam internals is disabled\n#define CUVSLAM_READING_SLAM_INTERNALS_DISABLED 8U\n///@}\n\n/**\n * Data layer\n */\nenum CUVSLAM_DataLayer {\n  LL_OBSERVATIONS = 0,            ///< Landmarks that are operated in current frame\n  LL_MAP = 1,                     ///< Landmarks of the map\n  LL_LOOP_CLOSURE = 2,            ///< Map's landmarks that are visible in the last loop closure event\n  LL_POSE_GRAPH = 3,              ///< Pose Graph\n  LL_LOCALIZER_PROBES = 4,        ///< Localizer probes\n  LL_LOCALIZER_MAP = 5,           ///< Landmarks of the Localizer map (opened database)\n  LL_LOCALIZER_OBSERVATIONS = 6,  ///< Landmarks that are visible in the localization\n  LL_LOCALIZER_LOOP_CLOSURE = 7,  ///< Landmarks that are visible in the final loop closure of the localization\n  LL_MAX = 8\n};\n\n/**\n * Image Encoding\n */\nenum CUVSLAM_ImageEncoding { MONO8, RGB8 };\n\n/**\n * Odometry Mode\n */\nenum CUVSLAM_OdometryMode { Multicamera, Inertial, RGBD };\n\n/**\n * Transformation from camera space to world space\n */\nstruct CUVSLAM_Pose {\n  float r[9];  ///< rotation column-major matrix\n  float t[3];  ///< translation vector\n};\n\n/**\n * IMU Calibration\n */\nstruct CUVSLAM_ImuCalibration {\n  struct CUVSLAM_Pose rig_from_imu;   /**< Rig from imu transformation.\n                                           vRig = rig_from_imu * vImu\n                                           - vImu - vector in imu coordinate system\n                                           - vRig - vector in rig coordinate system */\n  float gyroscope_noise_density;      ///< \\f$rad / (s * \\sqrt{hz})\\f$\n  float gyroscope_random_walk;        ///< \\f$rad / (s^2 * \\sqrt{hz})\\f$\n  float accelerometer_noise_density;  ///< \\f$m / (s^2 * \\sqrt{hz})\\f$\n  float accelerometer_random_walk;    ///< \\f$m / (s^3 * \\sqrt{hz})\\f$\n  float frequency;                    ///< \\f$hz\\f$\n};\n\n/**\n * Inertial measurement unit reading\n */\nstruct CUVSLAM_ImuMeasurement {\n  float linear_accelerations[3];  ///< in meters per squared second\n  float angular_velocities[3];    ///< in radians per second\n};\n\n/**\n * Describes intrinsic and extrinsic parameters of a camera.\n *\n * For camera coordinate system top left pixel has (0, 0) coordinate (y is down, x is right).\n * It's compatible with ROS CameraInfo/OpenCV.\n *\n * Supported values of distortion_model:\n * - brown5k (9 parameters):\n *   * 0-1: principal point \\f$(c_x, c_y)\\f$\\n\n *   * 2-3: focal length \\f$(f_x, f_y)\\f$\\n\n *   * 4-6: radial distortion coefficients \\f$(k_1, k_2, k_3)\\f$\\n\n *   * 7-8: tangential distortion coefficients \\f$(p_1, p_2)\\f$\\n\n *   .\n *   Each 3D point \\f$(x, y, z)\\f$ is projected in the following way:\\n\n *      \\f$(u, v) = (c_x, c_y) + diag(f_x, f_y) * (radial * (x_n, y_n) + tangentialDistortion)\\f$\\n\n *   where:\\n\n *      \\f$radial = (1 + k_1 * r^2 + k_2 * r^4 + k_3 * r^6)\\f$\\n\n *      \\f$tangentialDistortion.x = 2 * p_1 * x_n * y_n + p_2 * (r^2 + 2 * x_n^2)\\f$\\n\n *      \\f$tangentialDistortion.y = p_1 * (r^2 + 2 * y_n^2) + 2 * p_2 * x_n * y_n\\f$\\n\n *      \\f$x_n = \\frac{x}{z}\\f$\\n\n *      \\f$y_n = \\frac{y}{z}\\f$\\n\n *      \\f$r = \\sqrt{(x_n)^2 + (y_n)^2}\\f$\\n\n *\n * - pinhole (4 parameters):\\n\n *   no distortion, same as radial5 with \\f$k_0=k_1=k_2=p_0=p_1=0\\f$\\n\n *   * 0-1: principal point \\f$(c_x, c_y)\\f$\\n\n *   * 2-3: focal length \\f$(f_x, f_y)\\f$\\n\n *\n * - fisheye4 (8 parameters):\\n\n *   Also known as equidistant distortion model for pinhole cameras.\\n\n *   Coefficients k1, k2, k3, k4 are 100% compatible with ethz-asl/kalibr/pinhole-equi and OpenCV::fisheye.\\n\n *   See: \"A Generic Camera Model and Calibration Method for Conventional, Wide-Angle, and Fish-Eye Lenses\"\\n\n *   by Juho Kannala and Sami S. Brandt for further information.\\n\n *   Please note, this approach (pinhole+undistort) has a limitation and works only field-of-view below 180 deg.\\n\n *   For the TUMVI dataset FOV is ~190 deg.\\n\n *   EuRoC and ORB_SLAM3 use a different approach (direct project/unproject without pinhole) and support >180 deg, so\\n\n *   their coefficient is incompatible with this model.\\n\n *\n *   * 0-1: principal point \\f$(c_x, c_y)\\f$\\n\n *   * 2-3: focal length \\f$(f_x, f_y)\\f$\\n\n *   * 4-7: fisheye distortion coefficients \\f$(k_1, k_2, k_3, k_4)\\f$\\n\n *   .\n *   Each 3D point \\f$(x, y, z)\\f$ is projected in the following way:\\n\n *   \\f$(u, v) = (c_x, c_y) + diag(f_x, f_y) * (distortedR(r) * (x_n, y_n) / r)\\f$\\n\n *   where:\\n\n *     \\f$distortedR(r) = \\arctan(r) * (1 + k_1 * \\arctan^2(r) + k_2 * \\arctan^4(r) + k_3 * \\arctan^6(r) + k_4 *\n * \\arctan^8(r))\\f$\\n \\f$x_n = \\frac{x}{z}\\f$\\n \\f$y_n = \\frac{y}{z}\\f$\\n \\f$r = \\sqrt{(x_n)^2 + (y_n)^2}\\f$\\n\n *\n * - polynomial (12 parameters):\\n\n *   * 0-1: principal point \\f$(c_x, c_y)\\f$\\n\n *   * 2-3: focal length \\f$(f_x, f_y)\\f$\\n\n *   * 4-5: radial distortion coefficients \\f$(k_1, k_2)\\f$\\n\n *   * 6-7: tangential distortion coefficients \\f$(p_1, p_2)\\f$\\n\n *   * 8-11: radial distortion coefficients \\f$(k_3, k_4, k_5, k_6)\\f$\\n\n *   .\n *   Each 3D point \\f$(x, y, z)\\f$ is projected in the following way:\\n\n *     \\f$(u, v) = (c_x, c_y) + diag(f_x, f_y) * (radial * (x_n, y_n) + tangentialDistortion)\\f$\\n\n *   where:\\n\n *      \\f$radial = \\frac{1 + k_1 * r^2 + k_2 * r^4 + k_3 * r^6}{1 + k_4 * r^2 + k_5 * r^4 + k_6 * r^6}\\f$\\n\n *      \\f$tangentialDistortion.x = 2 * p_1 * x_n * y_n + p_2 * (r^2 + 2 * x_n^2)\\f$\\n\n *      \\f$tangentialDistortion.y = p_1 * (r^2 + 2 * y_n^2) + 2 * p_2 * x_n * y_n\\f$\\n\n *      \\f$x_n = \\frac{x}{z}\\f$\\n\n *      \\f$y_n = \\frac{y}{z}\\f$\\n\n *      \\f$r = \\sqrt{(x_n)^2 + (y_n)^2}\\f$\\n\n */\nstruct CUVSLAM_Camera {\n  const char *distortion_model;  ///< \"polynomial\" or \"fisheye4\" or \"pinhole\" or \"brown5k\"\n  const float *parameters;       ///< list of parameters\n  int32_t num_parameters;        ///< parameters number\n  int32_t width;                 ///< image width in pixels\n  int32_t height;                ///< image height in pixels\n  int32_t border_top;            ///< top border to ignore in pixels (0 to use full frame)\n  int32_t border_bottom;         ///< bottom border to ignore in pixels (0 to use full frame)\n  int32_t border_left;           ///< left border to ignore in pixels (0 to use full frame)\n  int32_t border_right;          ///< right border to ignore in pixels (0 to use full frame)\n\n  struct CUVSLAM_Pose pose;  ///< transformation from coordinate frame of the camera to frame of the rig\n};\n\n/**\n * Camera rig\n */\nstruct CUVSLAM_CameraRig {\n  const struct CUVSLAM_Camera *cameras;  ///< list of cameras parameters\n  int32_t num_cameras;                   ///< number of cameras (should be 2 for stereo)\n};\n\n/**\n * Localization settings\n */\nstruct CUVSLAM_LocalizationSettings {\n  float horizontal_search_radius;  ///< horizontal search radius in meters\n  float vertical_search_radius;    ///< vertical search radius in meters\n\n  float horizontal_step;    ///< horizontal step in meters\n  float vertical_step;      ///< vertical step in meters\n  float angular_step_rads;  ///< angular step around vertical axis in radians\n};\n\n/**\n * RGBD odometry settings\n */\nstruct CUVSLAM_RGBDOdometrySettings {\n  /**\n   * Scale of provided depth measurements, default 1.\n   */\n  float depth_scale_factor;\n\n  /**\n   * Depth camera id.\n   * Depth image is supposed to be pixel-to-pixel aligned with any RGB camera image.\n   * This field specifies camera id, that depth is aligned with. Default -1.\n   */\n  int32_t depth_camera_id;\n\n  /**\n   * Allows stereo 2D tracking between depth-aligned camera and any other camera. Default 0;\n   */\n  int32_t enable_depth_stereo_tracking;\n};\n\n/**\n * List of cameras available for SLAM\n */\nstruct CUVSLAM_SlamCameras {\n  uint32_t num;          ///< number of filled elements in camera list\n  int32_t *camera_list;  ///< cameras\n};\n\n/**\n * Configuration parameters that affect the whole tracking session\n */\nstruct CUVSLAM_Configuration {\n  /**\n   * Enable internal pose prediction mechanism based on a kinematic model.\n   * If frame rate is high enough it improves tracking performance\n   * and stability.\n   * Prediction passed into `CUVSLAM_TrackStereoSync` overrides prediction\n   * from the kinematic model.\n   * As a general rule it is better to use a pose prediction mechanism\n   * tailored to a specific application. If you have an IMU, consider using\n   * it to provide pose predictions to cuVSLAM.\n   *\n   */\n  int32_t use_motion_model;\n\n  /**\n   * Enable image denoising. Disable if the input images have already passed through a denoising filter.\n   */\n  int32_t use_denoising;\n\n  /**\n   * Enable feature tracking using GPU.\n   */\n  int32_t use_gpu;\n\n  /**\n   * Enable fast and robust left-to-right tracking for rectified cameras with principal points on the horizontal line.\n   */\n  int32_t horizontal_stereo_camera;\n\n  /**\n   * Allow to call CUVSLAM_GetLastLeftObservations.\n   */\n  int32_t enable_observations_export;\n\n  /**\n   * Allow to call CUVSLAM_GetLastLandmarks.\n   */\n  int32_t enable_landmarks_export;\n\n  /**\n   * Use localization and mapping.\n   */\n  int32_t enable_localization_n_mapping;\n\n  /**\n   * Size of map cell. Default is 0 (the size will be calculated from the camera baseline).\n   */\n  float map_cell_size;\n\n  /**\n   * If localization and mapping is used:\n   * sync mode (if true -> same thread with visual odometry). Default: slam_sync_mode = 0\n   */\n  int32_t slam_sync_mode;\n\n  // List of cameras available for SLAM\n  struct CUVSLAM_SlamCameras slam_cameras;\n\n  /**\n   * Enable reading internal data from SLAM\n   * CUVSLAM_EnableReadingDataLayer(), CUVSLAM_DisableReadingDataLayer()\n   */\n  int32_t enable_reading_slam_internals;\n\n  /**\n   * Set directory where the dump files will be saved:\n   *   stereo.edex - cameras and configuration\n   *   cam0.00000.png, cam1.00000.png, ... - input images\n   * example:\n   *    cfg->debug_dump_directory = \"/tmp/cuvslam\"\n   */\n  const char *debug_dump_directory;\n\n  /**\n   * Set maximum camera frame time in seconds.\n   * Compares delta between frames in tracker with max_frame_delta_s\n   * to warn user and prevent mistakes depending on hardware settings.\n   */\n  float max_frame_delta_s;\n\n  /**\n   * IMU calibration\n   */\n  struct CUVSLAM_ImuCalibration imu_calibration;\n\n  /**\n   * Visual odometry modes:\n   * Multicamera, Multicamera-Inertial, RGBD, default Multicamera.\n   */\n  enum CUVSLAM_OdometryMode odometry_mode;\n\n  /**\n   * Settings for the RGBD odometry.\n   */\n  struct CUVSLAM_RGBDOdometrySettings rgbd_settings;\n\n  /**\n   * Planar constraints.\n   * Slam poses will be modified so that the camera moves on a horizontal plane.\n   * See CUVSLAM_GetSlamPose()\n   */\n  int32_t planar_constraints;\n\n  /**\n   * Debug imu mode with only integration of rotation to check imu data correctness\n   */\n  int32_t debug_imu_mode;\n\n  /** It's a special slam mode for visual map building in case ground truth is present.\n   *  Not realtime, no loop closure, no map global optimization, SBA in main thread\n   * Requirements:\n   *      enable_localization_n_mapping = true\n   *      slam_sync_mode = 1\n   *      planar_constraints = 0\n   * Default is off (=0)\n   */\n  int32_t slam_gt_align_mode;\n\n  /** Maximum numbers of poses in SLAM pose graph.\n   * 300 is suitable for real-time mapping.\n   * Requirements:\n   *      enable_localization_n_mapping = true\n   * Default is 300\n   * The special value 0 means unlimited pose-graph.\n   */\n  int32_t slam_max_map_size;\n\n  /** Minimum time interval between loop closure events.\n   * 1000 is suitable for real-time mapping.\n   * Requirements:\n   *      enable_localization_n_mapping = true\n   * Default is 0\n   */\n  uint64_t slam_throttling_time_ms;\n\n  /**\n   * Multicamera mode: moderate (0), performance (1) or precision (2).\n   * Multicamera odometry settings will be adjusted depending on a chosen strategy.\n   * Default is moderate (0).\n   */\n  int32_t multicam_mode;\n\n  /**\n   * Localization Settings\n   */\n  struct CUVSLAM_LocalizationSettings localization_settings;\n};\n\n/**\n * Tracker\n */\nstruct CUVSLAM_Tracker;\n\n/**\n * Tracker Handle\n */\ntypedef struct CUVSLAM_Tracker *CUVSLAM_TrackerHandle;\n\n/**\n * Image\n */\nstruct CUVSLAM_Image {\n  const uint8_t *pixels;  ///< Pixels must be stored row-wise\n  int64_t timestamp_ns;   ///< pose timestamp will match image timestamp. Time must be in nanoseconds.\n  int32_t width;          ///< image width must match what was provided in CUVSLAM_Camera\n  int32_t height;         ///< image height must match what was provided in CUVSLAM_Camera\n  int32_t camera_index;   ///< index of the camera in the rig\n  int32_t pitch;          ///< bytes per image row including padding for GPU memory images\n  enum CUVSLAM_ImageEncoding image_encoding;  ///< grayscale (8 bit) and RGB (8 bit) are supported now\n  const uint8_t *input_mask;                  ///< input_mask must be stored row-wise\n  int32_t mask_width;\n  int32_t mask_height;\n  int32_t mask_pitch;\n};\n\n/**\n * DepthImage\n */\nstruct CUVSLAM_DepthImage {\n  const uint16_t *pixels;  ///< Pixels must be stored row-wise\n  int64_t timestamp_ns;    ///< pose timestamp will match image timestamp. Time must be in nanoseconds.\n  int32_t width;           ///< image width must match what was provided in CUVSLAM_Camera\n  int32_t height;          ///< image height must match what was provided in CUVSLAM_Camera\n  int32_t camera_index;    ///< index of the camera in the rig\n  int32_t pitch;           ///< bytes per image row including padding for GPU memory images\n};\n\n/**\n * Observation\n */\nstruct CUVSLAM_Observation {\n  int32_t id;  ///< id\n  float u;     ///< 0 <= u < image width\n  float v;     ///< 0 <= v < image height\n};\n\n/**\n * Observations list\n */\nstruct CUVSLAM_ObservationVector {\n  uint32_t num;                              ///< number of filled elements in observations list\n  uint32_t max;                              ///< size of pre-allocated observations\n  struct CUVSLAM_Observation *observations;  ///< observations\n};\n\n/**\n * Landmark\n * x, y, z in world frame\n */\nstruct CUVSLAM_Landmark {\n  uint64_t id;  ///< identifier\n  float x;      ///< x - coordinates\n  float y;      ///< y - coordinates\n  float z;      ///< z - coordinates\n};\n\n/**\n * List of landmarks\n */\nstruct CUVSLAM_LandmarkVector {\n  uint32_t num;                        ///< number of filled elements in landmarks list\n  uint32_t max;                        ///< size of pre-allocated landmarks\n  struct CUVSLAM_Landmark *landmarks;  ///< landmarks\n};\n\n/**\n * Gravity vector in the rig space\n */\nstruct CUVSLAM_Gravity {\n  float x;  ///< x - coordinate in meters\n  float y;  ///< y - coordinate in meters\n  float z;  ///< z - coordinate in meters\n};\n\n/**\n * Rig pose estimate from the tracker.\n * All fields will be set by the tracker.\n * The rig coordinate space is user-defined and depends on the extrinsic\n * parameters of the cameras.\n * The cameras' coordinate spaces may not match the rig coordinate space: extrinsic\n * The world coordinate space is an arbitrary 3D coordinate frame.\n * Pose covariance is defined via matrix exponential:\n * for a random zero-mean perturbation `u` in the tangent space\n * random pose is determined by `mean_pose * exp(u)`.\n */\nstruct CUVSLAM_PoseEstimate {\n  /**\n   * Transformation from the rig coordinate space to the world coordinate space.\n   */\n  struct CUVSLAM_Pose pose;\n\n  /**\n   * Pose timestamp in nanoseconds\n   */\n  int64_t timestamp_ns;\n\n  /** Row-major representation of the 6x6 covariance matrix\n   * The orientation parameters use a fixed-axis representation.\n   * In order, the parameters are:\n   * (rotation about X axis, rotation about Y axis, rotation about Z axis, x, y, z)\n   * Rotation in radians, translation in meters.\n   */\n  float covariance[6 * 6];\n};\n\n/**\n * Call result code\n */\ntypedef uint32_t CUVSLAM_Status;\n\n/**\n * Get internal slam metrics for visualization purpose.\n */\nstruct CUVSLAM_SlamMetrics {\n  int64_t timestamp_ns;                  ///< timestamp of these measurements (in nanoseconds)\n  uint32_t lc_status;                    ///< 0 - fail, 1 - success\n  uint32_t pgo_status;                   ///< 0 - fail, 1 - success\n  uint32_t lc_selected_landmarks_count;  ///< Count of Landmarks Selected\n  uint32_t lc_tracked_landmarks_count;   ///< Count of Landmarks Tracked\n  uint32_t lc_pnp_landmarks_count;       ///< Count of Landmarks in PNP\n  uint32_t lc_good_landmarks_count;      ///< Count of Landmarks in LC\n};\n\n/**\n * Landmarks coordinates in the camera space and pose graph reading\n */\nstruct CUVSLAM_LandmarkInfo {\n  int64_t id;    ///< identifier\n  float weight;  ///< weight\n  float x;       ///< x - coordinate in meters\n  float y;       ///< y - coordinate in meters\n  float z;       ///< z - coordinate in meters\n};\n\n/**\n * List of landmarks\n */\nstruct CUVSLAM_LandmarkInfoArrayRef {\n  uint64_t timestamp_ns;                         ///< timestamp of landmarks in nanoseconds\n  uint32_t num;                                  ///< landmarks number\n  const struct CUVSLAM_LandmarkInfo *landmarks;  ///< landmarks list\n};\n\n/**\n * Pose graph node\n */\nstruct CUVSLAM_PoseGraphNode {\n  uint64_t id;                    ///< node identifier\n  struct CUVSLAM_Pose node_pose;  ///< node pose\n};\n\n/**\n * Pose graph edge\n */\nstruct CUVSLAM_PoseGraphEdge {\n  uint64_t node_from;             ///< node id\n  uint64_t node_to;               ///< node id\n  struct CUVSLAM_Pose transform;  ///< transform\n  float covariance[6 * 6];        ///< covariance\n};\n\n/**\n * Pose graph\n */\nstruct CUVSLAM_PoseGraphRef {\n  uint64_t timestamp_ns;                      ///< timestamp of pose graph in nanoseconds\n  uint32_t num_edges;                         ///< edges number\n  uint32_t num_nodes;                         ///< nodes number\n  const struct CUVSLAM_PoseGraphNode *nodes;  ///< nodes list\n  const struct CUVSLAM_PoseGraphEdge *edges;  ///< edges list\n};\n\n/**\n * Localizer probes\n */\nstruct CUVSLAM_LocalizerProbe {\n  uint64_t id;                            ///< probe identifier\n  struct CUVSLAM_Pose guess_pose;         ///< input hint\n  struct CUVSLAM_Pose exact_result_pose;  ///< exact pose if localizer success\n  float weight;                           ///< input wight\n  float exact_result_weight;              ///< result weight\n  int32_t solved;                         ///< 1 for solved, 0 for unsolved\n};\n\n/**\n * List of localizer probes\n */\nstruct CUVSLAM_LocalizerProbesRef {\n  uint64_t timestamp_ns;                        ///< timestamp of localizer try in nanoseconds\n  uint32_t num_probes;                          ///< number of probes\n  float size;                                   ///< size of search area\n  const struct CUVSLAM_LocalizerProbe *probes;  ///< list of probes\n};\n\n/**\n * Pose with timestamp\n */\nstruct CUVSLAM_PoseStamped {\n  int64_t timestamp_ns;      ///< timestamp\n  struct CUVSLAM_Pose pose;  ///<\n};\n\n/** Pose graph nodes */\nstruct CUVSLAM_PoseGraphNodeVector {\n  uint32_t num;                       ///< number of filled elements in array\n  uint32_t max;                       ///< size of pre-allocated array\n  struct CUVSLAM_PoseStamped *nodes;  ///< nodes array\n};\n\n/** Pose graph edges */\nstruct CUVSLAM_PoseGraphEdgeVector {\n  uint32_t num;                         ///< number of filled elements in array\n  uint32_t max;                         ///< size of pre-allocated array\n  struct CUVSLAM_PoseGraphEdge *edges;  ///< edges array\n};\n\n/**\n * Asynchronous response for CUVSLAM_SaveToSlamDb()\n * @param[in] response_context - context\n * @param[in] status           - save result\n */\ntypedef void (*CUVSLAM_SaveToSlamDbResponse)(void *response_context, CUVSLAM_Status status);\n\n/**\n * Asynchronous response for CUVSLAM_LocalizeInExistDb()\n * @param[in] response_context - context\n * @param[in] status           - localize status\n * @param[in] pose_in_db       - position in database\n */\ntypedef void (*CUVSLAM_LocalizeInExistDbResponse)(void *response_context, CUVSLAM_Status status,\n                                                  const struct CUVSLAM_Pose *pose_in_db);\n\n/**\n * Use this function to check the version of the library you are using.\n * Any one of the pointers could be null.\n * @param[out] major   - major version\n * @param[out] minor   - minor version\n * @param[out] version - detailed version in string format\n */\nCUVSLAM_API\nvoid CUVSLAM_GetVersion(int32_t *major, int32_t *minor, const char **version);\n\n/**\n * Set verbosity. The higher the value, the more output from the library. 0 (default) for no output.\n * @param[in] verbosity new verbosity value\n */\nCUVSLAM_API\nvoid CUVSLAM_SetVerbosity(int verbosity);\n\n/**\n * Set Log filename. For internal usage only.\n * @param[in] filename new log filename\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_SetLogFilename(const char *filename);\n\n/**\n * Warms up GPU, creates CUDA runtime context.\n * This function is not mandatory to call, but helps to save some time in tracker initialization.\n */\nCUVSLAM_API\nvoid CUVSLAM_WarmUpGPU();\n\n/**\n * Creates the default configuration\n * @param[out] cfg default configuration\n */\nCUVSLAM_API\nvoid CUVSLAM_InitDefaultConfiguration(struct CUVSLAM_Configuration *cfg);\n\n/**\n * Get the default configuration\n * @return default configuration\n */\nCUVSLAM_API\nstruct CUVSLAM_Configuration CUVSLAM_GetDefaultConfiguration();\n\n/**\n * Use this to initialize cuVSLAM\n * CUVSLAM_TrackerHandle will remember number of cameras\n * from rig. cuVSLAM supports only Mono and Stereo rigs.\n * @param[out] tracker created tracker handle\n * @param[in] rig      rig setup\n * @param[in] cfg      tracker configuration\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_CreateTracker(CUVSLAM_TrackerHandle *tracker, const struct CUVSLAM_CameraRig *rig,\n                                     const struct CUVSLAM_Configuration *cfg);\n\n/**\n * Release all resources owned by the tracker\n * @param[in] tracker tracker handle\n */\nCUVSLAM_API\nvoid CUVSLAM_DestroyTracker(CUVSLAM_TrackerHandle tracker);\n\n/**\n * If visual odometry loses camera position, it briefly continues execution\n * using user-provided IMU measurements while trying to recover the position.\n * You should call this function several times between image acquisition.\n *\n * - CUVSLAM_Track\n * - CUVSLAM_RegisterImuMeasurement\n * - ...\n * - CUVSLAM_RegisterImuMeasurement\n * - CUVSLAM_Track\n *\n * Imu measurement and frame image both have timestamps, so it is important to call these functions in\n * strict timestamps in ascending order. CUVSLAM_RegisterImuMeasurement is a thread-safe so you can call\n * CUVSLAM_RegisterImuMeasurement and CUVSLAM_Track simultaneously.\n *\n * @param[in] tracker   tracker handle\n * @param[in] timestamp timestamp is in nanoseconds and should always increment\n * @param[in] imu       IMU measurements\n * @return result status (error code). This function returns false in a case of wrong timestamp order.\n * @see CUVSLAM_Track\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_RegisterImuMeasurement(CUVSLAM_TrackerHandle tracker, int64_t timestamp,\n                                              const struct CUVSLAM_ImuMeasurement *imu);\n\n/**\n * Track current frame synchronously:\n * the function blocks until the tracker has computed a pose.\n * By default, this function uses visual odometry to compute a pose. If visual\n * odometry tracker fails to compute a pose, the function returns the position\n * calculated from a user-provided IMU data.\n * If after several calls of CUVSLAM_Track the visual odometry tracker is not\n * recovered CUVSLAM_TRACKING_LOST will be returned.\n * The track will output poses in the same coordinate system\n * until a loss of tracking.\n * Image timestamps have to match. cuVSLAM will use timestamp\n * of the image taken with camera 0.\n * If your camera rig provides \"almost synchronized\" frames,\n * you could use one of the following for the common timestamp:\n * - timestamp from camera 0\n * - average timestamp\n *\n * You should use the same number of images for tracker equal to\n * rig->num_cameras in CUVSLAM_CreateTracker.\n * @param[in]  tracker        tracker handle\n * @param[in]  images         is a pointer to single image in case of mono or array of two images in case of stereo\n * @param[in]  images_size    number of images, provided to cuVSLAM\n * @param[in]  depth_image    depth image\n * @param[in]  predicted_pose If `predicted_pose` is not NULL, the tracker will use it as the initial guess.\n *                            For slam_gt_align_mode predicted pose is the ground truth pose.\n * @param[out] pose_estimate  On success (CUVSLAM_SUCCESS) `pose_estimate` contains estimated rig pose.\n *                            On failure value of `pose_estimate` is undefined.\n * @return result status (error code)\n * @see CUVSLAM_RegisterImuMeasurement\n * @note Use CUVSLAM_TrackGpuMem if images are stored in GPU memory.\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_Track(CUVSLAM_TrackerHandle tracker, const struct CUVSLAM_Image *images, size_t images_size,\n                             const struct CUVSLAM_DepthImage *depth_image, const struct CUVSLAM_Pose *predicted_pose,\n                             struct CUVSLAM_PoseEstimate *pose_estimate);\n\n/**\n * @cond Doxygen_Suppress\n * @brief Track current frame asynchronously with GPU images\n * @see CUVSLAM_Track\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_TrackGpuMem(CUVSLAM_TrackerHandle tracker, const struct CUVSLAM_Image *images,\n                                   size_t images_size, const struct CUVSLAM_DepthImage *depth_image,\n                                   const struct CUVSLAM_Pose *predicted_pose,\n                                   struct CUVSLAM_PoseEstimate *pose_estimate);\n/// @endcond\n\n/**\n * Get rig pose which was estimated by visual odometry.\n * Call CUVSLAM_Track() before CUVSLAM_GetOdometryPose().\n * @param[in]  tracker tracker handle\n * @param[out] pose    on success contains rig pose estimated by visual odometry\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_GetOdometryPose(CUVSLAM_TrackerHandle tracker, struct CUVSLAM_Pose *pose);\n\n/**\n * Get rig pose which was estimated by SLAM.\n * Call CUVSLAM_Track() before CUVSLAM_GetSlamPose().\n * You should set enable_localization_n_mapping=1 in the parameters of CUVSLAM_CreateTracker()\n * else CUVSLAM_SLAM_IS_NOT_INITIALIZED will be returned\n * @param[in]  tracker tracker handle\n * @param[out] pose    on success contains rig pose estimated by slam\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_GetSlamPose(CUVSLAM_TrackerHandle tracker, struct CUVSLAM_Pose *pose);\n\n/**\n * Set rig pose estimated by customer.\n * You should set enable_localization_n_mapping=1 in the parameters of CUVSLAM_CreateTracker()\n * else CUVSLAM_SLAM_IS_NOT_INITIALIZED will be returned\n * @param[in]  tracker tracker handle\n * @param[in]  pose    rig pose estimated by customer\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_SetSlamPose(CUVSLAM_TrackerHandle tracker, const struct CUVSLAM_Pose *pose);\n\n/**\n * Get a list of poses for each frame.\n * cuVSLAM keeps all poses (trajectory) relative to the slam pose-graph.\n * It's done special for CUVSLAM_GetAllPoses function.\n * It returns all poses optimized with the latest pose graph, so it's not the same as keeping run-time output.\n * With CUVSLAM_GetAllPoses, you will never have LC jumps because it recalculates past poses using\n * the current pose graph.\n * @param[in]      tracker                  tracker handle\n * @param[in]      max_poses_stamped_count  size of poses_stamped array\n * @param[in, out] poses_stamped            pre-allocated array to store poses\n * @return number of items copied to poses_stamped array\n * Requirements:\n *      enable_localization_n_mapping = true\n */\nCUVSLAM_API\nuint32_t CUVSLAM_GetAllSlamPoses(CUVSLAM_TrackerHandle tracker, uint32_t max_poses_stamped_count,\n                                 struct CUVSLAM_PoseStamped *poses_stamped);\n\n/**\n * Save Slam DB (map) to folder.\n * This folder will be created, if does not exist.\n * @param[in] tracker           tracker handle\n * @param[in] folder_name       Folder name, where Slam lmdb database (map) will be saved.\n * @param[in] response          User defined asynchronous response, which will be called before the end of\n *                              saving routine. May be used to handle various error codes.\n * @param[in] response_context  Pointer to the response context, which will be passed to\n *                              asynchronous response as argument.\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_SaveToSlamDb(CUVSLAM_TrackerHandle tracker, const char *folder_name,\n                                    CUVSLAM_SaveToSlamDbResponse response CUVSLAM_DEFAULT(nullptr),\n                                    void *response_context CUVSLAM_DEFAULT(nullptr));\n\n/**\n * Localize in the existing DB (map).\n * Finds the position of the camera in existing Slam lmdb database (map).\n * If success, moves the SLAM pose to the found position.\n * This is an asynchronous function. To receive result,\n * an asynchronous response CUVSLAM_LocalizeInExistDbResponse is used.\n * @param[in] tracker           tracker handle\n * @param[in] folder_name       Folder name, which stores saved Slam database (map).\n * @param[in] guess_pose        Pointer to the proposed pose, where the robot might be.\n * @param[in] images            Observed images. Will be used if CUVSLAM_Configuration.slam_sync_mode = 1.\n * @param[in] response          User defined asynchronous response, which will be called before the end of localization.\n *                              May be used to handle various error codes.\n * @param[in] response_context  Pointer to the response context, which will be passed to asynchronous\n *                              response as argument.\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_LocalizeInExistDb(CUVSLAM_TrackerHandle tracker, const char *folder_name,\n                                         const struct CUVSLAM_Pose *guess_pose,\n                                         const struct CUVSLAM_Image *images CUVSLAM_DEFAULT(nullptr),\n                                         size_t images_size CUVSLAM_DEFAULT(2),\n                                         CUVSLAM_LocalizeInExistDbResponse response CUVSLAM_DEFAULT(nullptr),\n                                         void *response_context CUVSLAM_DEFAULT(nullptr));\n\n/**\n * Get current observations (visual odometry 2d tracks) for visualization purpose.\n * @param[in]  tracker      tracker handle\n * @param[out] observations observations list\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_GetLastLeftObservations(CUVSLAM_TrackerHandle tracker,\n                                               struct CUVSLAM_ObservationVector *observations);\n\n/**\n * Get current landmarks (visual odometry 3d tracks) for visualization purpose.\n * @param[in]  tracker   tracker handle\n * @param[out] landmarks landmarks list\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_GetLastLandmarks(CUVSLAM_TrackerHandle tracker, struct CUVSLAM_LandmarkVector *landmarks);\n\n/**\n * Get gravity vector in the last VO frame\n * @param[in]  tracker tracker handle\n * @param[out] gravity gravity vector\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_GetLastGravity(CUVSLAM_TrackerHandle tracker, struct CUVSLAM_Gravity *gravity);\n\n/**\n * Get pose graph.\n * [DEPRECATED] use CUVSLAM_StartReadingPoseGraph/CUVSLAM_FinishReadingPoseGraph instead.\n * This function should not be used except on recordings after the replay.\n * It additionally returns timestamps for graph nodes.\n * @param[in]  tracker   tracker handle\n * @param[out] nodes pose graph nodes array\n * @param[out] edges pose graph edges array\n * @return result status (error code)\n * This call could be blocked by slam thread.\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_GetPoseGraph(CUVSLAM_TrackerHandle tracker, struct CUVSLAM_PoseGraphNodeVector *nodes,\n                                    struct CUVSLAM_PoseGraphEdgeVector *edges);\n\n/**\n * Get slam metrics\n * @param[in]  tracker      tracker handle\n * @param[out] slam_metrics slam info\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_GetSlamMetrics(CUVSLAM_TrackerHandle tracker, struct CUVSLAM_SlamMetrics *slam_metrics);\n\n/**\n * Get list of last 10 loop closure poses with timestamps.\n * lc_poses_stamped will be filled.\n * if lc_poses_stamped[index] is empty, then lc_poses_stamped[index].timestamp_ns < 0\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_GetLoopClosurePoseStamped(CUVSLAM_TrackerHandle tracker,\n                                                 struct CUVSLAM_PoseStamped *lc_poses_stamped);\n\n/**\n * Enable or disable landmarks layer reading\n * @param[in] tracker           tracker handle\n * @param[in] layer             any layer is acceptable\n * @param[in] max_items_count   maximum items number\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_EnableReadingDataLayer(CUVSLAM_TrackerHandle tracker, enum CUVSLAM_DataLayer layer,\n                                              uint32_t max_items_count);\n\n/**\n * Disable reading data layer\n * @param[in] tracker tracker handle\n * @param[in] layer   any layer is acceptable\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_DisableReadingDataLayer(CUVSLAM_TrackerHandle tracker, enum CUVSLAM_DataLayer layer);\n\n/**\n * Start landmarks layer reading. Have to call CUVSLAM_FinishReadingLandmarks. Thread-safe. Lock free\n * This function will fill all fields in the CUVSLAM_LandmarkInfoArrayRef structure\n * @param[in]  tracker    tracker handle\n * @param[in]  layer      one from LL_OBSERVATIONS, LL_MAP, LL_LOOP_CLOSURE, LL_LOCALIZER_MAP,\n *                        LL_LOCALIZER_OBSERVATIONS, LL_LOCALIZER_LOOP_CLOSURE\n * @param[out] landmarks  landmarks list\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_StartReadingLandmarks(CUVSLAM_TrackerHandle tracker, enum CUVSLAM_DataLayer layer,\n                                             struct CUVSLAM_LandmarkInfoArrayRef *landmarks);\n\n/**\n * Finish landmarks layer reading\n * @param[in] tracker tracker handle\n * @param[in] layer   one from LL_OBSERVATIONS, LL_MAP, LL_LOOP_CLOSURE, LL_LOCALIZER_MAP,\n *                    LL_LOCALIZER_OBSERVATIONS, LL_LOCALIZER_LOOP_CLOSURE\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_FinishReadingLandmarks(CUVSLAM_TrackerHandle tracker, enum CUVSLAM_DataLayer layer);\n\n/**\n * Start pose graph reading. Have to call CUVSLAM_FinishReadingPoseGraph. Thread-safe. Lock free\n * This function will fill all fields in the CUVSLAM_PoseGraphRef structure\n * @param[in]  tracker    tracker handle\n * @param[out] pose_graph pose graph\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_StartReadingPoseGraph(CUVSLAM_TrackerHandle tracker, struct CUVSLAM_PoseGraphRef *pose_graph);\n\n/**\n * Finish loop landmarks layer reading\n * @param[in] tracker tracker handle\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_FinishReadingPoseGraph(CUVSLAM_TrackerHandle tracker);\n\n/**\n * Start localizer probes reading. Have to call CUVSLAM_FinishReadingPoseGraph. Thread-safe. Lock free\n * This function will fill all fields in the CUVSLAM_LocalizerProbesRef structure\n * @param[in]  tracker           tracker handle\n * @param[out] localizer_probes  list of localizer probes\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_StartReadingLocalizerProbes(CUVSLAM_TrackerHandle tracker,\n                                                   struct CUVSLAM_LocalizerProbesRef *localizer_probes);\n\n/**\n * Finish reading localizer probes\n * @param[in] tracker tracker handle\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_FinishReadingLocalizerProbes(CUVSLAM_TrackerHandle tracker);\n\n/**\n * Merge existing maps into one map\n * @param[in] databases input array of directories with existing dbs\n * @param[in] databases_num size of input array\n * @param[in] output_folder directory to save output db\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_MergeDatabases(const struct CUVSLAM_CameraRig *rig, char const *const *databases,\n                                      int32_t databases_num, char const *output_folder);\n\n#ifdef __cplusplus\n}  // extern \"C\"\n#endif\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuvslam/include/cuvslam/cuvslam2.h",
    "content": "/*\n * Copyright (c) 2026, NVIDIA CORPORATION. All rights reserved.\n *\n * NVIDIA software released under the NVIDIA Open Software License is intended to be used permissively and enable the\n * further development of AI technologies. Subject to the terms of this License, NVIDIA confirms that you are free to\n * commercially use, modify, and distribute the software with NVIDIA hardware. NVIDIA does not claim ownership to any\n * outputs generated using the software or derivative works thereof. By using, reproducing, modifying, distributing,\n * performing or displaying any portion or element of the software or derivative works thereof, you agree to be bound by\n * this License.\n */\n\n#pragma once\n\n#include <array>\n#include <functional>\n#include <memory>\n#include <optional>\n#include <string>\n#include <string_view>\n#include <unordered_map>\n#include <vector>\n\n/// @cond Doxygen_Suppress\n#ifdef _WIN32\n#ifdef CUVSLAM_EXPORT\n#define CUVSLAM_API __declspec(dllexport)\n#else\n#define CUVSLAM_API __declspec(dllimport)\n#endif\n#else\n#define CUVSLAM_API __attribute__((visibility(\"default\")))\n#endif\n/// @endcond\n\nnamespace cuvslam {\n\n/**\n * @brief Get the version of the library.\n * Any one of the pointers could be null.\n * @param[out] major   - major version\n * @param[out] minor   - minor version\n * @param[out] patch   - patch version\n * @return semantic version string view\n */\nCUVSLAM_API\nstd::string_view GetVersion(int32_t* major, int32_t* minor, int32_t* patch);\n\n/**\n * Set verbosity. The higher the value, the more output from the library. 0 (default) for no output.\n * @param[in] verbosity new verbosity value\n */\nCUVSLAM_API\nvoid SetVerbosity(int verbosity);\n\n/**\n * Warms up GPU, creates CUDA runtime context.\n * This function is not mandatory to call, but helps to save some time in tracker initialization.\n * It can also be used to quickly diagnose issues with CUDA or CUDA libraries.\n * @throws std::runtime_error if CUDA, cusolver or cublas initialization fails.\n */\nCUVSLAM_API\nvoid WarmUpGPU();\n\n/**\n * Static-size array of 32-bit floats\n */\ntemplate <std::size_t N>\nusing Array = std::array<float, N>;\n\n/**\n * 3D vector of floats\n */\nusing Vector3f = Array<3>;\n\n/**\n * Static-size array of 32-bit integers\n */\ntemplate <std::size_t N>\nusing IntArray = std::array<int32_t, N>;\n\n/**\n * Transformation from one frame to another.\n * cuVSLAM uses OpenCV coordinate system convention: x is right, y is down, z is forward.\n */\nstruct Pose {\n  Array<4> rotation = {0, 0, 0, 1};  ///< rotation quaternion in (x, y, z, w) order\n  Array<3> translation = {0, 0, 0};  ///< translation vector\n};\n\n/**\n * 6x6 covariance matrix\n */\nusing PoseCovariance = Array<6 * 6>;\n\n/**\n * @brief Distortion model with parameters\n *\n * Terminology:\n * - principal point \\f$(c_x, c_y)\\f$\n * - focal length \\f$(f_x, f_y)\\f$\n * - 2x2 diagonal matrix \\f$\\mathrm{diag}(f_x, f_y) = \\begin{bmatrix} f_x & 0 \\\\ 0 & f_y \\end{bmatrix}\\f$\n *\n * Supported distortion models:\n *\n * - Pinhole (0 parameters)\n *   - No distortion; equivalent to Brown with \\f$k_0=k_1=k_2=p_0=p_1=0\\f$.\n *\n * - Fisheye (4 parameters)\n *   - Also known as equidistant model for pinhole cameras.\n *   - Coefficients \\f$k_1, k_2, k_3, k_4\\f$ are compatible with ethz-asl/kalibr (pinhole-equi) and OpenCV::fisheye.\n *   - Limitation: this (pinhole + undistort) approach works only for FOV < 180°. TUMVI has ~190°.\n *     EuRoC and ORB_SLAM3 use a different approach (direct project/unproject without pinhole) and support > 180°;\n *     their coefficients are incompatible with this model.\n *   - Parameters:\n *     - 0..3: \\f$(k_1, k_2, k_3, k_4)\\f$\n *   - Projection:\n *     - \\f$(u, v) = (c_x, c_y) + \\mathrm{diag}(f_x, f_y) \\cdot \\frac{\\mathrm{radial}(r) \\cdot (x_n, y_n)}{r}\\f$\n *     - where:\n *       - \\f$\\mathrm{radial}(r) = \\arctan(r) \\cdot \\left(1 + k_1 \\arctan^2(r) + k_2 \\arctan^4(r) + k_3 \\arctan^6(r)\n * + k_4 \\arctan^8(r)\\right)\\f$\n *       - \\f$x_n = x/z\\f$\n *       - \\f$y_n = y/z\\f$\n *       - \\f$r = \\sqrt{x_n^2 + y_n^2}\\f$\n *\n * - Brown (5 parameters)\n *   - Equivalent to Polynomial model with \\f$k_4=k_5=k_6=0\\f$; \\b note a different order of parameters.\n *   - Parameters:\n *     - 0..2: radial \\f$(k_1, k_2, k_3)\\f$\n *     - 3..4: tangential \\f$(p_1, p_2)\\f$\n *   - Projection:\n *     - \\f$(u, v) = (c_x, c_y) + \\mathrm{diag}(f_x, f_y) \\cdot \\left( \\mathrm{radial} \\cdot (x_n, y_n) + (t_x, t_y)\n * \\right)\\f$\n *     - where:\n *       - \\f$\\mathrm{radial} = 1 + k_1 r^2 + k_2 r^4 + k_3 r^6\\f$\n *       - \\f$t_x = 2 p_1 x_n y_n + p_2 (r^2 + 2 x_n^2)\\f$\n *       - \\f$t_y = p_1 (r^2 + 2 y_n^2) + 2 p_2 x_n y_n\\f$\n *       - \\f$x_n = x/z\\f$\n *       - \\f$y_n = y/z\\f$\n *       - \\f$r = \\sqrt{x_n^2 + y_n^2}\\f$\n *\n * - Polynomial (8 parameters)\n *   - Coefficients are compatible with the first 8 coefficients of the OpenCV distortion model.\n *   - Parameters:\n *     - 0..1: radial \\f$(k_1, k_2)\\f$\n *     - 2..3: tangential \\f$(p_1, p_2)\\f$\n *     - 4..7: radial \\f$(k_3, k_4, k_5, k_6)\\f$\n *   - Projection:\n *     - \\f$(u, v) = (c_x, c_y) + \\mathrm{diag}(f_x, f_y) \\cdot \\left( \\mathrm{radial} \\cdot (x_n, y_n) + (t_x, t_y)\n * \\right)\\f$\n *     - where:\n *       - \\f$\\mathrm{radial} = \\frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6}\\f$\n *       - \\f$t_x = 2 p_1 x_n y_n + p_2 (r^2 + 2 x_n^2)\\f$\n *       - \\f$t_y = p_1 (r^2 + 2 y_n^2) + 2 p_2 x_n y_n\\f$\n *       - \\f$x_n = x/z\\f$\n *       - \\f$y_n = y/z\\f$\n *       - \\f$r = \\sqrt{x_n^2 + y_n^2}\\f$\n */\nstruct Distortion {\n  /**\n   * Distortion model type\n   */\n  enum class Model : uint8_t {\n    Pinhole,\n    Fisheye,\n    Brown,\n    Polynomial,\n  };\n\n  Model model = Model::Pinhole;   ///< distortion model @see Model\n  std::vector<float> parameters;  ///< array of distortion parameters depending on model\n};\n\n/**\n * @brief Camera parameters\n *\n * Describes intrinsic and extrinsic parameters of a camera and per-camera settings.\n *\n * For camera coordinate system top left pixel has (0, 0) coordinate (y is down, x is right).\n * It's compatible with ROS CameraInfo/OpenCV.\n */\nstruct Camera {\n  IntArray<2> size;          ///< image size in pixels (width, height)\n  Array<2> principal;        ///< principal point in pixels \\f$(c_x, c_y)\\f$\n  Array<2> focal;            ///< focal length in pixels \\f$(f_x, f_y)\\f$\n  Pose rig_from_camera;      ///< transformation from coordinate frame of the camera to frame of the rig\n  Distortion distortion;     ///< distortion parameters\n  int32_t border_top{0};     ///< offset from the top border where visual features will be ignored (default: 0)\n  int32_t border_bottom{0};  ///< offset from the bottom border where visual features will be ignored (default: 0)\n  int32_t border_left{0};    ///< offset from the left border where visual features will be ignored (default: 0)\n  int32_t border_right{0};   ///< offset from the right border where visual features will be ignored (default: 0)\n};\n\n/**\n * @brief IMU Calibration parameters\n *\n * Describes intrinsic and extrinsic (noise and random walk) parameters of an IMU sensor.\n * See [IMU Noise Model](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model)\n */\nstruct ImuCalibration {\n  Pose rig_from_imu;                  /**< Rig from imu transformation.\n                                           vRig = rig_from_imu * vImu\n                                           - vImu - vector in imu coordinate system\n                                           - vRig - vector in rig coordinate system */\n  float gyroscope_noise_density;      ///< \\f$rad / (s * \\sqrt{hz})\\f$\n  float gyroscope_random_walk;        ///< \\f$rad / (s^2 * \\sqrt{hz})\\f$\n  float accelerometer_noise_density;  ///< \\f$m / (s^2 * \\sqrt{hz})\\f$\n  float accelerometer_random_walk;    ///< \\f$m / (s^3 * \\sqrt{hz})\\f$\n  float frequency;                    ///< \\f$hz\\f$\n};\n\n/**\n * @brief Rig consisting of cameras and IMU sensors\n *\n * @note 1 to 32 cameras are supported now.\n * @note 0 or 1 IMU sensor is supported now.\n * @note IMU sensor can be used only in Odometry::OdometryMode::Inertial mode with a single stereo camera.\n */\nstruct Rig {\n  std::vector<Camera> cameras;       ///< Cameras; 1 to 32 cameras are supported now\n  std::vector<ImuCalibration> imus;  ///< IMU sensors; 0 or 1 sensor is supported now\n};\n\n/**\n * @brief Image data structure\n *\n * @note Image pixels must be stored row-wise (right to left, top to bottom)\n * @note Image width and height must match Camera::size\n */\nstruct ImageData {\n  /// @brief Image encoding\n  enum class Encoding : uint8_t {\n    MONO,  ///< grayscale or other single-channel data\n    RGB,   ///< RGB\n  };\n  /// @brief Image data type\n  enum class DataType : uint8_t {\n    UINT8,    ///< 8-bit unsigned integer\n    UINT16,   ///< 16-bit unsigned integer\n    FLOAT32,  ///< 32-bit floating point\n  };\n\n  const void* pixels;  ///< Pixels must be stored row-wise (right to left, top to bottom)\n  int32_t width;       ///< image width must match Camera::size\n  int32_t height;      ///< image height must match Camera::size\n  int32_t pitch;       ///< bytes per image row including padding for GPU memory images, ignored for CPU images\n  Encoding encoding;   ///< grayscale and RGB are supported now\n  DataType data_type;  ///< image data type\n  bool is_gpu_mem;     ///< is pixels pointer points to GPU or CPU memory buffer\n};\n\n/**\n * @brief Image with timestamp and camera index\n */\nstruct Image : public ImageData {\n  int64_t timestamp_ns;   ///< Image timestamp in nanoseconds\n  uint32_t camera_index;  ///< index of the camera in the rig\n};\n\n/**\n * @brief IMU measurement\n */\nstruct ImuMeasurement {\n  int64_t timestamp_ns;           ///< IMU measurement timestamp in nanoseconds\n  Array<3> linear_accelerations;  ///<  \\f$m / s^2\\f$\n  Array<3> angular_velocities;    ///< \\f$rad / s\\f$\n};\n\n/**\n * @brief Pose with timestamp\n */\nstruct PoseStamped {\n  int64_t timestamp_ns;  ///< Pose timestamp in nanoseconds\n  Pose pose;             ///< Pose (transformation between two coordinate frames)\n};\n\n/**\n * @brief Pose with covariance\n *\n * Pose covariance is defined via matrix exponential:\n * for a random zero-mean perturbation `u` in the tangent space\n * random pose is determined by `mean_pose * exp(u)`.\n */\nstruct PoseWithCovariance {\n  Pose pose;                 ///< Pose (transformation between two coordinate frames)\n  PoseCovariance covariance; /**< Row-major representation of the 6x6 covariance matrix.\n                              The orientation parameters use a fixed-axis representation.\n                              In order, the parameters are:\n                              (rotation about X axis, rotation about Y axis, rotation about Z axis, x, y, z)\n                              Rotation in radians, translation in meters.*/\n};\n\n/**\n * @brief Rig pose estimate from the tracker\n *\n * The rig coordinate frame is user-defined and depends on the extrinsic parameters of the cameras.\n * The cameras' coordinate frames may not match the rig coordinate frame - depending on camers extrinsics.\n * The world coordinate frame is an arbitrary 3D coordinate frame. It coincides with the rig coordinate frame at the\n * first frame.\n */\nstruct PoseEstimate {\n  int64_t timestamp_ns;                              ///< Pose timestamp (in nanoseconds) will match image timestamp\n  std::optional<PoseWithCovariance> world_from_rig;  ///< Transform from rig coordinate frame to world coordinate frame\n};\n\n/**\n * @brief Observation\n *\n * 2D point with coordinates in image.\n * (0, 0) is the top-left corner.\n */\nstruct Observation {\n  uint64_t id;            ///< observation id\n  float u;                ///< 0 <= u < image width; (0, 0) is the top-left corner\n  float v;                ///< 0 <= v < image height; (0, 0) is the top-left corner\n  uint32_t camera_index;  ///< camera index\n};\n\n/**\n * @brief Landmark\n *\n * 3D point with coordinates in meters in world frame\n */\nstruct Landmark {\n  uint64_t id;      ///< landmark id\n  Vector3f coords;  ///< x, y, z in meters in world frame\n};\n\n/**\n * @brief Visual Inertial Odometry (VIO) Tracker\n */\nclass CUVSLAM_API Odometry {\npublic:\n  /// Image set\n  using ImageSet = std::vector<Image>;\n  /// Gravity vector\n  using Gravity = Vector3f;\n\n  /**\n   * @brief Multicamera mode\n   *\n   * Multicamera mode defines which cameras will be used for mono SOF (primary cameras)\n   */\n  enum class MulticameraMode : uint8_t {\n    /// primary cameras auto selection, each secondary camera must be connected to only one primary camera\n    Performance,\n    /// all cameras are primary cameras\n    Precision,\n    /// primary cameras auto selection, secondary cameras may be connected to more than one primary camera\n    Moderate,\n  };\n\n  /**\n   * @brief Odometry mode\n   *\n   * Odometry mode defines what data is expected and how it will be used by odometry tracker\n   */\n  enum class OdometryMode : uint8_t {\n    Multicamera,  ///< Uses multiple synchronized stereo cameras, all cameras need to have frustum overlap with at least\n                  ///< one another camera.\n    Inertial,     ///< Uses stereo camera and IMU measurements. A single stereo-camera with a single IMU sensor is\n                  ///< supported.\n    RGBD,  ///< Uses RGB-D camera for tracking. A single RGB-D camera is supported. RGB & Depth images must be aligned.\n    Mono   ///< Uses a single camera, tracking is accurate up to scale.\n  };\n\n  /**\n   * @brief RGBD odometry settings\n   */\n  struct RGBDSettings {\n    /// @brief Scale of provided depth measurements. Default: 1.f\n    ///\n    /// The scale factor is the denominator used to convert raw depth values from the input depth image to actual\n    /// distance measurements in meters. For example, in [TUM\n    /// RGB-D](https://cvg.cit.tum.de/data/datasets/rgbd-dataset/file_formats#intrinsic_camera_calibration_of_the_kinect)\n    /// a factor of 5000 is used for 16-bit PNG images, meaning each pixel value should be divided by 5000 to get the\n    /// depth in meters, while a factor of 1 is used for 32-bit float images, where the depth values are already in\n    /// meters.\n    float depth_scale_factor = 1.f;\n\n    /// @brief Depth camera id.\n    ///\n    /// Depth image is supposed to be pixel-to-pixel aligned with some RGB camera image.\n    /// This field specifies camera id, that depth is aligned with. Default: -1\n    int32_t depth_camera_id = -1;\n\n    /// Allows stereo 2D tracking between depth-aligned camera and any other camera. Default: false\n    bool enable_depth_stereo_tracking = false;\n  };\n\n  /**\n   * @brief Configuration parameters of the VIO tracker\n   */\n  struct Config {\n    /// Multicamera mode. Default: MulticameraMode::Precision\n    MulticameraMode multicam_mode = MulticameraMode::Precision;\n    /// Odometry mode. Default: OdometryMode::Multicamera\n    OdometryMode odometry_mode = OdometryMode::Multicamera;\n    /// Enable tracking using GPU. Default: true.\n    bool use_gpu = true;\n    /// Enable SBA asynchronous mode. Default: true.\n    bool async_sba = true;\n    /**\n     * @brief Enable internal pose prediction mechanism. Default: true\n     *\n     * If frame rate is high enough it improves tracking performance and stability.\n     * As a general rule it is better to use a pose prediction mechanism\n     * tailored to a specific application. If you have an IMU, consider using\n     * it to provide pose predictions to cuVSLAM.\n     */\n    bool use_motion_model = true;\n    /// Enable image denoising. Disable if the input images have already passed through a denoising filter.\n    /// Default: false\n    bool use_denoising = false;\n    /// Enable fast and robust tracking between rectified cameras with principal points on the horizontal line.\n    /// Default: false\n    bool rectified_stereo_camera = false;\n    /// Enable GetLastObservations(). Warning: export flags slow down execution and result in additional memory usage.\n    /// Default: false\n    bool enable_observations_export = false;\n    /// Enable GetLastLandmarks(). Warning: export flags slow down execution and result in additional memory usage.\n    /// Default: false\n    bool enable_landmarks_export = false;\n    /// Enable GetFinalLandmarks(). Warning: export flags slow down execution and result in additional memory usage.\n    /// This flag also sets enable_landmarks_export and enable_observations_export.\n    /// Default: false\n    bool enable_final_landmarks_export = false;\n    /// Maximum frame delta in seconds. Odometry will warn if time delta between frames is higher than the threshold.\n    /// Default: 1.f\n    float max_frame_delta_s = 1.f;\n    /// Directory where input data will be dumped in edex format.\n    std::string_view debug_dump_directory;\n    /// Enable IMU debug mode. Default: false\n    bool debug_imu_mode = false;\n    /// RGBD odometry settings.\n    RGBDSettings rgbd_settings;\n  };\n\n  // TODO(vikuznetsov): remove when https://gcc.gnu.org/bugzilla/show_bug.cgi?id=88165 is fixed\n  /// @brief Get default configuration.\n  ///\n  /// @see Config for default values.\n  /// @return default configuration\n  static Config GetDefaultConfig() { return Config{}; }\n\n  /**\n   * @brief State of the odometry tracker\n   *\n   * Only available if data export is enabled in Config.\n   */\n  struct State {\n    struct Context;\n    using ContextMap = std::unordered_map<uint8_t, std::shared_ptr<Context>>;\n\n    uint64_t frame_id;               ///< Internal frame id\n    int64_t timestamp_ns;            ///< Timestamp in nanoseconds\n    Pose delta;                      ///< Pose change since last keyframe\n    bool keyframe;                   ///< Is this frame a keyframe?\n    bool warming_up;                 ///< Is the tracker in warming up phase?\n    std::optional<Gravity> gravity;  ///< Optional gravity information. Only available in OdometryMode::Inertial mode.\n    std::vector<Observation> observations;  ///< Observations for this frame\n    std::vector<Landmark> landmarks;        ///< Landmarks for this frame\n    ContextMap context;                     ///< Opaque context information for this frame (used internally by Slam)\n  };\n\n  /**\n   * @brief Construct a tracker\n   *\n   * @param[in] rig  rig setup\n   * @param[in] cfg  tracker configuration\n   * @throws std::runtime_error if tracker fails to initialize\n   * @throws std::invalid_argument if rig or config is invalid\n   */\n  Odometry(const Rig& rig, const Config& cfg = GetDefaultConfig());\n\n  /**\n   * @brief Move constructor\n   *\n   * @param[in] other other tracker\n   */\n  Odometry(Odometry&& other) noexcept;\n\n  /// @brief Destructor\n  ~Odometry();\n\n  /**\n   * @brief Track a rig pose using current frame\n   *\n   * Track current frame synchronously: the function blocks until the tracker has computed a pose.\n   * By default, this function uses visual odometry to compute a pose.\n   * If visual odometry tracker fails to compute a pose, in inertial mode the function returns the position\n   * calculated from a user-provided IMU data.\n   * If after several calls of Track() visual odometry is not able to recover,\n   * then invalid pose will be returned.\n   *\n   * The track will output poses in the same coordinate system until a loss of tracking.\n   *\n   * Image timestamps have to match. cuVSLAM will use timestamp from the camera 0 image.\n   * If a camera rig provides \"almost synchronized\" frames, the timestamps should be within 1 millisecond.\n   * The number of images for tracker should not exceed rig->num_cameras.\n   *\n   * @param[in]  images  an array of synchronized images no more than rig->num_cameras.\n   * Must use ImageData::DataType::UINT8. Partial ImageSet is supported, for example due to frame drops. Corresponding\n   * cameras are identified by Image::camera_index.\n   * @param[in]  masks  (Optional) an array of corresponding masks no more than rig->num_cameras.\n   * Must use ImageData::DataType::UINT8. Partial ImageSet is supported, for example if mask is calculated on for some\n   * cameras. Corresponding cameras are identified by Image::camera_index.\n   * @param[in]  depths  (Optional) an array of corresponding depth images. One depth image is now supported.\n   * Must use ImageData::Encoding::MONO and ImageData::DataType::UINT16 or ImageData::DataType::FLOAT32.\n   *\n   * @return On success `PoseEstimate` contains estimated rig pose, on failure `PoseEstimate::world_from_rig` will be\n   * `nullopt`.\n   * @throws std::invalid_argument if image parameters are invalid\n   * @throws std::runtime_error in case of unexpected errors\n   */\n  PoseEstimate Track(const ImageSet& images, const ImageSet& masks = {}, const ImageSet& depths = {});\n\n  /**\n   * @brief Register IMU measurement\n   *\n   * If visual odometry loses camera position, it briefly continues execution\n   * using user-provided IMU measurements while trying to recover the position.\n   * You should call these functions in the order of ascending timestamps however many IMU measurements you have\n   * between image acquisitions:\n   *\n   * - tracker.Track\n   * - tracker.RegisterImuMeasurement\n   * - ...\n   * - tracker.RegisterImuMeasurement\n   * - tracker.Track\n   *\n   * IMU measurement and frame image both have timestamps, so it is important to call these functions in\n   * strict ascending order of timestamps. RegisterImuMeasurement is thread-safe so it's allowed to call\n   * RegisterImuMeasurement and Track in parallel.\n   *\n   * @param[in] sensor_index Sensor index; must be 0, as only one sensor is supported now\n   * @param[in] imu IMU measurements\n   * @throws std::invalid_argument if IMU fusion is disabled or if called out of the order of timestamps\n   * @see Track\n   */\n  void RegisterImuMeasurement(uint32_t sensor_index, const ImuMeasurement& imu);\n\n  /**\n   * @brief Get Last Observations\n   *\n   * Get an array of observations from the last VO frame for a specific camera\n   *\n   * @param[in] camera_index Index of the camera to get observations for\n   * @return Array of observations\n   * @throws std::invalid_argument if stats export is disabled\n   * @see Observation\n   */\n  std::vector<Observation> GetLastObservations(uint32_t camera_index) const;\n\n  /**\n   * @brief Get Last Landmarks\n   *\n   * Get an array of landmarks from the last VO frame;\n   * Landmarks are 3D points in the last camera frame.\n   * @return Array of landmarks\n   * @throws std::invalid_argument if stats export is disabled\n   * @see Landmark\n   */\n  std::vector<Landmark> GetLastLandmarks() const;\n\n  /**\n   * @brief Get Last Gravity\n   *\n   * Get gravity vector in the last VO frame\n   * @return Optional gravity vector. Empty if gravity is not yet available.\n   * @throws std::invalid_argument if IMU fusion is disabled\n   * @see Gravity\n   */\n  std::optional<Gravity> GetLastGravity() const;\n\n  /**\n   * @brief Get tracker state\n   *\n   * Only available if data export is enabled in Config.\n   *\n   * @param[out] state Odometry state to be filled\n   * @throws std::invalid_argument if stats export is disabled\n   * @see State\n   */\n  void GetState(Odometry::State& state) const;\n\n  /**\n   * @brief Get all final landmarks from all frames\n   *\n   * Landmarks are 3D points in the odometry start frame.\n   * @return std::unordered_map<uint64_t, Vector3f>\n   * @throws std::invalid_argument if stats export is disabled\n   * @see Landmark\n   */\n  std::unordered_map<uint64_t, Vector3f> GetFinalLandmarks() const;\n\n  /**\n   * @brief Get primary camera indices used for tracking\n   *\n   * @return Vector of primary camera indices\n   */\n  const std::vector<uint8_t>& GetPrimaryCameras() const;\n\nprivate:\n  class Impl;\n  std::unique_ptr<Impl> impl;\n};\n\n/**\n * @brief Result type that can hold either success data or error information.\n *\n * For use in callbacks. Result::error_message should not outlive the callback scope.\n */\n// TODO(C++23): replace with std::expected\ntemplate <typename T>\nstruct Result {\n  std::optional<T> data;           ///< data\n  std::string_view error_message;  ///< error message\n\n  /// Create a success result\n  /// @param[in] value data\n  /// @return Result\n  static Result<T> Success(T&& value) { return Result<T>{std::move(value), \"\"}; }\n\n  /// Create an error result\n  /// @param[in] message error message\n  /// @return Result\n  static Result<T> Error(std::string_view message) { return Result<T>{std::nullopt, message}; }\n};\n\n/**\n * Simultaneous Localization and Mapping (SLAM)\n */\nclass CUVSLAM_API Slam {\npublic:\n  /// @brief Image set\n  using ImageSet = std::vector<Image>;\n\n  /**\n   * @brief SLAM configuration parameters\n   */\n  struct Config {\n    /// If empty, map is kept in memory only. Else, map is synced to disk (LMDB) at this path, allowing large-scale\n    /// maps; if the path already exists it will be overwritten. To load an existing map, use LocalizeInMap(). To save\n    /// map, use SaveMap().\n    std::string_view map_cache_path = \"\";\n    /// Enable GPU use for SLAM\n    bool use_gpu = true;\n    /// Synchronous mode (does not run a separate work thread if true)\n    bool sync_mode = false;\n    /// Enable reading internal data from SLAM (Pose Graph, Loop Closures, Landmarks, etc.).\n    /// Additionally separate data layers are enabled by `EnableReadingData`.\n    bool enable_reading_internals = false;\n    /// Planar constraints. SLAM poses will be modified so that the camera moves on a horizontal plane.\n    bool planar_constraints = false;\n    /// Special SLAM mode for visual map building in case ground truth is present.\n    /// Not realtime, no loop closure, no map global optimization, SBA must be in main thread.\n    bool gt_align_mode = false;\n    /// Size of map cell. Default is 0 (the size will be calculated from the camera baseline).\n    float map_cell_size = 0.0f;\n    /// Maximum distance from camera to landmark for inclusion in map. Default is 100 meters.\n    float max_landmarks_distance = 100.f;\n    /// Maximum number of poses in SLAM pose graph. 300 is suitable for real-time mapping.\n    /// The special value 0 means unlimited pose-graph.\n    uint32_t max_map_size = 300;\n    /// Minimum time interval between loop closure events in milliseconds.\n    /// 1000 is suitable for real-time mapping.\n    uint32_t throttling_time_ms = 0;\n  };\n\n  // TODO(vikuznetsov): remove when https://gcc.gnu.org/bugzilla/show_bug.cgi?id=88165 is fixed\n  /// Get default configuration\n  /// @return default configuration\n  static Config GetDefaultConfig() { return Config{}; }\n\n  /**\n   * @brief Localization settings for use in LocalizeInMap\n   */\n  struct LocalizationSettings {\n    float horizontal_search_radius;  ///< horizontal search radius in meters\n    float vertical_search_radius;    ///< vertical search radius in meters\n    float horizontal_step;           ///< horizontal step in meters\n    float vertical_step;             ///< vertical step in meters\n    float angular_step_rads;         ///< angular step around vertical axis in radians\n    bool enable_reading_internals;   ///< enable reading internal data from SLAM\n  };\n\n  /**\n   * @brief Metrics\n   */\n  struct Metrics {\n    int64_t timestamp_ns;                  ///< timestamp of these measurements (in nanoseconds)\n    bool lc_status;                        ///< loop closure status\n    bool pgo_status;                       ///< pose graph optimization status\n    uint32_t lc_selected_landmarks_count;  ///< Count of Landmarks Selected\n    uint32_t lc_tracked_landmarks_count;   ///< Count of Landmarks Tracked\n    uint32_t lc_pnp_landmarks_count;       ///< Count of Landmarks in PNP\n    uint32_t lc_good_landmarks_count;      ///< Count of Landmarks in LC\n  };\n\n  /**\n   * @brief Data layer for SLAM\n   */\n  enum class DataLayer : uint8_t {\n    Landmarks,             ///< Landmarks that are visible in the current frame\n    Map,                   ///< Landmarks of the map\n    LoopClosure,           ///< Map's landmarks that are visible in the last loop closure event\n    PoseGraph,             ///< Pose Graph\n    LocalizerProbes,       ///< Localizer probes\n    LocalizerMap,          ///< Landmarks of the Localizer map (opened database)\n    LocalizerLandmarks,    ///< Landmarks that are visible in the localization\n    LocalizerLoopClosure,  ///< Landmarks that are visible in the final loop closure of the localization\n    Max,\n  };\n\n  /**\n   * @brief Pose graph node\n   */\n  struct PoseGraphNode {\n    uint64_t id;     ///< node identifier\n    Pose node_pose;  ///< node pose\n  };\n\n  /**\n   * @brief Pose graph edge\n   */\n  struct PoseGraphEdge {\n    uint64_t node_from;         ///< node id\n    uint64_t node_to;           ///< node id\n    Pose transform;             ///< transform\n    PoseCovariance covariance;  ///< covariance\n  };\n\n  /**\n   * @brief Pose graph\n   */\n  struct PoseGraph {\n    int64_t timestamp_ns;              ///< timestamp of the pose graph in nanoseconds\n    std::vector<PoseGraphNode> nodes;  ///< nodes list\n    std::vector<PoseGraphEdge> edges;  ///< edges list\n  };\n\n  /**\n   * @brief Landmark with additional information\n   */\n  struct Landmark {\n    uint64_t id;      ///< identifier\n    float weight;     ///< weight (ignored now)\n    Vector3f coords;  ///< x, y, z in meters in world frame\n  };\n\n  /**\n   * Landmarks array\n   */\n  struct Landmarks {\n    int64_t timestamp_ns;  ///< timestamp of landmarks in nanoseconds; corresponds to the timestamp of the frame where\n                           ///< the landmarks were observed\n    std::vector<Landmark> landmarks;  ///< landmarks list\n  };\n\n  /**\n   * @brief Localizer probe\n   *\n   * Debug data from localizer for internal use.\n   */\n  struct LocalizerProbe {\n    uint64_t id;                ///< probe identifier\n    Pose guess_pose;            ///< input hint\n    Pose exact_result_pose;     ///< exact pose if localizer success\n    float weight;               ///< input weight\n    float exact_result_weight;  ///< result weight\n    bool solved;                ///< true for solved, false for unsolved\n  };\n\n  /**\n   * @brief Localizer probes array.\n   *\n   * Debug data from localizer for internal use.\n   */\n  struct LocalizerProbes {\n    int64_t timestamp_ns;                ///< timestamp of localizer try in nanoseconds\n    float size;                          ///< size of search area in meters\n    std::vector<LocalizerProbe> probes;  ///< list of probes\n  };\n\n  /**\n   * Construct a SLAM instance with rig and primary cameras\n   * @param[in] rig Camera rig configuration\n   * @param[in] primary_cameras Vector of primary camera indices\n   * @param[in] config SLAM configuration\n   * @throws std::runtime_error if SLAM initialization fails\n   */\n  Slam(const Rig& rig, const std::vector<uint8_t>& primary_cameras, const Config& config = GetDefaultConfig());\n\n  /**\n   * Move constructor\n   * @param[in] other other SLAM instance\n   */\n  Slam(Slam&& other) noexcept;\n\n  /// Destructor\n  ~Slam();\n\n  /**\n   * Process tracking results from `Odometry::Track`. This should be called after each successful tracking.\n   * @param[in] state Odometry state containing all tracking data\n   * @param[in] gt_pose Optional ground truth pose. Should be provided if `gt_align_mode` is enabled, otherwise\n   * should be nullptr.\n   * @see `Odometry::Track`\n   * @throws std::invalid_argument if `gt_pose` is passed incorrectly\n   * @return On success `Pose` contains rig pose estimated by SLAM\n   */\n  Pose Track(const Odometry::State& state, const Pose* gt_pose = nullptr);\n\n  /**\n   * Set rig pose estimated by a user.\n   * @param[in] pose rig pose estimated by a user\n   */\n  void SetSlamPose(const Pose& pose);\n\n  /**\n   * Get all SLAM poses for each frame.\n   * @param[in] max_poses_count maximum number of poses to return\n   * @param[out] poses Vector of poses with timestamps\n   * This call could be blocked by slam thread.\n   */\n  void GetAllSlamPoses(std::vector<PoseStamped>& poses, uint32_t max_poses_count = 0) const;\n\n  /**\n   * Save SLAM database (map) to folder asynchronously.\n   * This folder will be created, if it does not exist.\n   * Contents of the folder will be overwritten.\n   * @param[in] folder_name Folder name, where SLAM database (map) will be saved\n   * @param[in] callback Callback function to be called when save is complete, may be called in a separate thread\n   */\n  void SaveMap(const std::string_view& folder_name, std::function<void(bool success)> callback) const;\n\n  /// Localization callback, may be called in a separate thread\n  using LocalizationCallback = std::function<void(const Result<Pose>& result)>;\n\n  /**\n   * Localize in the existing database (map) asynchronously.\n   * Finds the position of the camera in existing SLAM database (map).\n   * If successful, moves the SLAM pose to the found position.\n   * @param[in] folder_name Folder name, which stores saved SLAM database (map)\n   * @param[in] guess_pose Pointer to the proposed pose, where the robot might be\n   * @param[in] images Observed images. Will be used if Config::slam_sync_mode = true\n   * @param[in] settings Localization settings\n   * @param[in] callback Callback function to be called when localization is complete, may be called in a separate\n   * thread.\n   * @note Errors will be reported in the callback.\n   */\n  void LocalizeInMap(const std::string_view& folder_name, const Pose& guess_pose, const ImageSet& images,\n                     LocalizationSettings settings, LocalizationCallback callback);\n\n  /**\n   * Get SLAM metrics.\n   * @param[out] metrics SLAM metrics\n   */\n  void GetSlamMetrics(Metrics& metrics) const;\n\n  /**\n   * Get list of last 10 loop closure poses with timestamps.\n   * @param[out] poses Vector of poses with timestamps\n   */\n  void GetLoopClosurePoses(std::vector<PoseStamped>& poses) const;\n\n  /**\n   * Enable reading data layer.\n   * @param[in] layer Data layer to enable/disable\n   * @param[in] max_items_count Maximum number of items to allocate in the layer\n   */\n  void EnableReadingData(DataLayer layer, uint32_t max_items_count);\n\n  /**\n   * Disable reading data layer.\n   * @param[in] layer Data layer to disable\n   */\n  void DisableReadingData(DataLayer layer);\n\n  /**\n   * Read landmarks from a given data layer. Enabled by `EnableReadingData`.\n   * @param[in] layer Data layer to read\n   * @return Landmarks\n   */\n  std::shared_ptr<const Landmarks> ReadLandmarks(DataLayer layer);\n\n  /**\n   * Read pose graph. Enabled by `EnableReadingData(DataLayer::PoseGraph)`.\n   * @return Pose graph\n   */\n  std::shared_ptr<const PoseGraph> ReadPoseGraph();\n\n  /**\n   * Read localizer probes. Enabled by `EnableReadingData(DataLayer::LocalizerProbes)`.\n   *\n   * Debug data from localizer for internal use.\n   * @return Localizer probes\n   */\n  std::shared_ptr<const LocalizerProbes> ReadLocalizerProbes();\n\n  /**\n   * Merge existing maps into one map.\n   * @param[in] rig Camera rig configuration\n   * @param[in] databases Input array of directories with existing databases\n   * @param[in] output_folder Directory to save output database\n   * @throws std::runtime_error if merge fails\n   * @see Rig\n   */\n  static void MergeMaps(const Rig& rig, const std::vector<std::string_view>& databases,\n                        const std::string_view& output_folder);\n\nprivate:\n  class Impl;\n  std::unique_ptr<Impl> impl;\n};\n\n}  // namespace cuvslam\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuvslam/include/cuvslam/ground_constraint.h",
    "content": "/**\n * @file ground_constraint.h\n\n * @copyright Copyright (c) 2025, NVIDIA CORPORATION.  All rights reserved.\\n\\n\n * NVIDIA CORPORATION and its licensors retain all intellectual property\n * and proprietary rights in and to this software, related documentation\n * and any modifications thereto.  Any use, reproduction, disclosure or\n * distribution of this software and related documentation without an express\n * license agreement from NVIDIA CORPORATION is strictly prohibited.\n */\n\n#pragma once\n\n#include \"cuvslam/cuvslam.h\"\n\n#ifdef __cplusplus\nextern \"C\" {\n#endif\n/// @endcond\n\n/**\n * Ground constraint Handle\n *\n * The ground constraint is a post-processing tool to constrain 3d poses from Odometry on the ground plane.\n * Because of the drifts or failures after some time, 3D poses from odometry will have roll and pitch,\n * even if the robot moves on a flat surface. To resolve it, GroundConstraint will integrate 2d deltas in each frame,\n * removing roll and pitch angles on small deltas.\n */\ntypedef struct CUVSLAM_GroundConstraint* CUVSLAM_GroundConstraintHandle;\n\n/**\n * Initialize ground constraint\n * @param[out] ground                  created ground constraint handle\n * @param[in]  world_from_ground       world from ground plane transformation\n * @param[in]  initial_pose_on_ground  initial pose on ground in world coordinate frame\n                                       if pose is not belong to the ground plane it will projected\n                                       and rotated with nearest angle\n * @param[in]  initial_pose_in_space   initial pose in space in world coordinate frame\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_GroundConstraintCreate(CUVSLAM_GroundConstraintHandle* ground,\n                                              const struct CUVSLAM_Pose* world_from_ground,\n                                              const struct CUVSLAM_Pose* initial_pose_on_ground,\n                                              const struct CUVSLAM_Pose* initial_pose_in_space);\n\n/**\n * Release all resources owned by the ground constraint\n * @param[in] ground ground constraint handle\n */\nCUVSLAM_API\nvoid CUVSLAM_GroundConstraintDestroy(CUVSLAM_GroundConstraintHandle ground);\n\n/**\n * Add new 3d position from Odometry to 2d integrator\n * @param[in]  ground              ground constraint handle\n * @param[in]  next_pose_in_space  next to previous (or to initial) pose in 3d space in world frame\n *                                 pose it expected have just small delta from previous one.\n *                                 After projection roll and pitch angles will be removed.\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_GroundConstraintAddNextPose(CUVSLAM_GroundConstraintHandle ground,\n                                                   const struct CUVSLAM_Pose* next_pose_in_space);\n\n/**\n * Return current integrated 2d pose\n * @param[in]  handle          ground constraint handle\n * @param[in]  pose_on_ground  pose on the ground-plane in world frame\n * @return result status (error code)\n */\nCUVSLAM_API\nCUVSLAM_Status CUVSLAM_GroundConstraintGetPoseOnGround(CUVSLAM_GroundConstraintHandle handle,\n                                                       struct CUVSLAM_Pose* pose_on_ground);\n\n#ifdef __cplusplus\n}  // extern \"C\"\n#endif\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuvslam/include/cuvslam/ground_constraint2.h",
    "content": "/*\n * Copyright (c) 2026, NVIDIA CORPORATION. All rights reserved.\n *\n * NVIDIA software released under the NVIDIA Open Software License is intended to be used permissively and enable the\n * further development of AI technologies. Subject to the terms of this License, NVIDIA confirms that you are free to\n * commercially use, modify, and distribute the software with NVIDIA hardware. NVIDIA does not claim ownership to any\n * outputs generated using the software or derivative works thereof. By using, reproducing, modifying, distributing,\n * performing or displaying any portion or element of the software or derivative works thereof, you agree to be bound by\n * this License.\n */\n\n#pragma once\n\n#include <memory>\n\n#include \"cuvslam/cuvslam2.h\"\n\nnamespace cuvslam {\n\n/**\n * Ground constraint\n *\n * The ground constraint is a post-processing tool to constrain 3d poses from Odometry on the ground plane.\n * Because of the drifts or failures after some time, 3D poses from odometry will have roll and pitch,\n * even if the robot moves on a flat surface. To resolve it, GroundConstraint will integrate 2d deltas in each frame,\n * removing roll and pitch angles on small deltas.\n */\nclass CUVSLAM_API GroundConstraint {\npublic:\n  /**\n   * Construct a ground constraint integrator.\n   * @param[in] world_from_ground       Ground plane pose in world frame\n   * @param[in] initial_pose_on_ground  Initial pose projected on ground in world frame\n   * @param[in] initial_pose_in_space   Initial unconstrained pose in world frame\n   * @throws std::runtime_error on initialization failure\n   */\n  GroundConstraint(const Pose& world_from_ground, const Pose& initial_pose_on_ground,\n                   const Pose& initial_pose_in_space);\n\n  ~GroundConstraint();\n\n  GroundConstraint(const GroundConstraint&) = delete;\n  GroundConstraint& operator=(const GroundConstraint&) = delete;\n  /**\n   * Move constructor\n   * @param[in] other other ground constraint\n   */\n  GroundConstraint(GroundConstraint&& other) noexcept;\n  /**\n   * Move assignment operator\n   * @param[in] other other ground constraint\n   * @return reference to this\n   */\n  GroundConstraint& operator=(GroundConstraint&& other) noexcept;\n\n  /**\n   * Add next pose in space (3D) to the integrator.\n   * Removes roll and pitch after projecting to the ground plane.\n   * @param[in] next_pose_in_space Next pose in world frame\n   */\n  void AddNextPose(const Pose& next_pose_in_space);\n\n  /**\n   * Get current pose constrained to the ground plane in world frame.\n   * @return pose on the ground plane in world frame\n   */\n  Pose GetPoseOnGround() const;\n\nprivate:\n  struct Impl;\n  std::unique_ptr<Impl> impl;\n};\n\n}  // namespace cuvslam\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuvslam/lib_aarch64_jetpack61/cuvslam_api_launcher",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:483a184c4b5711d724bf7cec3274cd5f399ecf2a3bbcb68c53ba74a428781ed8\nsize 1481880\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuvslam/lib_aarch64_jetpack70/cuvslam_api_launcher",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:c8b7bcb74c3da5adc69fdadf511b1096ccb1828e5fc5c78e308f977be837a630\nsize 1510552\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuvslam/lib_x86_64_cuda_12_6/cuvslam_api_launcher",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:ce358e94403698807e4924a620333aeddd0864eac95059112d935424ab115df9\nsize 1629560\n"
  },
  {
    "path": "isaac_ros_nitros/lib/cuvslam/lib_x86_64_cuda_13_0/cuvslam_api_launcher",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:809717836ad7281cf5682f17c813c6418cc944a638072d43baad0f50d8bd7ecb\nsize 1711480\n"
  },
  {
    "path": "isaac_ros_nitros/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022-2025, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS Nitros</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n  <author>Swapnesh Wani</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>negotiated</depend>\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>std_msgs</depend>\n  <depend>diagnostic_msgs</depend>\n  <depend>isaac_ros_gxf</depend>\n  <depend>gxf_isaac_optimizer</depend>\n  <depend>gxf_isaac_message_compositor</depend>\n  <depend>ament_index_cpp</depend>\n  <depend>libnvvpi4</depend>\n\n  <exec_depend>gxf_isaac_gxf_helpers</exec_depend>\n  <exec_depend>gxf_isaac_sight</exec_depend>\n  <exec_depend>gxf_isaac_atlas</exec_depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>vpi4-dev</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros/scripts/diagnostic_viewer.py",
    "content": "#!/usr/bin/env python3\n\n# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport curses\nimport threading\nimport time\n\nfrom diagnostic_msgs.msg import DiagnosticArray\nimport rclpy\nfrom rclpy.node import Node\n\n\n\"\"\"\nUtility for viewing the nitros diagnostics in a terminal interface.\n\nYou can either use diagnostics that are built into applications, for example the\nisaac_ros_data_recorder, or you can export ENABLE_GLOBAL_DIAGNOSTICS=1 and view much more\n\"\"\"\n\n\nclass DiagnosticsDisplay(Node):\n\n    def __init__(self):\n        super().__init__('diagnostics_display')\n        self.subscription = self.create_subscription(\n            DiagnosticArray,\n            '/diagnostics',\n            self.listener_callback,\n            10)\n        self.subscription\n\n        # Thread-safe storage for diagnostic data\n        self.data_lock = threading.Lock()\n        self.status_data = {}\n\n    def listener_callback(self, msg):\n        with self.data_lock:\n            for status in msg.status:\n                self.status_data[status.name] = status\n\n\ndef curses_main(stdscr, node):\n    stdscr.nodelay(True)\n    curses.curs_set(0)\n    stdscr.keypad(True)  # Enable keypad mode to capture special keys\n\n    start_idx = 0\n    key = -1\n\n    while rclpy.ok():\n        stdscr.clear()\n        # Get terminal dimensions\n        height, width = stdscr.getmaxyx()\n\n        MAX_NAME_WIDTH = 100\n        FRAME_RATE_WIDTH = 15\n        REALTIME_DELAY_WIDTH = 15\n\n        # Adjust column widths if terminal is too narrow, +2 for spaces\n        total_width_needed = MAX_NAME_WIDTH + 2 * FRAME_RATE_WIDTH + 2 * REALTIME_DELAY_WIDTH + 2\n        if total_width_needed > width:\n            # Reduce the widths proportionally\n            scaling_factor = width / total_width_needed\n            MAX_NAME_WIDTH = int(MAX_NAME_WIDTH * scaling_factor)\n            FRAME_RATE_WIDTH = int(FRAME_RATE_WIDTH * scaling_factor)\n            REALTIME_DELAY_WIDTH = int(REALTIME_DELAY_WIDTH * scaling_factor)\n\n        header = (f\"{'Topic Name':<{MAX_NAME_WIDTH}} {'msg_frame_rate_hz':<{FRAME_RATE_WIDTH}} \"\n                  f\"{'pub_frame_rate_hz':<{FRAME_RATE_WIDTH}} \"\n                  f\"{'realtime_delay_ms':<{REALTIME_DELAY_WIDTH}}\")\n        separator = '-' * min(\n            width, MAX_NAME_WIDTH + 2 * FRAME_RATE_WIDTH + 2 * REALTIME_DELAY_WIDTH + 2)\n\n        try:\n            stdscr.addstr(0, 0, header[:width - 1])\n            stdscr.addstr(1, 0, separator)\n        except curses.error:\n            pass  # Ignore if cannot write header (terminal too small)\n\n        with node.data_lock:\n            status_list = list(node.status_data.values())\n\n        # Handle key presses\n        try:\n            key = stdscr.getch()\n        except Exception:\n            key = -1\n\n        # Up and down arrow keys to scroll\n        if key == curses.KEY_UP:\n            start_idx = max(0, start_idx - 1)\n        elif key == curses.KEY_DOWN:\n            start_idx = min(len(status_list) - (height - 5), start_idx + 1)\n        elif key == curses.KEY_PPAGE:  # Page Up\n            start_idx = max(0, start_idx - (height - 5))\n        elif key == curses.KEY_NPAGE:  # Page Down\n            start_idx = min(len(status_list) - (height - 5), start_idx + (height - 5))\n        elif key == ord('q') or key == ord('Q'):  # Quit on 'q' or 'Q'\n            break\n\n        # Ensure start_idx is within valid range\n        if start_idx < 0:\n            start_idx = 0\n        max_start_idx = max(0, len(status_list) - (height - 5))\n        if start_idx > max_start_idx:\n            start_idx = max_start_idx\n\n        visible_height = height - 5  # Adjusted for header, footer, and indicators\n\n        visible_status_list = status_list[start_idx:start_idx + visible_height]\n\n        for idx, status in enumerate(visible_status_list):\n            name = status.name\n            frame_rate_msg = ''\n            frame_rate_node = ''\n            current_delay_from_realtime_ms = ''\n            # Extract frame rates from key-value pairs\n            for kv in status.values:\n                if kv.key == 'frame_rate_msg':\n                    frame_rate_msg = kv.value\n                if kv.key == 'frame_rate_node':\n                    frame_rate_node = kv.value\n                if kv.key == 'current_delay_from_realtime_ms':\n                    current_delay_from_realtime_ms = kv.value\n\n            # No need to truncate the name since MAX_NAME_WIDTH is large\n            name_display = name.ljust(MAX_NAME_WIDTH)\n\n            # Format the data\n            frame_rate_msg_display = frame_rate_msg.ljust(FRAME_RATE_WIDTH)\n            frame_rate_node_display = frame_rate_node.ljust(FRAME_RATE_WIDTH)\n            current_delay_from_realtime_ms_display = current_delay_from_realtime_ms.ljust(\n                REALTIME_DELAY_WIDTH)\n            line = f'{name_display} {frame_rate_msg_display} {frame_rate_node_display}'\n            line += f' {current_delay_from_realtime_ms_display}'\n\n            # Truncate the line if necessary\n            line = line[:width - 1]  # Reserve last character for cursor\n\n            try:\n                stdscr.addstr(idx + 2, 0, line)\n            except curses.error:\n                pass  # Ignore if cannot write line (terminal too small)\n\n        # Add indicators if there's more data above or below\n        try:\n            if start_idx > 0:\n                stdscr.addstr(2, width - 2, '↑')\n            if start_idx + visible_height < len(status_list):\n                stdscr.addstr(height - 3, width - 2, '↓')\n        except curses.error:\n            pass\n\n        # Add a footer with status information\n        status_line = (f'Showing {start_idx + 1} - {start_idx + len(visible_status_list)} of '\n                       f'{len(status_list)} topics. Press \"q\" to quit.')\n\n        try:\n            stdscr.addstr(height - 2, 0, status_line[:width - 1])\n        except curses.error:\n            pass\n\n        stdscr.refresh()\n        time.sleep(0.1)\n\n\ndef main(args=None):\n    rclpy.init(args=args)\n    node = DiagnosticsDisplay()\n\n    try:\n        thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)\n        thread.start()\n        curses.wrapper(curses_main, node)\n    except KeyboardInterrupt:\n        pass\n    finally:\n        node.destroy_node()\n        rclpy.shutdown()\n\n\nif __name__ == '__main__':\n    main()\n"
  },
  {
    "path": "isaac_ros_nitros/src/nitros_context.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <cuda_runtime.h>\n#include <dlfcn.h>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros/nitros_context.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr uint64_t kDefaultCUDAMemoryPoolSize = 2048ULL * 1024 * 1024;\nconstexpr uint32_t kNumCudaStreams = 3;\nconstexpr char kDisableCUDAMemPoolEnv[] = \"DISABLE_NITROS_CUDA_MEM_POOL\";\n\ngxf_context_t NitrosContext::main_context_ = nullptr;\ngxf_context_t NitrosContext::shared_context_ = nullptr;\nstd::mutex NitrosContext::shared_context_mutex_;\nstd::set<std::string> NitrosContext::loaded_extension_file_paths_;\ngxf_severity_t NitrosContext::extension_log_severity_ = gxf_severity_t::GXF_SEVERITY_WARNING;\n\nstd::vector<std::string> SplitStrings(const std::string & list_of_files)\n{\n  std::vector<std::string> filenames;\n\n  for (size_t begin = 0;; ) {\n    const size_t end = list_of_files.find(',', begin);\n    if (end == std::string::npos) {\n      if (begin == 0 && !list_of_files.empty()) {\n        filenames.push_back(list_of_files);\n      } else if (!list_of_files.substr(begin).empty()) {\n        filenames.push_back(list_of_files.substr(begin));\n      }\n      break;\n    } else {\n      filenames.push_back(list_of_files.substr(begin, end - begin));\n      begin = end + 1;\n    }\n  }\n\n  return filenames;\n}\n\nstd::vector<char *> ToCStringArray(const std::vector<std::string> & strings)\n{\n  std::vector<char *> cstrings;\n  cstrings.reserve(strings.size());\n  for (size_t i = 0; i < strings.size(); ++i) {\n    cstrings.push_back(const_cast<char *>(strings[i].c_str()));\n  }\n  return cstrings;\n}\n\nbool IsCUDAMemPoolDisabledFromEnv(const char * env_name)\n{\n  const char * disable_cuda_mem_pool_env = std::getenv(env_name);\n  bool disable_cuda_mem_pool = false;\n  if (disable_cuda_mem_pool_env != nullptr) {\n    auto disable_cuda_mem_pool_str = std::string(disable_cuda_mem_pool_env);\n    if (disable_cuda_mem_pool_str == \"1\" || disable_cuda_mem_pool_str == \"true\") {\n      disable_cuda_mem_pool = true;\n    }\n  }\n  return disable_cuda_mem_pool;\n}\n\nNitrosContext::NitrosContext()\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n  gxf_result_t code;\n  if (NitrosContext::shared_context_ == nullptr) {\n    code = GxfContextCreate(&NitrosContext::main_context_);\n    if (code != GXF_SUCCESS) {\n      RCLCPP_ERROR(\n        get_logger(),\n        \"[NitrosContext] GxfContextCreate Error: %s\", GxfResultStr(code));\n      return;\n    }\n    code = GxfGetSharedContext(NitrosContext::main_context_, &NitrosContext::shared_context_);\n    if (code != GXF_SUCCESS) {\n      RCLCPP_ERROR(\n        get_logger(),\n        \"[NitrosContext] GxfGetSharedContext Error: %s\", GxfResultStr(code));\n      return;\n    }\n    if (!IsCUDAMemPoolDisabledFromEnv(kDisableCUDAMemPoolEnv)) {\n      RCLCPP_DEBUG(get_logger(), \"[NitrosContext] CUDA Memory Pool is enabled\");\n      code = setCUDAMemoryPoolSize(kDefaultCUDAMemoryPoolSize);\n      if (code != GXF_SUCCESS) {\n        RCLCPP_ERROR(\n          get_logger(),\n          \"[NitrosContext] setCUDAMemoryPoolSize Error: %s\", GxfResultStr(code));\n        return;\n      }\n    }\n  }\n\n  code = GxfContextCreateShared(NitrosContext::shared_context_, &context_);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfContextCreateShared Error: %s\", GxfResultStr(code));\n    return;\n  }\n\n  // Set log severity level for this GXF context if GXF_LOG_LEVEL is not set\n  if (GetSeverityFromEnv() == Severity::COUNT) {\n    code = GxfSetSeverity(context_, NitrosContext::extension_log_severity_);\n    if (code != GXF_SUCCESS) {\n      RCLCPP_ERROR(\n        get_logger(),\n        \"[NitrosContext] GxfSetSeverity Error: %s\", GxfResultStr(code));\n      return;\n    }\n  }\n\n  is_cuda_stream_initialized_ = false;\n  // End Mutex: shared_context_mutex_\n}\n\nvoid NitrosContext::setGraphNamespace(const std::string & graph_namespace)\n{\n  graph_namespace_ = graph_namespace;\n}\n\nvoid NitrosContext::setNode(const rclcpp::Node * node)\n{\n  node_ = node;\n}\n\ngxf_context_t NitrosContext::getContext()\n{\n  return context_;\n}\n\ngxf_result_t NitrosContext::initCudaStreamPool()\n{\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n  if (is_cuda_stream_initialized_) {\n    return GXF_SUCCESS;\n  }\n  // Initialise CUDA stream pool from the graph configuration\n  char kEntityName[] = \"cuda_stream_pool\";\n  char kComponentName[] = \"stream\";\n  char kComponentTypeName[] = \"nvidia::gxf::CudaStreamPool\";\n  gxf_uid_t cid;\n  gxf_result_t code = getCid(kEntityName, kComponentName, kComponentTypeName, cid);\n\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(), \"[NitrosContext] CUDA Stream Pool Error: %s\", GxfResultStr(code));\n    throw std::runtime_error(\"CUDA Stream Pool Initialisation failure\");\n  }\n\n  gxf::Handle<gxf::CudaStreamPool> cuda_stream_pool_ =\n    UNWRAP_OR_RETURN(gxf::Handle<gxf::CudaStreamPool>::Create(context_, cid));\n\n  // Allocate all available CUDA streams from the pool.\n  cuda_streams_.clear();\n  for (size_t i = 0; i < kNumCudaStreams; i++) {\n    gxf::Handle<gxf::CudaStream> stream_handle =\n      UNWRAP_OR_RETURN(cuda_stream_pool_.get()->allocateStream());\n\n    cudaStream_t maybe_raw_stream =\n      UNWRAP_OR_RETURN(stream_handle->stream());\n    cuda_streams_.push_back(maybe_raw_stream);\n  }\n\n  if (cuda_streams_.empty()) {\n    throw std::runtime_error(\"[NitrosContext] No CUDA streams could be allocated from the pool\");\n  }\n\n  for (size_t i = 0; i < kNumCudaStreams; i++) {\n    ::nvidia::isaac_ros::common::nameExistingCudaStream(\n      cuda_streams_[i],\n      \"nitros_type_adaptation_ros_to_custom_\" + std::to_string(i));\n  }\n\n  RCLCPP_INFO(\n    get_logger(),\n    \"[NitrosContext] Successfully allocated %zu CUDA streams\",\n    cuda_streams_.size());\n  is_cuda_stream_initialized_ = true;\n  return GXF_SUCCESS;\n}\n\ngxf_result_t NitrosContext::getComponentPointer(\n  const std::string & entity_name,\n  const std::string & component_name,\n  const std::string & component_type,\n  void ** pointer)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n\n  gxf_uid_t cid;\n  code = getCid(entity_name, component_name, component_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] getCid Error: %s\", GxfResultStr(code));\n    return code;\n  }\n\n  gxf_tid_t tid;\n  code = GxfComponentTypeId(context_, component_type.c_str(), &tid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfComponentTypeId Error: %s\", GxfResultStr(code));\n    return code;\n  }\n\n  code = GxfComponentPointer(context_, cid, tid, pointer);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfComponentPointer Error: %s\", GxfResultStr(code));\n    return code;\n  }\n\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::getEid(\n  const std::string & entity_name,\n  gxf_uid_t & eid)\n{\n  return GxfEntityFind(context_, getNamespacedEntityName(entity_name).c_str(), &eid);\n}\n\ngxf_result_t NitrosContext::getCid(\n  const std::string & entity_name,\n  const std::string & component_type,\n  gxf_uid_t & cid)\n{\n  return getCid(entity_name, \"\", component_type, cid);\n}\n\n// Get CUDA stream from the underlying CUDA stream pool variable in the graph\ncudaStream_t NitrosContext::getCudaStreamFromNitrosGraph()\n{\n  if (cuda_streams_.empty()) {\n    throw std::runtime_error(\"[NitrosContext] No CUDA streams available\");\n  }\n  size_t index = std::rand() % cuda_streams_.size();\n  cudaStream_t stream = cuda_streams_[index];\n  return stream;\n}\n\ngxf_result_t NitrosContext::getCid(\n  const std::string & entity_name,\n  const std::string & component_name,\n  const std::string & component_type,\n  gxf_uid_t & cid)\n{\n  gxf_result_t code;\n\n  gxf_uid_t eid;\n  code = GxfEntityFind(context_, getNamespacedEntityName(entity_name).c_str(), &eid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GXFEntityFind Error: %s\", GxfResultStr(code));\n    return code;\n  }\n\n  gxf_tid_t tid;\n  code = GxfComponentTypeId(context_, component_type.c_str(), &tid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfComponentTypeId Error: %s\", GxfResultStr(code));\n    return code;\n  }\n\n  if (component_name.empty()) {\n    code = GxfComponentFind(context_, eid, tid, nullptr, nullptr, &cid);\n  } else {\n    code = GxfComponentFind(context_, eid, tid, component_name.c_str(), nullptr, &cid);\n  }\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfComponentFind Error: %s\", GxfResultStr(code));\n    return code;\n  }\n\n  return GXF_SUCCESS;\n}\n\n// Override a parameter value in the graph to be loaded\nvoid NitrosContext::preLoadGraphSetParameter(\n  const std::string & entity_name,\n  const std::string & component_name,\n  const std::string & parameter_name,\n  const std::string & value\n)\n{\n  graph_param_override_string_list_.push_back(\n    graph_namespace_ + \"_\" + entity_name +\n    \"/\" + component_name +\n    \"/\" + parameter_name +\n    \"=\" + value);\n}\n\ngxf_result_t NitrosContext::loadExtension(\n  const std::string & base_dir,\n  const std::string & extension)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  // As the underlying context is shared across multiple NitrosNodes, we only need to load\n  // those extensions that have not been loaded. Loading same extensions twice will throw an\n  // error and GxfLoadExtensions() will stop.\n  std::string extension_file_path = base_dir + \"/\" + extension;\n  gxf_result_t code;\n  if (NitrosContext::loaded_extension_file_paths_.count(extension_file_path) > 0) {\n    // This extension has been loaded before\n    return GXF_SUCCESS;\n  }\n  RCLCPP_INFO(\n    get_logger(),\n    \"[NitrosContext] Loading extension: %s\", extension.c_str());\n\n  // Actually load the extension\n  NitrosContext::loaded_extension_file_paths_.insert(extension_file_path);\n  const char * extension_array[] = {extension.c_str()};\n  const GxfLoadExtensionsInfo load_extension_info{\n    extension_array, 1, nullptr, 0, base_dir.c_str()};\n\n  code = GxfLoadExtensions(context_, &load_extension_info);\n  if ((code != GXF_SUCCESS) && (code != GXF_FACTORY_DUPLICATE_TID)) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfLoadExtensions Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::loadExtensions(\n  const std::string & base_dir,\n  const std::vector<std::string> & extensions)\n{\n  gxf_result_t code;\n  for (const std::string & extension : extensions) {\n    code = loadExtension(base_dir, extension);\n    if (code != GXF_SUCCESS) {\n      RCLCPP_ERROR(\n        get_logger(),\n        \"[NitrosContext] loadExtension Error: %s\", GxfResultStr(code));\n      return code;\n    }\n  }\n  return GXF_SUCCESS;\n}\n\n// Loads application graph file(s)\ngxf_result_t NitrosContext::loadApplication(const std::string & list_of_files)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  const auto filenames = SplitStrings(list_of_files);\n\n  if (filenames.empty()) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] An NitrosNode application file has to be specified\");\n    return GXF_FILE_NOT_FOUND;\n  }\n\n  std::vector<char *> param_override_cstring = ToCStringArray(graph_param_override_string_list_);\n  for (const auto & filename : filenames) {\n    RCLCPP_DEBUG(get_logger(), \"[NitrosContext] Loading application: '%s'\", filename.c_str());\n    const gxf_result_t code = GxfGraphLoadFile(\n      context_,\n      filename.c_str(),\n      (const char **) &param_override_cstring[0],\n      graph_param_override_string_list_.size());\n    if (code != GXF_SUCCESS) {return code;}\n  }\n\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::runGraphAsync()\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n\n  RCLCPP_DEBUG(get_logger(), \"[NitrosContext] Initializing application...\");\n  code = GxfGraphActivate(context_);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfGraphActivate Error: %s\", GxfResultStr(code));\n    return code;\n  }\n\n  RCLCPP_DEBUG(get_logger(), \"[NitrosContext] Running application...\");\n  code = GxfGraphRunAsync(context_);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfGraphRunAsync Error: %s\", GxfResultStr(code));\n    return code;\n  }\n\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameterInt64(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const int64_t parameter_value)\n{\n  return setParameterInt64(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameterInt64(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const int64_t parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n  code = GxfParameterSetInt64(context_, cid, parameter_name.c_str(), parameter_value);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSetInt64 Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameterUInt64(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const uint64_t parameter_value)\n{\n  return setParameterUInt64(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameterUInt64(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const uint64_t parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n  code = GxfParameterSetUInt64(context_, cid, parameter_name.c_str(), parameter_value);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] setParameterUInt64 Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameterInt32(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const int32_t parameter_value)\n{\n  return setParameterInt32(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameterInt32(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const int32_t parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n  code = GxfParameterSetInt32(context_, cid, parameter_name.c_str(), parameter_value);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSetInt32 Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameterUInt32(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const uint32_t parameter_value)\n{\n  return setParameterUInt32(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameterUInt32(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const uint32_t parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n  code = GxfParameterSetUInt32(context_, cid, parameter_name.c_str(), parameter_value);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSetUInt32 Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameterUInt16(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const uint16_t parameter_value)\n{\n  return setParameterUInt16(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameterUInt16(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const uint16_t parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n  code = GxfParameterSetUInt16(context_, cid, parameter_name.c_str(), parameter_value);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSetUInt16 Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameterFloat32(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const float parameter_value)\n{\n  return setParameterFloat32(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameterFloat32(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const float parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n  code = GxfParameterSetFloat32(context_, cid, parameter_name.c_str(), parameter_value);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSetFloat32 Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameterFloat64(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const double parameter_value)\n{\n  return setParameterFloat64(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameterFloat64(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const double parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n  code = GxfParameterSetFloat64(context_, cid, parameter_name.c_str(), parameter_value);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSetFloat64 Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameterStr(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const std::string & parameter_value)\n{\n  return setParameterStr(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameterStr(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const std::string & parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n  code = GxfParameterSetStr(context_, cid, parameter_name.c_str(), parameter_value.c_str());\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSetStr Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameterHandle(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const gxf_uid_t & uid)\n{\n  return setParameterHandle(entity_name, \"\", codelet_type, parameter_name, uid);\n}\n\ngxf_result_t NitrosContext::setParameterHandle(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const gxf_uid_t & uid)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n  code = GxfParameterSetHandle(context_, cid, parameter_name.c_str(), uid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSetHandle Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameterBool(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const bool parameter_value)\n{\n  return setParameterBool(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameterBool(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const bool parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n  code = GxfParameterSetBool(context_, cid, parameter_name.c_str(), parameter_value);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSetBool Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameter1DStrVector(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const std::vector<std::string> & parameter_value)\n{\n  return setParameter1DStrVector(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameter1DStrVector(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  const std::vector<std::string> & parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n\n  std::vector<char *> parameter_value_cstring = ToCStringArray(parameter_value);\n\n  code = GxfParameterSet1DStrVector(\n    context_, cid, parameter_name.c_str(),\n    (const char **) &parameter_value_cstring[0], parameter_value_cstring.size());\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSet1DStrVector Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameter1DInt32Vector(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  std::vector<int32_t> & parameter_value)\n{\n  return setParameter1DInt32Vector(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameter1DInt32Vector(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  std::vector<int32_t> & parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n\n  code = GxfParameterSet1DInt32Vector(\n    context_, cid, parameter_name.c_str(),\n    parameter_value.data(), parameter_value.size());\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSet1DInt32Vector Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameter1DInt64Vector(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  std::vector<int64_t> & parameter_value)\n{\n  return setParameter1DInt64Vector(entity_name, \"\", codelet_type, parameter_name, parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameter1DInt64Vector(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  std::vector<int64_t> & parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n\n  code = GxfParameterSet1DInt64Vector(\n    context_, cid, parameter_name.c_str(),\n    parameter_value.data(), parameter_value.size());\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSet1DInt64Vector Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\ngxf_result_t NitrosContext::setParameter1DFloat64Vector(\n  const std::string & entity_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  std::vector<double> & parameter_value)\n{\n  return setParameter1DFloat64Vector(\n    entity_name, \"\", codelet_type, parameter_name,\n    parameter_value);\n}\n\ngxf_result_t NitrosContext::setParameter1DFloat64Vector(\n  const std::string & entity_name,\n  const std::string & codelet_name,\n  const std::string & codelet_type,\n  const std::string & parameter_name,\n  std::vector<double> & parameter_value)\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  gxf_uid_t cid;\n  code = getCid(entity_name, codelet_name, codelet_type, cid);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get CID for setting parameters\");\n    return code;\n  }\n\n  code = GxfParameterSet1DFloat64Vector(\n    context_, cid, parameter_name.c_str(),\n    parameter_value.data(), parameter_value.size());\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfParameterSet1DFloat64Vector Error: %s\", GxfResultStr(code));\n    return code;\n  }\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\nstd::string NitrosContext::getNamespacedEntityName(const std::string & entity_name)\n{\n  if (graph_namespace_.empty()) {\n    return entity_name;\n  }\n  return graph_namespace_ + \"_\" + entity_name;\n}\n\nrclcpp::Logger NitrosContext::get_logger()\n{\n  if (node_ != nullptr) {\n    return node_->get_logger();\n  }\n  return rclcpp::get_logger(\"NitrosContext\");\n}\n\ngxf_result_t NitrosContext::getEntityTimestamp(\n  const gxf_uid_t eid,\n  std_msgs::msg::Header & ros_header)\n{\n  nvidia::gxf::Expected<nvidia::gxf::Entity> entity = nvidia::gxf::Entity::Shared(context_, eid);\n\n  nvidia::gxf::Expected<nvidia::gxf::Handle<nvidia::gxf::Timestamp>> timestamp_handle =\n    entity->get<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!timestamp_handle) {\n    timestamp_handle = entity->get<nvidia::gxf::Timestamp>();\n  }\n\n  if (!timestamp_handle) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] Failed to get a timestamp component from an entity: %s\",\n      GxfResultStr(timestamp_handle.error()));\n    return timestamp_handle.error();\n  }\n\n  ros_header.stamp.sec =\n    static_cast<int32_t>(timestamp_handle.value()->acqtime / static_cast<uint64_t>(1e9));\n  ros_header.stamp.nanosec =\n    static_cast<uint32_t>(timestamp_handle.value()->acqtime % static_cast<uint64_t>(1e9));\n\n  return GXF_SUCCESS;\n}\n\nvoid NitrosContext::setExtensionLogSeverity(gxf_severity_t severity_level)\n{\n  if (GetSeverityFromEnv() != Severity::COUNT) {\n    // GXF_LOG_LEVEL environment variable is set, so ignore the current setting\n    return;\n  }\n  if (severity_level > NitrosContext::extension_log_severity_) {\n    NitrosContext::extension_log_severity_ = severity_level;\n  }\n  gxf_result_t code;\n  code = GxfSetSeverity(context_, NitrosContext::extension_log_severity_);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      get_logger(),\n      \"[NitrosContext] GxfSetSeverity Error: %s\", GxfResultStr(code));\n  }\n}\n\ngxf_result_t NitrosContext::setCUDAMemoryPoolSize(uint64_t cuda_mem_pool_size)\n{\n  // Set the minimal default CUDA memory pool release threshold to 2 GB\n  int n_devices;\n  cudaMemPool_t default_cuda_mem_pool;\n  cudaError_t cuda_error{cudaSuccess};\n\n  cuda_error = cudaGetDeviceCount(&n_devices);\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      cudaGetErrorName(cuda_error) << \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosContext\"), error_msg.str().c_str());\n    return GXF_FAILURE;\n  }\n\n  if (n_devices == 0) {\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosContext\"), \"No device is available\");\n    return GXF_FAILURE;\n  }\n\n  cuda_error = cudaDeviceGetDefaultMemPool(&default_cuda_mem_pool, 0);\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      cudaGetErrorName(cuda_error) << \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosContext\"), error_msg.str().c_str());\n    return GXF_FAILURE;\n  }\n\n  uint64_t cur_release_threshold = 0;\n  cuda_error = cudaMemPoolGetAttribute(\n    default_cuda_mem_pool, cudaMemPoolAttrReleaseThreshold, &cur_release_threshold);\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      cudaGetErrorName(cuda_error) << \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosContext\"), error_msg.str().c_str());\n    return GXF_FAILURE;\n  }\n\n  if (cur_release_threshold < cuda_mem_pool_size) {\n    auto cuda_error = cudaMemPoolSetAttribute(\n      default_cuda_mem_pool, cudaMemPoolAttrReleaseThreshold, &cuda_mem_pool_size);\n    if (cuda_error != cudaSuccess) {\n      std::stringstream error_msg;\n      error_msg <<\n        cudaGetErrorName(cuda_error) << \" (\" << cudaGetErrorString(cuda_error) << \")\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosContext\"), error_msg.str().c_str());\n      return GXF_FAILURE;\n    }\n  }\n\n  return GXF_SUCCESS;\n}\n\ngxf_result_t NitrosContext::destroy()\n{\n  // Mutex: shared_context_mutex_\n  const std::lock_guard<std::mutex> lock(NitrosContext::shared_context_mutex_);\n\n  gxf_result_t code;\n  RCLCPP_INFO(get_logger(), \"[NitrosContext] Interrupting GXF...\");\n  code = GxfGraphInterrupt(context_);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(get_logger(), \"[NitrosContext] GxfGraphInterrupt Error: %s\", GxfResultStr(code));\n  }\n\n  RCLCPP_INFO(get_logger(), \"[NitrosContext] Waiting on GXF...\");\n  code = GxfGraphWait(context_);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(get_logger(), \"[NitrosContext] GxfGraphWait Error: %s\", GxfResultStr(code));\n    return code;\n  }\n\n  RCLCPP_INFO(get_logger(), \"[NitrosContext] Deinitializing...\");\n  code = GxfGraphDeactivate(context_);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(get_logger(), \"[NitrosContext] GxfGraphDeactivate Error: %s\", GxfResultStr(code));\n    return code;\n  }\n\n  RCLCPP_INFO(get_logger(), \"[NitrosContext] Destroying context\");\n  code = GxfContextDestroy(context_);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(get_logger(), \"[NitrosContext] GxfContextDestroy Error: %s\", GxfResultStr(code));\n    return code;\n  }\n\n  return GXF_SUCCESS;\n  // End Mutex: shared_context_mutex_\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros/src/nitros_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <unistd.h>\n#include <filesystem>\n\n#include <ament_index_cpp/get_package_share_directory.hpp>\n\n#include \"extensions/gxf_optimizer/core/optimizer.hpp\"\n#include \"gxf/core/gxf.h\"\n\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/logger.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char kGraphExportDirectory[] = \"/tmp/isaac_ros_nitros/graphs\";\n\nconstexpr char kGxfVaultComponentTypeName[] = \"nvidia::gxf::Vault\";\nconstexpr char kGxfMessageRelayComponentTypeName[] =\n  \"nvidia::isaac_ros::MessageRelay\";\n\nconst std::vector<std::pair<std::string, std::string>> BUILTIN_EXTENSIONS = {\n  {\"isaac_ros_gxf\", \"gxf/lib/std/libgxf_std.so\"},\n  {\"isaac_ros_gxf\", \"gxf/lib/multimedia/libgxf_multimedia.so\"},\n  {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n};\n\nconst std::vector<std::string> BUILTIN_EXTENSION_SPEC_FILENAMES = {};\n\nconst std::vector<std::string> PRESET_EXTENSION_SPEC_NAMES = {\n  \"isaac_ros_nitros\",\n};\n\nnamespace\n{\n// Generate a random string of the given length\nunsigned int seed = time(NULL) ^ getpid();\nstd::string GenerateRandomString(const size_t length)\n{\n  std::string output_string;\n  char rand_char;\n  for (size_t i = 0; i < length; i++) {\n    rand_char = 'A' + rand_r(&seed) % 26;\n    output_string.push_back(rand_char);\n  }\n  return output_string;\n}\n}  // namespace\n\nNitrosNode::NitrosNode(\n  const rclcpp::NodeOptions & options,\n  const std::string & app_yaml_filename,\n  const GraphIOGroupSupportedDataTypesInfoList & gxf_io_group_info_list,\n  const NitrosPublisherSubscriberConfigMap & config_map,\n  const std::vector<std::string> & extension_spec_filenames,\n  const std::vector<std::string> & generator_rule_filenames,\n  const std::vector<std::pair<std::string, std::string>> & extensions,\n  const std::string & package_name)\n: NitrosNode(\n    options,\n    app_yaml_filename,\n    gxf_io_group_info_list,\n    true,  // Use the given gxf_io_group_info_list\n    true,\n    config_map,\n    {},\n    extension_spec_filenames,\n    generator_rule_filenames,\n    extensions,\n    package_name)\n{\n}\n\nNitrosNode::NitrosNode(\n  const rclcpp::NodeOptions & options,\n  const std::string & app_yaml_filename,\n  const GraphIOGroupSupportedDataTypesInfoList & gxf_io_group_info_list,\n  const NitrosPublisherSubscriberConfigMap & config_map,\n  const std::vector<std::string> & extension_spec_preset_names,\n  const std::vector<std::string> & extension_spec_filenames,\n  const std::vector<std::string> & generator_rule_filenames,\n  const std::vector<std::pair<std::string, std::string>> & extensions,\n  const std::string & package_name)\n: NitrosNode(\n    options,\n    app_yaml_filename,\n    gxf_io_group_info_list,\n    true,  // Use the given gxf_io_group_info_list\n    false,\n    config_map,\n    extension_spec_preset_names,\n    extension_spec_filenames,\n    generator_rule_filenames,\n    extensions,\n    package_name)\n{\n}\n\n\nNitrosNode::NitrosNode(\n  const rclcpp::NodeOptions & options,\n  const std::string & app_yaml_filename,\n  const NitrosPublisherSubscriberConfigMap & config_map,\n  const std::vector<std::string> & extension_spec_filenames,\n  const std::vector<std::string> & generator_rule_filenames,\n  const std::vector<std::pair<std::string, std::string>> & extensions,\n  const std::string & package_name)\n: NitrosNode(\n    options,\n    app_yaml_filename,\n    {},  // Use an empty GXF IO information (to be initialized by optimizer)\n    false,\n    false,\n    config_map,\n    {},\n    extension_spec_filenames,\n    generator_rule_filenames,\n    extensions,\n    package_name)\n{\n}\n\nNitrosNode::NitrosNode(\n  const rclcpp::NodeOptions & options,\n  const std::string & app_yaml_filename,\n  const NitrosPublisherSubscriberConfigMap & config_map,\n  const std::vector<std::string> & extension_spec_preset_names,\n  const std::vector<std::string> & extension_spec_filenames,\n  const std::vector<std::string> & generator_rule_filenames,\n  const std::vector<std::pair<std::string, std::string>> & extensions,\n  const std::string & package_name)\n: NitrosNode(\n    options,\n    app_yaml_filename,\n    {},  // Use an empty GXF IO information (to be initialized by optimizer)\n    false,\n    false,\n    config_map,\n    extension_spec_preset_names,\n    extension_spec_filenames,\n    generator_rule_filenames,\n    extensions,\n    package_name)\n{\n}\n\nNitrosNode::NitrosNode(\n  const rclcpp::NodeOptions & options,\n  const std::string & app_yaml_filename,\n  const GraphIOGroupSupportedDataTypesInfoList & gxf_io_group_info_list,\n  const bool use_custom_io_group_info_list,\n  const bool use_raw_graph_no_optimizer,\n  const NitrosPublisherSubscriberConfigMap & config_map,\n  const std::vector<std::string> & extension_spec_preset_names,\n  const std::vector<std::string> & extension_spec_filenames,\n  const std::vector<std::string> & generator_rule_filenames,\n  const std::vector<std::pair<std::string, std::string>> & extensions,\n  const std::string & package_name)\n: Node(\"NitrosNode\", options),\n  gxf_io_group_info_list_(gxf_io_group_info_list),\n  use_custom_io_group_info_list_(use_custom_io_group_info_list),\n  config_map_(config_map),\n  app_yaml_filename_(app_yaml_filename),\n  extension_spec_preset_names_(extension_spec_preset_names),\n  extension_spec_filenames_(extension_spec_filenames),\n  generator_rule_filenames_(generator_rule_filenames),\n  extensions_(extensions),\n  package_name_(package_name),\n  use_raw_graph_no_optimizer_(use_raw_graph_no_optimizer),\n  nitros_context_ptr_(std::make_shared<NitrosContext>())\n{\n  RCLCPP_INFO(get_logger(), \"[NitrosNode] Initializing NitrosNode\");\n\n  // Set extension log level from an user-specified parameter\n  nitros_context_ptr_->setExtensionLogSeverity(\n    static_cast<gxf_severity_t>(declare_parameter<uint16_t>(\n      \"extension_log_level\",\n      gxf_severity_t::GXF_SEVERITY_WARNING)));\n\n  // This line triggers the creation of a type adapter context that\n  // loads a GXF graph containing common components accessible by\n  // all NitrosNode subclasses created in the same process.\n  GetTypeAdapterNitrosContext();\n\n  nitros_context_ptr_->setNode(this);\n\n  // Create an NITROS type manager for the node\n  diagnostics_config_ = std::make_shared<NitrosDiagnosticsConfig>();\n\n  // Setup ROS parameters for NITROS diagnostics\n  diagnostics_config_->enable_node_time_diagnostics = declare_parameter<bool>(\n    \"enable_node_time_diagnostics\", false);\n  diagnostics_config_->enable_msg_time_diagnostics = declare_parameter<bool>(\n    \"enable_msg_time_diagnostics\", false);\n  diagnostics_config_->enable_increasing_msg_time_diagnostics = declare_parameter<bool>(\n    \"enable_increasing_msg_time_diagnostics\", false);\n  diagnostics_config_->enable_all_diagnostics = declare_parameter<bool>(\n    \"enable_all_diagnostics\", false);\n  if (diagnostics_config_->enable_all_diagnostics) {\n    diagnostics_config_->enable_node_time_diagnostics = true;\n    diagnostics_config_->enable_msg_time_diagnostics = true;\n    diagnostics_config_->enable_increasing_msg_time_diagnostics = true;\n  }\n\n  const char * enable_global_diagnostics = std::getenv(\"ENABLE_GLOBAL_NITROS_DIAGNOSTICS\");\n  std::string global_env_value;\n\n  if (enable_global_diagnostics != nullptr) {\n    global_env_value = std::string(enable_global_diagnostics);\n  } else {\n    global_env_value = \"\";\n  }\n\n  if (global_env_value == \"True\" ||\n    global_env_value == \"TRUE\" ||\n    global_env_value == \"true\" ||\n    global_env_value == \"1\")\n  {\n    diagnostics_config_->enable_all_topic_diagnostics = true;\n  }\n\n  if (\n    diagnostics_config_->enable_all_topic_diagnostics ||\n    diagnostics_config_->enable_node_time_diagnostics ||\n    diagnostics_config_->enable_msg_time_diagnostics ||\n    diagnostics_config_->enable_increasing_msg_time_diagnostics)\n  {\n    diagnostics_config_->enable_diagnostics = true;\n    diagnostics_config_->diagnostics_publish_rate = declare_parameter<float>(\n      \"diagnostics_publish_rate\",\n      1.0);\n    diagnostics_config_->filter_window_size =\n      declare_parameter<int>(\"filter_window_size\", 100);\n    diagnostics_config_->jitter_tolerance_us =\n      declare_parameter<int>(\"jitter_tolerance_us\", 5000.0);\n    // List of expected topic names\n    std::vector<std::string> topics_name_list =\n      declare_parameter<std::vector<std::string>>(\"topics_list\", std::vector<std::string>());\n    // List of fps for each expected topic\n    std::vector<double> expected_fps_list =\n      declare_parameter<std::vector<double>>(\"expected_fps_list\", std::vector<double>());\n    if (topics_name_list.size() !=\n      expected_fps_list.size())\n    {\n      std::stringstream error_msg;\n      error_msg << \"[NitrosNode] topics_name_list and expected_fps_list do not have the same size\";\n      RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n    // Populate the topic_name -> expected time difference\n    for (size_t i = 0; i < topics_name_list.size(); i++) {\n      diagnostics_config_->topic_name_expected_dt_map[topics_name_list[i]] = 1000000 /\n        expected_fps_list[i];\n    }\n  }\n\n  // Data type negotiation duration\n  type_negotiation_duration_s_ =\n    declare_parameter<int>(\"type_negotiation_duration_s\", 1);\n  RCLCPP_DEBUG(\n    get_logger(),\n    \"[NitrosNode] Type negotiation duration (seconds) = %ld\",\n    type_negotiation_duration_s_);\n\n  if (use_raw_graph_no_optimizer_) {\n    graph_namespace_ = \"\";\n    RCLCPP_INFO(get_logger(), \"[NitrosNode] Enabled to use raw graph(s)\");\n  } else {\n    // Genearate a random unique namespace for the underlying GXF graph\n    graph_namespace_ = GenerateRandomString(10);\n  }\n  nitros_context_ptr_->setGraphNamespace(graph_namespace_);\n  RCLCPP_DEBUG(get_logger(), \"[NitrosNode] Namespace = %s\", graph_namespace_.c_str());\n\n  package_share_directory_ =\n    ament_index_cpp::get_package_share_directory(package_name_);\n  RCLCPP_DEBUG(\n    get_logger(),\n    \"[NitrosNode] Package's directory: %s\", package_share_directory_.c_str());\n\n  nitros_package_share_directory_ =\n    ament_index_cpp::get_package_share_directory(\"isaac_ros_nitros\");\n  RCLCPP_DEBUG(\n    get_logger(),\n    \"[NitrosNode] Nitros's directory: %s\", nitros_package_share_directory_.c_str());\n\n  // Create a frame_id map for the node\n  frame_id_map_ptr_ = std::make_shared<std::map<ComponentKey, std::string>>();\n\n  // Create an NITROS type manager for the node\n  nitros_type_manager_ = std::make_shared<NitrosTypeManager>(this);\n}\n\nNitrosContext & NitrosNode::getNitrosContext()\n{\n  return *nitros_context_ptr_;\n}\n\nvoid NitrosNode::startNitrosNode()\n{\n  RCLCPP_INFO(get_logger(), \"[NitrosNode] Starting NitrosNode\");\n\n  // Process user-custom per-topic qos and format parameters\n  for (auto & config_pair : config_map_) {\n    const std::string topic_name = config_pair.second.topic_name;\n\n    // QoS setting\n    const std::string topic_qos_parameter_name = topic_name + \"_qos_depth\";\n    const int topic_qos_depth = declare_parameter<int>(\n      topic_qos_parameter_name, -1);\n    if (topic_qos_depth > 0) {\n      RCLCPP_INFO(\n        get_logger(),\n        \"[NitrosNode] Overriding QoS depth for topic \\\"%s\\\" to %d\",\n        topic_name.c_str(), topic_qos_depth);\n      config_pair.second.qos = rclcpp::QoS(topic_qos_depth);\n    }\n\n    // Data format setting\n    const std::string topic_format_parameter_name = topic_name + \"_nitros_format\";\n    const std::string topic_nitros_format = declare_parameter<std::string>(\n      topic_format_parameter_name, \"\");\n    if (!topic_nitros_format.empty()) {\n      // Validate if the given format string is valid and supported\n      if (!nitros_type_manager_->hasFormat(topic_nitros_format)) {\n        std::stringstream error_msg;\n        error_msg << \"[NitrosNode] The given pinning data format \\\"\" <<\n          topic_nitros_format << \"\\\" for topic \\\"\" << topic_name << \"\\\" is invalid\";\n        RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n        throw std::runtime_error(error_msg.str().c_str());\n      }\n\n      RCLCPP_INFO(\n        get_logger(),\n        \"[NitrosNode] Pinning format for topic \\\"%s\\\" to \\\"%s\\\"\",\n        topic_name.c_str(), topic_nitros_format.c_str());\n      config_pair.second.compatible_data_format = topic_nitros_format;\n      config_pair.second.use_compatible_format_only = true;\n    }\n  }\n\n  // Initialize the GXF graph optimizer\n\n  // Load built-in preset extension specs\n  RCLCPP_DEBUG(get_logger(), \"[NitrosNode] Loading built-in preset extension specs\");\n  for (const std::string & preset_name : PRESET_EXTENSION_SPEC_NAMES) {\n    auto load_result =\n      optimizer_.loadPresetExtensionSpecs(preset_name);\n    if (!load_result) {\n      std::stringstream error_msg;\n      error_msg << \"[NitrosNode] loadPresetExtensionSpecs Error: \" <<\n        GxfResultStr(load_result.error());\n      RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n\n  // Load built-in extension specs\n  RCLCPP_DEBUG(get_logger(), \"[NitrosNode] Loading built-in extension specs\");\n  for (const std::string & filename : BUILTIN_EXTENSION_SPEC_FILENAMES) {\n    auto load_result =\n      optimizer_.loadExtensionSpecs(nitros_package_share_directory_ + \"/\" + filename);\n    if (!load_result) {\n      std::stringstream error_msg;\n      error_msg << \"[NitrosNode] loadExtensionSpecs Error: \" << GxfResultStr(load_result.error());\n      RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n\n  // Load preset extension specs\n  RCLCPP_DEBUG(get_logger(), \"[NitrosNode] Loading preset extension specs\");\n  for (const std::string & preset_name : extension_spec_preset_names_) {\n    auto load_result =\n      optimizer_.loadPresetExtensionSpecs(preset_name);\n    if (!load_result) {\n      std::stringstream error_msg;\n      error_msg << \"[NitrosNode] loadPresetExtensionSpecs Error: \" <<\n        GxfResultStr(load_result.error());\n      RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n\n  // Load extension specs\n  RCLCPP_DEBUG(get_logger(), \"[NitrosNode] Loading extension specs\");\n  for (const std::string & filename : extension_spec_filenames_) {\n    auto load_result = optimizer_.loadExtensionSpecs(package_share_directory_ + \"/\" + filename);\n    if (!load_result) {\n      std::stringstream error_msg;\n      error_msg << \"[NitrosNode] loadExtensionSpecs Error: \" << GxfResultStr(load_result.error());\n      RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n\n  // Load generator rules\n  RCLCPP_DEBUG(get_logger(), \"[NitrosNode] Loading generator rules\");\n  for (const std::string & filename : generator_rule_filenames_) {\n    auto load_result = optimizer_.loadRules(package_share_directory_ + \"/\" + filename);\n    if (!load_result) {\n      std::stringstream error_msg;\n      error_msg << \"[NitrosNode] loadRules Error: \" << GxfResultStr(load_result.error());\n      RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n\n  // Load required extension files\n  RCLCPP_INFO(get_logger(), \"[NitrosNode] Loading extensions\");\n  std::vector<std::pair<std::string, std::string>> combined_extensions = BUILTIN_EXTENSIONS;\n  combined_extensions.insert(\n    combined_extensions.end(),\n    extensions_.begin(),\n    extensions_.end());\n  gxf_result_t code;\n  for (const auto & extension_pair : combined_extensions) {\n    const std::string package_directory =\n      ament_index_cpp::get_package_share_directory(extension_pair.first);\n    code = nitros_context_ptr_->loadExtension(package_directory, extension_pair.second);\n    if (code != GXF_SUCCESS) {\n      std::stringstream error_msg;\n      error_msg << \"[NitrosNode] loadExtensions Error: \" << GxfResultStr(code);\n      RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n  // Load extensions of the registered NITROS types\n  nitros_type_manager_->loadExtensions();\n\n  if (!use_raw_graph_no_optimizer_) {\n    // Load graph\n    RCLCPP_INFO(get_logger(), \"[NitrosNode] Loading graph to the optimizer\");\n    auto load_graph_result =\n      optimizer_.loadGraph(package_share_directory_ + \"/\" + app_yaml_filename_);\n    if (!load_graph_result) {\n      std::stringstream error_msg;\n      error_msg << \"[NitrosNode] optimizer_.loadGraph Error: \" <<\n        GxfResultStr(load_graph_result.error());\n      RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n\n    // Run optimization\n    RCLCPP_INFO(get_logger(), \"[NitrosNode] Running optimization\");\n    auto optimize_result =\n      optimizer_.optimize(nvidia::gxf::optimizer::gxf_cog_factor_t::THROUGHPUT);\n    if (!optimize_result) {\n      std::stringstream error_msg;\n      error_msg << \"[NitrosNode] optimize Error: \" << GxfResultStr(optimize_result.error());\n      RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n\n  if (!use_custom_io_group_info_list_) {\n    // Use the optimizer to get IO groups for the underlying graph\n    RCLCPP_INFO(\n      get_logger(),\n      \"[NitrosNode] Obtaining graph IO group info from the optimizer\");\n    auto maybe_gxf_io_group_info_list = optimizer_.exportGraphIOGroupSupportedDataTypesInfoList();\n    if (!maybe_gxf_io_group_info_list) {\n      std::stringstream error_msg;\n      error_msg << \"[NitrosNode] exportGraphIOGroupSupportedDataTypesInfoList Error: \" <<\n        GxfResultStr(maybe_gxf_io_group_info_list.error());\n      RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n    gxf_io_group_info_list_ = maybe_gxf_io_group_info_list.value();\n  } else {\n    RCLCPP_INFO(get_logger(), \"[NitrosNode] Use the given graph IO group info\");\n  }\n\n  // Print all the supported data formats\n  RCLCPP_DEBUG(get_logger().get_child(\"NitrosNode\"), \"Supported data format combinations:\");\n  for (size_t group_index = 0; group_index < gxf_io_group_info_list_.size(); group_index++) {\n    RCLCPP_DEBUG(get_logger().get_child(\"NitrosNode\"), \"#%ld I/O group:\", group_index + 1);\n    const auto & gxf_io_group = gxf_io_group_info_list_[group_index];\n    for (size_t combo_index = 0;\n      combo_index < gxf_io_group.supported_data_types.size();\n      combo_index++)\n    {\n      const auto & supported_data_type_map = gxf_io_group.supported_data_types[combo_index];\n      RCLCPP_DEBUG(\n        get_logger().get_child(\"NitrosNode\"), \"\\t#%ld format combination:\",\n        combo_index + 1);\n      // ingress ports\n      for (const auto & ingress_comp_info : gxf_io_group.ingress_infos) {\n        const std::string component_key =\n          gxf::optimizer::GenerateComponentKey(ingress_comp_info);\n        const std::string supported_format = supported_data_type_map.at(component_key);\n        RCLCPP_DEBUG(\n          get_logger().get_child(\"NitrosNode\"), \"\\t\\t[in]\\t%s: %s\",\n          component_key.c_str(), supported_format.c_str());\n      }\n      // egress ports\n      for (const auto & egress_comp_info : gxf_io_group.egress_infos) {\n        const std::string component_key =\n          gxf::optimizer::GenerateComponentKey(egress_comp_info);\n        const std::string supported_format = supported_data_type_map.at(component_key);\n        RCLCPP_DEBUG(\n          get_logger().get_child(\"NitrosNode\"), \"\\t\\t[out]\\t%s: %s\",\n          component_key.c_str(), supported_format.c_str());\n      }\n    }\n  }\n\n  // Create publishers and subscribers from each GXF ingress-egress group\n  RCLCPP_DEBUG(get_logger(), \"[NitrosNode] Creating negotiated publishers/subscribers\");\n  for (const auto & gxf_io_group : gxf_io_group_info_list_) {\n    nitros_pub_sub_groups_.emplace_back(\n      std::make_shared<NitrosPublisherSubscriberGroup>(\n        *this, nitros_context_ptr_->getContext(),\n        nitros_type_manager_, gxf_io_group, config_map_, frame_id_map_ptr_, *diagnostics_config_));\n  }\n\n  // Start negotiation\n  RCLCPP_INFO(get_logger(), \"[NitrosNode] Starting negotiation...\");\n  for (auto & nitros_pub_sub_group : nitros_pub_sub_groups_) {\n    nitros_pub_sub_group->start();\n  }\n\n  // Set the negotiation timer\n  negotiation_timer_ = create_wall_timer(\n    std::chrono::seconds(type_negotiation_duration_s_),\n    [this]() -> void {\n      negotiation_timer_->cancel();\n      postNegotiationCallback();\n    });\n}\n\nvoid NitrosNode::postNegotiationCallback()\n{\n  RCLCPP_INFO(get_logger(), \"[NitrosNode] Starting post negotiation setup\");\n\n  // Get negotiated data types\n  RCLCPP_INFO(get_logger(), \"[NitrosNode] Getting data format negotiation results\");\n  std::vector<GraphIOGroupDataTypeConfigurations> configs;\n  for (const auto & nitros_pub_sub_group : nitros_pub_sub_groups_) {\n    nitros_pub_sub_group->postNegotiationCallback();\n    auto io_group_config = nitros_pub_sub_group->getDataFormatConfigurations();\n    configs.push_back(io_group_config);\n  }\n\n  std::string temp_yaml_filename;\n  if (!use_raw_graph_no_optimizer_) {\n    std::string node_graph_export_directory =\n      std::string(kGraphExportDirectory) + \"/\" + graph_namespace_;\n    std::filesystem::create_directories(node_graph_export_directory);\n    // Get the graph with the data formats assigned\n    RCLCPP_INFO(\n      get_logger(),\n      \"[NitrosNode] Exporting the final graph based on the negotiation results\");\n    auto export_result = optimizer_.exportGraphToFiles(\n      configs, node_graph_export_directory, graph_namespace_, graph_namespace_);\n    if (!export_result) {\n      std::stringstream error_msg;\n      error_msg << \"[NitrosNode] exportGraphToFiles Error: \" <<\n        GxfResultStr(export_result.error());\n      RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n    temp_yaml_filename = node_graph_export_directory + \"/\" + graph_namespace_ + \".yaml\";\n  } else {\n    // Use the original graph intact\n    temp_yaml_filename = package_share_directory_ + \"/\" + app_yaml_filename_;\n  }\n\n  RCLCPP_INFO(\n    get_logger(),\n    \"[NitrosNode] Wrote the final top level YAML graph to \\\"%s\\\"\", temp_yaml_filename.c_str());\n\n  // Load the application graph\n  gxf_result_t code;\n\n  // Call the user's pre-load-graph initialization callback\n  RCLCPP_DEBUG(get_logger(), \"[NitrosNode] Calling user's pre-load-graph callback\");\n  preLoadGraphCallback();\n\n  // Load the new YAML graph that's configured with the selected data types\n  RCLCPP_INFO(get_logger(), \"[NitrosNode] Loading application\");\n  code = nitros_context_ptr_->loadApplication(temp_yaml_filename);\n  if (code != GXF_SUCCESS) {\n    std::stringstream error_msg;\n    error_msg << \"[NitrosNode] LoadApplication Error: \" << GxfResultStr(code);\n    RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Get the pointers to the ingress/egress components in the loaded GXF graph and set\n  // them to the coresponding Nitros publishers/subscribers\n  RCLCPP_DEBUG(get_logger(), \"[NitrosNode] Linking Nitros pub/sub to the loaded application\");\n  for (auto & pub_sub_group_ptr : nitros_pub_sub_groups_) {\n    // Subscribers\n    for (auto & sub_ptr : pub_sub_group_ptr->getNitrosSubscribers()) {\n      auto comp_info = sub_ptr->getComponentInfo();\n      void * pointer;\n      code = nitros_context_ptr_->getComponentPointer(\n        comp_info.entity_name,\n        comp_info.component_name,\n        comp_info.component_type_name,\n        &pointer);\n      if (code != GXF_SUCCESS) {\n        std::stringstream error_msg;\n        error_msg <<\n          \"[NitrosNode] Failed to get the pointer of \" <<\n          comp_info.component_type_name.c_str() << \" (\" <<\n          comp_info.entity_name.c_str() << \"/\" <<\n          comp_info.component_name.c_str() << \") for linking a NitrosSubscriber: \" <<\n          GxfResultStr(code);\n        RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n        throw std::runtime_error(error_msg.str().c_str());\n      }\n      RCLCPP_DEBUG(\n        get_logger(),\n        \"[NitrosNode] Setting up a subscriber's underyling receiver (pointer=%p)\", pointer);\n      sub_ptr->setReceiverPointer(pointer);\n      sub_ptr->setReceiverPolicy(0);\n      const std::string topic_qos_parameter_name = sub_ptr->getConfig().topic_name + \"_qos_depth\";\n      const int custom_sub_qos_depth = get_parameter(topic_qos_parameter_name).as_int();\n      if (custom_sub_qos_depth > 0) {\n        sub_ptr->setReceiverCapacity(custom_sub_qos_depth);\n      }\n      if (heartbeat_eid_ == -1) {\n        nitros_context_ptr_->getEid(comp_info.entity_name, heartbeat_eid_);\n      }\n    }\n\n    // Publishers\n    for (auto & pub_ptr : pub_sub_group_ptr->getNitrosPublishers()) {\n      auto comp_info = pub_ptr->getComponentInfo();\n      if (comp_info.component_type_name == kGxfVaultComponentTypeName) {\n        void * pointer = nullptr;\n        code = nitros_context_ptr_->getComponentPointer(\n          comp_info.entity_name,\n          comp_info.component_name,\n          comp_info.component_type_name,\n          &pointer);\n        pub_ptr->setVaultPointer(pointer);\n        RCLCPP_DEBUG(\n          get_logger(), \"[NitrosNode] Set a publisher's pointer: %p\", pointer);\n        pub_ptr->startGxfVaultPeriodicPollingTimer();\n      } else if (comp_info.component_type_name == kGxfMessageRelayComponentTypeName) {\n        // Set egress component's pointer\n        void * pointer = nullptr;\n        code = nitros_context_ptr_->getComponentPointer(\n          comp_info.entity_name,\n          comp_info.component_name,\n          comp_info.component_type_name,\n          &pointer);\n        pub_ptr->setMessageRelayPointer(pointer);\n        pub_ptr->enableNitrosPublisherWaitable();\n\n        // Set egress component's callback function\n        void * cb_func_pointer = &pub_ptr->getGxfMessageRelayCallbackFunc();\n        nitros_context_ptr_->setParameterInt64(\n          comp_info.entity_name, comp_info.component_type_name,\n          \"callback_address\",\n          reinterpret_cast<uintptr_t>(cb_func_pointer));\n        RCLCPP_DEBUG(\n          get_logger(), \"[NitrosNode] Set a publisher's callback pointer: %p\", cb_func_pointer);\n      } else {\n        std::stringstream error_msg;\n        error_msg <<\n          \"[NitrosNode] Unsupported egress component type: \" <<\n          comp_info.component_type_name.c_str() << \" (\" <<\n          comp_info.entity_name.c_str() << \"/\" <<\n          comp_info.component_name.c_str() << \")\";\n        RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n        throw std::runtime_error(error_msg.str().c_str());\n      }\n      if (heartbeat_eid_ == -1) {\n        nitros_context_ptr_->getEid(comp_info.entity_name, heartbeat_eid_);\n      }\n    }\n  }\n\n  // Call the user's post-load-graph initialization callback\n  RCLCPP_DEBUG(get_logger(), \"[NitrosNode] Calling user's post-load-graph callback\");\n  postLoadGraphCallback();\n\n  RCLCPP_INFO(get_logger(), \"[NitrosNode] Initializing and running GXF graph\");\n  code = nitros_context_ptr_->runGraphAsync();\n  if (code != GXF_SUCCESS) {\n    std::stringstream error_msg;\n    error_msg << \"[NitrosNode] runGraphAsync Error: \" << GxfResultStr(code);\n    RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Set the timer for checking the status of the underlying graph\n  if (heartbeat_eid_ != -1) {\n    RCLCPP_DEBUG(\n      get_logger(),\n      \"[NitrosNode] Starting a heartbeat timer (eid=%ld)\",\n      heartbeat_eid_);\n    gxf_heartbeat_timer_ = create_wall_timer(\n      std::chrono::milliseconds(1000),\n      [this]() -> void {\n        gxfHeartbeatCallback();\n      });\n  }\n\n  // Enable NitrosSbuscriber's callbacks\n  RCLCPP_DEBUG(get_logger(), \"[NitrosNode] Enabling NitrosSubscriber's callbacks\");\n  for (auto & pub_sub_group_ptr : nitros_pub_sub_groups_) {\n    for (auto & sub_ptr : pub_sub_group_ptr->getNitrosSubscribers()) {\n      sub_ptr->setIsGxfRunning(true);\n    }\n  }\n\n  RCLCPP_INFO(get_logger(), \"[NitrosNode] Node was started\");\n}\n\nvoid NitrosNode::gxfHeartbeatCallback()\n{\n  std::stringstream nvtx_tag_name;\n  nvtx_tag_name <<\n    \"[\" << get_name() << \"] NitrosNode::gxfHeartbeatCallback()\";\n  nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_GRAY);\n\n  if (heartbeat_eid_ == -1) {\n    RCLCPP_ERROR(get_logger(), \"[NitrosNode] The heartbeat entity was not set\");\n    nvtxRangePopWrapper();\n    rclcpp::shutdown();\n  }\n\n  // Check the status of the target heartbeat entity.\n  // gxf_entity_status_t is expected to be GXF_ENTITY_STATUS_STARTED (2)\n  // when the graph is running\n  gxf_entity_status_t entity_status;\n  gxf_result_t code =\n    GxfEntityGetStatus(nitros_context_ptr_->getContext(), heartbeat_eid_, &entity_status);\n  if (code == GXF_ENTITY_NOT_FOUND) {\n    RCLCPP_WARN(\n      get_logger(),\n      \"[NitrosNode] The heartbeat entity (eid=%ld) was stopped. \"\n      \"The graph may have been terminated.\",\n      heartbeat_eid_);\n    nvtxRangePopWrapper();\n    rclcpp::shutdown();\n  } else if (code != GXF_SUCCESS) {\n    RCLCPP_WARN(\n      get_logger(),\n      \"[NitrosNode] Failed to get the heartbeat entity (eid=%ld) status: %s\",\n      heartbeat_eid_,\n      GxfResultStr(code));\n    nvtxRangePopWrapper();\n    rclcpp::shutdown();\n  }\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\n      std::string(\"GXFMonitor.\") + std::string(get_logger().get_name())),\n    \"Heartbeat entity (eid=%ld) status: %d\",\n    heartbeat_eid_,\n    static_cast<int>(entity_status));\n  nvtxRangePopWrapper();\n}\n\nvoid NitrosNode::preLoadGraphSetParameter(\n  const std::string & entity_name,\n  const std::string & component_name,\n  const std::string & parameter_name,\n  const std::string & value\n)\n{\n  nitros_context_ptr_->preLoadGraphSetParameter(entity_name, component_name, parameter_name, value);\n}\n\nstd::shared_ptr<NitrosPublisher> NitrosNode::findNitrosPublisher(\n  const gxf::optimizer::ComponentInfo & comp_info)\n{\n  for (auto & nitros_pub_sub_group : nitros_pub_sub_groups_) {\n    auto nitros_pub = nitros_pub_sub_group->findNitrosPublisher(comp_info);\n    if (nitros_pub != nullptr) {\n      return nitros_pub;\n    }\n  }\n  return nullptr;\n}\n\nstd::shared_ptr<NitrosSubscriber> NitrosNode::findNitrosSubscriber(\n  const gxf::optimizer::ComponentInfo & comp_info)\n{\n  {\n    for (auto & nitros_pub_sub_group : nitros_pub_sub_groups_) {\n      auto nitros_sub = nitros_pub_sub_group->findNitrosSubscriber(comp_info);\n      if (nitros_sub != nullptr) {\n        return nitros_sub;\n      }\n    }\n    return nullptr;\n  }\n}\n\nstd::string NitrosNode::getNegotiatedDataFormat(const ComponentInfo comp_info) const\n{\n  for (auto & nitros_pub_sub_group : nitros_pub_sub_groups_) {\n    auto nitros_sub = nitros_pub_sub_group->findNitrosSubscriber(comp_info);\n    if (nitros_sub != nullptr) {\n      return nitros_sub->getNegotiatedDataFormat();\n    }\n    auto nitros_pub = nitros_pub_sub_group->findNitrosPublisher(comp_info);\n    if (nitros_pub != nullptr) {\n      return nitros_pub->getNegotiatedDataFormat();\n    }\n  }\n  RCLCPP_WARN(\n    get_logger(),\n    \"[NitrosNode] Could not recognize the component \\\"%s/%s\\\" (type=\\\"%s\\\") for \"\n    \"getting its negotiated data format\",\n    comp_info.entity_name.c_str(),\n    comp_info.component_name.c_str(),\n    comp_info.component_type_name.c_str());\n  return \"\";\n}\n\nstd::string NitrosNode::getFinalDataFormat(const ComponentInfo comp_info) const\n{\n  for (auto & nitros_pub_sub_group : nitros_pub_sub_groups_) {\n    auto nitros_sub = nitros_pub_sub_group->findNitrosSubscriber(comp_info);\n    if (nitros_sub != nullptr) {\n      return nitros_sub->getFinalDataFormat();\n    }\n    auto nitros_pub = nitros_pub_sub_group->findNitrosPublisher(comp_info);\n    if (nitros_pub != nullptr) {\n      return nitros_pub->getFinalDataFormat();\n    }\n  }\n  RCLCPP_WARN(\n    get_logger(),\n    \"[NitrosNode] Could not recognize the component \\\"%s/%s\\\" (type=\\\"%s\\\") for \"\n    \"getting its final data format\",\n    comp_info.entity_name.c_str(),\n    comp_info.component_name.c_str(),\n    comp_info.component_type_name.c_str());\n  return \"\";\n}\n\nvoid NitrosNode::setUseCustomIOGroupInfoList(const bool use_custom)\n{\n  use_custom_io_group_info_list_ = use_custom;\n}\n\nvoid NitrosNode::addExtensionSpecFilename(std::string package_relative_filepath)\n{\n  extension_spec_filenames_.push_back(package_relative_filepath);\n}\n\nvoid NitrosNode::addGeneratorRuleFilename(std::string package_relative_filepath)\n{\n  generator_rule_filenames_.push_back(package_relative_filepath);\n}\n\nvoid NitrosNode::setAppYamlFilename(std::string package_relative_filepath)\n{\n  app_yaml_filename_ = package_relative_filepath;\n}\n\nvoid NitrosNode::setFrameIdSource(std::string source_frame_id_map_key, std::string frame_id)\n{\n  if (frame_id_map_ptr_ == nullptr) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[NitrosNode] setFrameIdSource was called before frame_id_map_ptr_ was initialized\";\n    RCLCPP_ERROR(get_logger(), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  (*frame_id_map_ptr_.get())[source_frame_id_map_key] = frame_id;\n  RCLCPP_DEBUG(\n    get_logger(),\n    \"[NitrosNode] Set frame id for key %s to %s\", source_frame_id_map_key.c_str(),\n    (*frame_id_map_ptr_.get())[source_frame_id_map_key].c_str());\n}\n\nNitrosNode::~NitrosNode()\n{\n  RCLCPP_INFO(get_logger(), \"[NitrosNode] Terminating the running application\");\n  nitros_context_ptr_->destroy();\n  RCLCPP_INFO(get_logger(), \"[NitrosNode] Application termination done\");\n}\n\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros/src/nitros_publisher.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <stdexcept>\n#include <sstream>\n#include <cstdio>\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\n#include \"isaac_ros_nitros/nitros_publisher.hpp\"\n\n#include \"rclcpp/logger.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nNitrosPublisherWaitable::NitrosPublisherWaitable(\n  rclcpp::Node & node,\n  NitrosPublisher & nitros_publisher)\n: rclcpp::Waitable(),\n  node_(node),\n  nitros_publisher_(nitros_publisher)\n{}\n\nvoid NitrosPublisherWaitable::trigger()\n{\n  std::stringstream nvtx_tag_name;\n  nvtx_tag_name <<\n    \"[\" << node_.get_name() << \"] NitrosPublisherWaitable::trigger(\" <<\n    nitros_publisher_.getConfig().topic_name << \")\";\n  nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_BLUE);\n\n  std::lock_guard<std::mutex> lock(guard_condition_mutex_);\n  is_trigger_pending_ = true;\n  guard_condition_.trigger();\n\n  nvtxRangePopWrapper();\n}\n\nbool NitrosPublisherWaitable::is_ready(rcl_wait_set_t * wait_set)\n{\n  std::stringstream nvtx_tag_name;\n  nvtx_tag_name <<\n    \"[\" << node_.get_name() << \"] NitrosPublisherWaitable::is_ready(\" <<\n    nitros_publisher_.getConfig().topic_name << \")\";\n  nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_BLUE);\n\n  std::lock_guard<std::mutex> lock(guard_condition_mutex_);\n  for (size_t ii = 0; ii < wait_set->size_of_guard_conditions; ++ii) {\n    auto rcl_guard_condition = wait_set->guard_conditions[ii];\n\n    if (nullptr == rcl_guard_condition) {\n      continue;\n    }\n\n    if (&guard_condition_.get_rcl_guard_condition() == rcl_guard_condition) {\n      nvtx_tag_name << \" = true\";\n      nvtxMarkExWrapper(nvtx_tag_name.str().c_str(), CLR_BLUE);\n      nvtxRangePopWrapper();\n      return true;\n    }\n  }\n\n  nvtx_tag_name << \" = false\";\n  nvtxMarkExWrapper(nvtx_tag_name.str().c_str(), CLR_BLUE);\n  nvtxRangePopWrapper();\n  return false;\n}\n\nvoid NitrosPublisherWaitable::execute(std::shared_ptr<void> & data)\n{\n  (void)data;  // unused\n  std::stringstream nvtx_tag_name;\n  nvtx_tag_name <<\n    \"[\" << node_.get_name() << \"] NitrosPublisherWaitable::execute(\" <<\n    nitros_publisher_.getConfig().topic_name << \")\";\n  nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_BLUE);\n\n  nitros_publisher_.extractMessagesFromGXF();\n  is_trigger_pending_ = false;\n\n  nvtxRangePopWrapper();\n}\n\nvoid NitrosPublisherWaitable::add_to_wait_set(rcl_wait_set_t * wait_set)\n{\n  std::lock_guard<std::mutex> lock(guard_condition_mutex_);\n\n  if (is_trigger_pending_) {\n    guard_condition_.trigger();\n  }\n\n  guard_condition_.add_to_wait_set(*wait_set);\n}\n\nNitrosPublisher::NitrosPublisher(\n  rclcpp::Node & node,\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n  const gxf::optimizer::ComponentInfo & gxf_component_info,\n  const std::vector<std::string> & supported_data_formats,\n  const NitrosPublisherSubscriberConfig & config,\n  const negotiated::NegotiatedPublisherOptions & negotiated_pub_options)\n: NitrosPublisherSubscriberBase(\n    node, nitros_type_manager, gxf_component_info, supported_data_formats, config)\n{\n  // Bind the callback function to be used by a message relay in GXF graph\n  gxf_message_relay_callback_func_ = std::bind(\n    &NitrosPublisher::gxfMessageRelayCallback,\n    this);\n\n  if (config_.type == NitrosPublisherSubscriberType::NOOP) {\n    return;\n  }\n\n  // Check and throw error if the specified compatible data format is not supported\n  // from the underlying graph\n  throwOnUnsupportedCompatibleDataFormat();\n\n  // Create the compatible data format publisher\n  createCompatiblePublisher();\n\n  if (config_.type == NitrosPublisherSubscriberType::NON_NEGOTIATED) {\n    return;\n  }\n\n  // We don't consider the case of a node being removed after started\n  negotiated::NegotiatedPublisherOptions updated_negotiated_pub_options =\n    negotiated_pub_options;\n  updated_negotiated_pub_options.negotiate_on_subscription_removal = false;\n\n  // Create a negotiated publisher object\n  negotiated_pub_ = std::make_shared<negotiated::NegotiatedPublisher>(\n    node_,\n    compatible_pub_->get_topic_name() + std::string(\"/nitros\"),\n    updated_negotiated_pub_options);\n\n  // Add supported data formats (which also adds the compatible publisher)\n  double weight = 1.0;\n  for (std::string data_format : supported_data_formats_) {\n    weight -= 0.1;\n    addSupportedDataFormat(\n      data_format,\n      weight);\n  }\n\n  // Initialize the cuda stream and event with priority 0, assumes only 1 cuda device on hand with\n  // dev id 0, Might want to make this more flexible in the future\n  CHECK_CUDA_ERROR(\n    ::nvidia::isaac_ros::common::initNamedCudaStream(\n      cuda_stream_, \"nitros_type_adaptation_custom_to_ros\"),\n    \"Error initializing CUDA stream\");\n}\n\nNitrosPublisher::NitrosPublisher(\n  rclcpp::Node & node,\n  const gxf_context_t context,\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n  const gxf::optimizer::ComponentInfo & gxf_component_info,\n  const std::vector<std::string> & supported_data_formats,\n  const NitrosPublisherSubscriberConfig & config,\n  const negotiated::NegotiatedPublisherOptions & negotiated_pub_options)\n: NitrosPublisher(\n    node, nitros_type_manager, gxf_component_info, supported_data_formats, config,\n    negotiated_pub_options)\n{\n  setContext(context);\n}\n\nNitrosPublisher::NitrosPublisher(\n  rclcpp::Node & node,\n  const gxf_context_t context,\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n  const gxf::optimizer::ComponentInfo & gxf_component_info,\n  const std::vector<std::string> & supported_data_formats,\n  const NitrosPublisherSubscriberConfig & config,\n  const negotiated::NegotiatedPublisherOptions & negotiated_pub_options,\n  const NitrosDiagnosticsConfig & diagnostics_config)\n: NitrosPublisher(\n    node, context, nitros_type_manager, gxf_component_info, supported_data_formats, config,\n    negotiated_pub_options)\n{\n  diagnostics_config_ = diagnostics_config;\n\n  if (diagnostics_config_.enable_all_topic_diagnostics ||\n    (diagnostics_config_.enable_diagnostics &&\n    diagnostics_config_.topic_name_expected_dt_map.find(config_.topic_name) !=\n    diagnostics_config_.topic_name_expected_dt_map.end()))\n  {\n    // Initialize statistics variables and message fields\n    initDiagnostics();\n  }\n}\n\nNitrosPublisher::NitrosPublisher(\n  rclcpp::Node & node,\n  const gxf_context_t context,\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n  const std::vector<std::string> & supported_data_formats,\n  const NitrosPublisherSubscriberConfig & config,\n  const NitrosDiagnosticsConfig & diagnostics_config)\n: NitrosPublisher(\n    node, context, nitros_type_manager, {}, supported_data_formats, config,\n    {}, diagnostics_config)\n{}\n\nstd::shared_ptr<negotiated::NegotiatedPublisher> NitrosPublisher::getNegotiatedPublisher()\n{\n  return negotiated_pub_;\n}\n\nvoid NitrosPublisher::addSupportedDataFormat(\n  const std::string & data_format,\n  const double weight)\n{\n  rclcpp::PublisherOptions pub_options;\n  pub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;\n\n  if (!nitros_type_manager_->hasFormat(data_format)) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[NitrosPublisher] Could not identify the supported data foramt: \" <<\n      \"\\\"\" << data_format.c_str() << \"\\\"\";\n    RCLCPP_ERROR(\n      node_.get_logger(), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  if (data_format == config_.compatible_data_format) {\n    nitros_type_manager_->getFormatCallbacks(data_format).addCompatiblePublisherCallback(\n      node_,\n      negotiated_pub_,\n      compatible_pub_,\n      weight\n    );\n\n    RCLCPP_DEBUG(\n      node_.get_logger(),\n      \"[NitrosPublisher] Added a compatible publisher: \"\n      \"topic_name=\\\"%s\\\", data_format=\\\"%s\\\"\",\n      compatible_pub_->get_topic_name(), config_.compatible_data_format.c_str());\n  } else {\n    nitros_type_manager_->getFormatCallbacks(data_format).addPublisherSupportedFormatCallback(\n      node_,\n      negotiated_pub_,\n      weight,\n      config_.qos,\n      pub_options);\n\n    RCLCPP_DEBUG(\n      node_.get_logger(),\n      \"[NitrosPublisher] Added a supported data format: \"\n      \"topic_name=\\\"%s\\\", data_format=\\\"%s\\\"\",\n      compatible_pub_->get_topic_name(), data_format.c_str());\n  }\n}\n\nvoid NitrosPublisher::start()\n{\n  if (config_.type == NitrosPublisherSubscriberType::NEGOTIATED) {\n    negotiated_pub_->start();\n  }\n}\n\nvoid NitrosPublisher::createCompatiblePublisher()\n{\n  rclcpp::PublisherOptions pub_options;\n  pub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;\n\n  if (!nitros_type_manager_->hasFormat(config_.compatible_data_format)) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[NitrosPublisher] Could not identify the compatible data foramt: \" <<\n      \"\\\"\" << config_.compatible_data_format.c_str() << \"\\\"\";\n    RCLCPP_ERROR(\n      node_.get_logger(), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  nitros_type_manager_->getFormatCallbacks(config_.compatible_data_format).\n  createCompatiblePublisherCallback(\n    node_,\n    compatible_pub_,\n    config_.topic_name,\n    config_.qos,\n    pub_options\n  );\n}\n\nvoid NitrosPublisher::postNegotiationCallback()\n{\n  if (config_.type != NitrosPublisherSubscriberType::NEGOTIATED) {\n    return;\n  }\n\n  auto topics_info = negotiated_pub_->get_negotiated_topics_info();\n  if (!topics_info.success || topics_info.negotiated_topics.size() == 0) {\n    negotiated_data_format_ = \"\";\n    RCLCPP_INFO(\n      node_.get_logger(),\n      \"[NitrosPublisher] Negotiation ended with no results\");\n    RCLCPP_INFO(\n      node_.get_logger(),\n      \"[NitrosPublisher] Use only the compatible publisher: \"\n      \"topic_name=\\\"%s\\\", data_format=\\\"%s\\\"\",\n      compatible_pub_->get_topic_name(), config_.compatible_data_format.c_str());\n  } else {\n    negotiated_data_format_ = topics_info.negotiated_topics[0].supported_type_name;\n    RCLCPP_INFO(\n      node_.get_logger(),\n      \"[NitrosPublisher] Use the negotiated data format: \\\"%s\\\"\",\n      negotiated_data_format_.c_str());\n  }\n}\n\nstd::function<void(void)> &\nNitrosPublisher::getGxfMessageRelayCallbackFunc()\n{\n  return gxf_message_relay_callback_func_;\n}\n\nvoid NitrosPublisher::setVaultPointer(void * gxf_vault_ptr)\n{\n  gxf_vault_ptr_ = reinterpret_cast<nvidia::gxf::Vault *>(gxf_vault_ptr);\n}\n\nvoid NitrosPublisher::setMessageRelayPointer(void * gxf_message_relay_ptr)\n{\n  gxf_message_relay_ptr_ =\n    reinterpret_cast<nvidia::isaac_ros::MessageRelay *>(gxf_message_relay_ptr);\n}\n\nvoid NitrosPublisher::startGxfVaultPeriodicPollingTimer()\n{\n  // Set the Vault data polling timer\n  gxf_vault_periodic_polling_timer_ = node_.create_wall_timer(\n    std::chrono::microseconds(100),\n    [this]() -> void {\n      extractMessagesFromGXF();\n    });\n}\n\nvoid NitrosPublisher::enableNitrosPublisherWaitable()\n{\n  auto waitable_interfaces = node_.get_node_waitables_interface();\n  waitable_ = std::make_shared<NitrosPublisherWaitable>(\n    node_,\n    *this\n  );\n  waitable_interfaces->add_waitable(waitable_, nullptr);\n}\n\nvoid NitrosPublisher::extractMessagesFromGXF()\n{\n  std::stringstream nvtx_tag_name;\n  nvtx_tag_name <<\n    \"[\" << node_.get_name() << \"] NitrosPublisher::extractMessagesFromGXF(\" <<\n    config_.topic_name << \")\";\n  nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_MAGENTA);\n\n  if (gxf_vault_ptr_ == nullptr && gxf_message_relay_ptr_ == nullptr) {\n    nvtxRangePopWrapper();\n    return;\n  }\n\n  if (gxf_vault_ptr_ != nullptr) {\n    // Get messages from Vault\n    auto msg_eids = gxf_vault_ptr_->store(100);\n    if (msg_eids.size() > 0) {\n      RCLCPP_DEBUG(\n        node_.get_logger(),\n        \"[NitrosPublisher] Obtained %ld messages from the vault\", msg_eids.size());\n    }\n    for (const auto msg_eid : msg_eids) {\n      publish(msg_eid);\n      gxf_vault_ptr_->free({msg_eid});\n    }\n  } else {\n    // Get messgaes from MessageRelay\n    auto msg_eids = gxf_message_relay_ptr_->store(100);\n    if (msg_eids.size() > 0) {\n      RCLCPP_DEBUG(\n        node_.get_logger(),\n        \"[NitrosPublisher] Obtained %ld messages from the message relay for topic_name=\\\"%s\\\"\",\n        msg_eids.size(), config_.topic_name.c_str());\n    }\n    for (const auto msg_eid : msg_eids) {\n      publish(msg_eid);\n      gxf_message_relay_ptr_->free({msg_eid});\n    }\n  }\n\n  nvtxRangePopWrapper();\n}\n\nvoid NitrosPublisher::gxfMessageRelayCallback()\n{\n  RCLCPP_DEBUG(node_.get_logger(), \"[NitrosPublisher] gxfMessageRelayCallback\");\n  waitable_->trigger();\n}\n\nvoid NitrosPublisher::publish(const int64_t handle)\n{\n  std::string frame_id = \"\";\n  if ((frame_id_map_ptr_ != nullptr) && (!config_.frame_id_source_key.empty())) {\n    if (frame_id_map_ptr_->count(config_.frame_id_source_key) > 0) {\n      frame_id = frame_id_map_ptr_->at(config_.frame_id_source_key);\n      RCLCPP_DEBUG(\n        node_.get_logger(),\n        \"[NitrosPublisher] Associating frame_id=\\\"%s\\\" to an Nitros-typed \"\n        \"message (eid=%ld)\", frame_id.c_str(), handle);\n    }\n  }\n\n  std::shared_ptr<cudaEvent_t> event_ = std::shared_ptr<cudaEvent_t>(\n    new cudaEvent_t,\n    [](cudaEvent_t * p) {\n      if (p) {\n        cudaEventDestroy(*p);\n        delete p;\n      }\n    }\n  );\n  cudaError_t event_result = cudaEventCreate(event_.get());\n  CHECK_CUDA_ERROR(event_result, \"Failure creating CUDA event\");\n\n  NitrosTypeBase nitros_msg {\n    handle, negotiated_data_format_,\n    config_.compatible_data_format, frame_id, cuda_stream_};\n\n  publish(nitros_msg);\n}\n\nvoid NitrosPublisher::publish(const int64_t handle, const std_msgs::msg::Header & ros_header)\n{\n  RCLCPP_DEBUG(\n    node_.get_logger(),\n    \"[NitrosPublisher] Publishing an Nitros-typed message with timestamps updated (eid=%ld)\",\n    handle);\n\n  bool is_timestamp_updated = false;\n\n  auto msg_entity = nvidia::gxf::Entity::Shared(context_, handle);\n\n  uint64_t input_timestamp =\n    ros_header.stamp.sec * static_cast<uint64_t>(1e9) +\n    ros_header.stamp.nanosec;\n\n  auto maybe_input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>();\n  if (maybe_input_timestamp) {\n    maybe_input_timestamp.value()->acqtime = input_timestamp;\n    is_timestamp_updated = true;\n  }\n\n  maybe_input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (maybe_input_timestamp) {\n    maybe_input_timestamp.value()->acqtime = input_timestamp;\n    is_timestamp_updated = true;\n  }\n\n  if (!is_timestamp_updated) {\n    RCLCPP_ERROR(\n      node_.get_logger(),\n      \"[NitrosPublisher] Failed to update timestamps in a message entity (eid=%ld) as \"\n      \"no Timestamp component was found\",\n      handle);\n  }\n\n  publish(handle);\n}\n\nvoid NitrosPublisher::publish(\n  NitrosTypeBase & base_msg,\n  const std_msgs::msg::Header & ros_header)\n{\n  RCLCPP_DEBUG(\n    node_.get_logger(),\n    \"[NitrosPublisher] Publishing an Nitros-typed message with timestamps updated (eid=%ld)\",\n    base_msg.handle);\n\n  bool is_timestamp_updated = false;\n\n  auto msg_entity = nvidia::gxf::Entity::Shared(context_, base_msg.handle);\n\n  uint64_t input_timestamp =\n    ros_header.stamp.sec * static_cast<uint64_t>(1e9) +\n    ros_header.stamp.nanosec;\n\n  auto maybe_input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>();\n  if (maybe_input_timestamp) {\n    maybe_input_timestamp.value()->acqtime = input_timestamp;\n    is_timestamp_updated = true;\n  }\n\n  maybe_input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (maybe_input_timestamp) {\n    maybe_input_timestamp.value()->acqtime = input_timestamp;\n    is_timestamp_updated = true;\n  }\n\n  if (!is_timestamp_updated) {\n    RCLCPP_ERROR(\n      node_.get_logger(),\n      \"[NitrosPublisher] Failed to update timestamps in a message entity (eid=%ld) as \"\n      \"no Timestamp component was found\",\n      base_msg.handle);\n  }\n\n  publish(base_msg);\n}\n\nvoid NitrosPublisher::publish(NitrosTypeBase & base_msg)\n{\n  {\n    std::stringstream nvtx_tag_name;\n    nvtx_tag_name <<\n      \"[\" << node_.get_name() << \"] NitrosPublisher::publish(\" <<\n      config_.topic_name << \", t=\" <<\n      getTimestamp(base_msg) << \")\";\n    nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_PURPLE);\n  }\n\n  // Invoke user-defined callback if needed\n  if (config_.callback != nullptr) {\n    {\n      std::stringstream nvtx_tag_name;\n      nvtx_tag_name <<\n        \"[\" << node_.get_name() << \"] NitrosPublisher::publish(\" <<\n        config_.topic_name << \")::config_.callback\";\n      nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_PURPLE);\n    }\n\n    RCLCPP_DEBUG(\n      node_.get_logger(),\n      \"[NitrosPublisher] Calling user-defined callback for an Nitros-typed \"\n      \"message (eid=%ld)\", base_msg.handle);\n    config_.callback(context_, base_msg);\n\n    nvtxRangePopWrapper();\n  }\n\n  // Skip publishing if its a noop type\n  if (config_.type == NitrosPublisherSubscriberType::NOOP) {\n    nvtxRangePopWrapper();\n    return;\n  }\n\n  RCLCPP_DEBUG(\n    node_.get_logger(),\n    \"[NitrosPublisher] Publishing an Nitros-typed message (eid=%ld)\", base_msg.handle);\n\n  if (!negotiated_data_format_.empty()) {\n    nitros_type_manager_->getFormatCallbacks(negotiated_data_format_).negotiatedPublishCallback(\n      node_,\n      negotiated_pub_,\n      base_msg);\n  }\n\n  if (config_.compatible_data_format != negotiated_data_format_) {\n    nitros_type_manager_->getFormatCallbacks(config_.compatible_data_format).\n    compatiblePublishCallback(\n      node_,\n      compatible_pub_,\n      base_msg);\n  }\n\n  if (diagnostics_config_.enable_all_topic_diagnostics ||\n    (diagnostics_config_.enable_diagnostics &&\n    diagnostics_config_.topic_name_expected_dt_map.find(config_.topic_name) !=\n    diagnostics_config_.topic_name_expected_dt_map.end()))\n  {\n    updateDiagnostics(getTimestamp(base_msg));\n  }\n\n  nvtxRangePopWrapper();\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros/src/nitros_publisher_subscriber_group.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros/nitros_publisher_subscriber_group.hpp\"\n\n#include \"extensions/gxf_optimizer/common/type.hpp\"\n#include \"negotiated/combinations.hpp\"\n#include \"rclcpp/logger.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nNitrosPublisherSubscriberGroup::NitrosPublisherSubscriberGroup(\n  rclcpp::Node & node,\n  const gxf_context_t context,\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n  const gxf::optimizer::GraphIOGroupSupportedDataTypesInfo & gxf_io_supported_data_formats_info,\n  const NitrosPublisherSubscriberConfigMap & nitros_pub_sub_configs,\n  const std::shared_ptr<std::map<ComponentKey, std::string>> frame_id_map_ptr,\n  const NitrosDiagnosticsConfig & diagnostics_config)\n: node_(node),\n  context_(context),\n  nitros_type_manager_(nitros_type_manager),\n  gxf_io_supported_data_formats_info_(gxf_io_supported_data_formats_info),\n  nitros_pub_sub_configs_(nitros_pub_sub_configs),\n  frame_id_map_ptr_(frame_id_map_ptr),\n  diagnostics_config_(diagnostics_config)\n{\n  // Expand data formats if all ports in this IO group support \"any\" data formats\n  expandAnyDataFormats();\n\n  // Remove undesired supported data formats if needed\n  if (!applyUseCompatibleFormatOnly()) {\n    RCLCPP_ERROR(\n      node_.get_logger(),\n      \"[NitrosPublisherSubscriberGroup] applyUseCompatibleFormatOnly Error\");\n    throw std::runtime_error(\n            \"[NitrosPublisherSubscriberGroup] applyUseCompatibleFormatOnly Error\");\n  }\n\n  // Create Nitros subscribers\n  createNitrosSubscribers();\n\n  // Create Nitros publishers\n  createNitrosPublishers();\n}\n\nstd::vector<std::shared_ptr<NitrosPublisher>>\nNitrosPublisherSubscriberGroup::getNitrosPublishers()\n{\n  return nitros_pubs_;\n}\n\nstd::vector<std::shared_ptr<NitrosSubscriber>>\nNitrosPublisherSubscriberGroup::getNitrosSubscribers()\n{\n  return nitros_subs_;\n}\n\nvoid NitrosPublisherSubscriberGroup::postNegotiationCallback()\n{\n  for (auto & nitros_pub : nitros_pubs_) {\n    nitros_pub->postNegotiationCallback();\n  }\n  for (auto & nitros_sub : nitros_subs_) {\n    nitros_sub->postNegotiationCallback();\n  }\n\n  // In the case that some pubs/subs have sucessful negotiation and some don't (and hence\n  // compatible formats are chosen for these pubs/subs), the resulting group format\n  // combaination may not be valid. We validate and adjust the compatible formats as needed.\n  postNegotiationAdjustCompatibleFormats();\n}\n\nstd::vector<gxf::optimizer::ComponentInfo>\nNitrosPublisherSubscriberGroup::getAllComponentInfos() const\n{\n  std::vector<gxf::optimizer::ComponentInfo> component_info_list =\n    gxf_io_supported_data_formats_info_.ingress_infos;\n  component_info_list.insert(\n    component_info_list.end(),\n    gxf_io_supported_data_formats_info_.egress_infos.begin(),\n    gxf_io_supported_data_formats_info_.egress_infos.end());\n  return component_info_list;\n}\n\nvoid NitrosPublisherSubscriberGroup::expandAnyDataFormats()\n{\n  // In the case that all ingress/egress ports in this group support \"any\" format,\n  // the supported data formats are expanded using the registered data formats.\n  // In such a case, the given IO group data format support information would\n  // only contain one data format combination with all port's supported data format\n  // as \"any\".\n  if (gxf_io_supported_data_formats_info_.supported_data_types.size() == 1) {\n    bool to_be_resolved = true;\n    // Check if every port's supported data format is \"any\"\n    for (const auto & supported_data_type :\n      gxf_io_supported_data_formats_info_.supported_data_types[0])\n    {\n      if (supported_data_type.second != gxf::optimizer::kAnyDataType) {\n        to_be_resolved = false;\n        break;\n      }\n    }\n    if (to_be_resolved == false) {\n      return;\n    }\n\n    // This IO group has all ingress and egress ports supporting \"any\".\n    // We make them actually support any format by expanding the formats with all\n    // the registered data formats.\n    RCLCPP_INFO(\n      node_.get_logger(),\n      \"[NitrosPublisherSubscriberGroup] Expanding \\\"any\\\" data formats to all \"\n      \"registered data formats\");\n    gxf_io_supported_data_formats_info_.supported_data_types.clear();\n    std::map<ComponentKey, std::string> supported_data_format_map;\n    for (const std::string & registered_data_format :\n      nitros_type_manager_->getAllRegisteredDataFormats())\n    {\n      std::vector<gxf::optimizer::ComponentInfo> ingress_egress_infos =\n        gxf_io_supported_data_formats_info_.ingress_infos;\n      ingress_egress_infos.insert(\n        ingress_egress_infos.end(),\n        gxf_io_supported_data_formats_info_.egress_infos.begin(),\n        gxf_io_supported_data_formats_info_.egress_infos.end()\n      );\n      for (const auto & ingress_comp_info : ingress_egress_infos) {\n        const std::string component_key =\n          gxf::optimizer::GenerateComponentKey(ingress_comp_info);\n        supported_data_format_map[component_key] = registered_data_format;\n      }\n      gxf_io_supported_data_formats_info_.supported_data_types.push_back(supported_data_format_map);\n    }\n  }\n}\n\nbool NitrosPublisherSubscriberGroup::applyUseCompatibleFormatOnly()\n{\n  // Iterate through each component in the set and remove the supported format sets that\n  // do not contain the component's compatible format\n  for (const auto & comp_info : getAllComponentInfos()) {\n    ComponentKey comp_key = GenerateComponentKey(comp_info);\n    if (nitros_pub_sub_configs_.count(comp_key) == 0) {\n      RCLCPP_WARN(\n        node_.get_logger(),\n        \"[NitrosPublisherSubscriberGroup] Could not find a config for component \\\"%s/%s\\\" \"\n        \"(type=\\\"%s\\\")\",\n        comp_info.entity_name.c_str(),\n        comp_info.component_name.c_str(),\n        comp_info.component_type_name.c_str());\n      continue;\n    }\n    if (nitros_pub_sub_configs_[comp_key].use_compatible_format_only == false) {\n      continue;\n    }\n    std::string compatible_format = nitros_pub_sub_configs_[comp_key].compatible_data_format;\n    RCLCPP_INFO(\n      node_.get_logger(),\n      \"[NitrosPublisherSubscriberGroup] Pinning the component \\\"%s/%s\\\" \"\n      \"(type=\\\"%s\\\") to use its compatible format only: \\\"%s\\\"\",\n      comp_info.entity_name.c_str(),\n      comp_info.component_name.c_str(),\n      comp_info.component_type_name.c_str(),\n      compatible_format.c_str());\n\n    std::vector<std::map<ComponentKey, std::string>> filtered_supported_data_types;\n    for (const auto & supported_data_type_map :\n      gxf_io_supported_data_formats_info_.supported_data_types)\n    {\n      if (supported_data_type_map.at(comp_key) == compatible_format) {\n        // Only keep the supported format map that has the comp_info's compatible format\n        filtered_supported_data_types.push_back(supported_data_type_map);\n        continue;\n      }\n    }\n    RCLCPP_DEBUG(\n      node_.get_logger(),\n      \"[NitrosPublisherSubscriberGroup] Filtering supported formats based on the compatible \"\n      \"format for the component \\\"%s/%s\\\" (type=\\\"%s\\\"): # of supported formats %ld -> %ld\",\n      comp_info.entity_name.c_str(),\n      comp_info.component_name.c_str(),\n      comp_info.component_type_name.c_str(),\n      gxf_io_supported_data_formats_info_.supported_data_types.size(),\n      filtered_supported_data_types.size());\n    // Update the supported data format list with the filtered one\n    gxf_io_supported_data_formats_info_.supported_data_types = filtered_supported_data_types;\n    if (gxf_io_supported_data_formats_info_.supported_data_types.size() == 0) {\n      RCLCPP_ERROR(\n        node_.get_logger(),\n        \"[NitrosPublisherSubscriberGroup] Supported formats for the component \\\"%s/%s\\\" \"\n        \"(type=\\\"%s\\\") became emtpy\",\n        comp_info.entity_name.c_str(),\n        comp_info.component_name.c_str(),\n        comp_info.component_type_name.c_str());\n      return false;\n    }\n  }\n  return true;\n}\n\nvoid NitrosPublisherSubscriberGroup::postNegotiationAdjustCompatibleFormats()\n{\n  std::vector<std::shared_ptr<NitrosPublisherSubscriberBase>> all_nitros_pubsubs_;\n  all_nitros_pubsubs_.insert(all_nitros_pubsubs_.end(), nitros_pubs_.begin(), nitros_pubs_.end());\n  all_nitros_pubsubs_.insert(all_nitros_pubsubs_.end(), nitros_subs_.begin(), nitros_subs_.end());\n\n  // Find the valid format combinations based on the successfully negotiated formats\n  std::vector<std::map<ComponentKey, std::string>> valid_negotiated_format_combination_maps;\n  for (const auto & supported_data_type_map :\n    gxf_io_supported_data_formats_info_.supported_data_types)\n  {\n    bool is_valid_format_combination = true;\n    for (auto & nitros_pubsub : all_nitros_pubsubs_) {\n      std::string negotiated_format = nitros_pubsub->getNegotiatedDataFormat();\n      if (negotiated_format.empty()) {\n        continue;\n      }\n      ComponentKey comp_key = GenerateComponentKey(nitros_pubsub->getComponentInfo());\n      if (supported_data_type_map.at(comp_key) != negotiated_format) {\n        is_valid_format_combination = false;\n        break;\n      }\n    }\n    if (is_valid_format_combination) {\n      valid_negotiated_format_combination_maps.push_back(supported_data_type_map);\n    }\n  }\n\n  if (valid_negotiated_format_combination_maps.size() == 0) {\n    // No valid data format combination based on the successfully negotiated formats\n    std::string error_msg = \"[NitrosPublisherSubscriberGroup] Detected inconsistent negotiated \"\n      \"formats! The selected data format combination was invalid.\";\n    RCLCPP_ERROR(node_.get_logger(), error_msg.c_str());\n    throw std::runtime_error(error_msg.c_str());\n  }\n\n  // At this point, there is one or more valid format combinations based on the successfully\n  // negotiated formats. Based on these valid combinations, find the follwoing two groups of\n  // valid combinations:\n  // 1. the format combinations that match all the compatible formats for those pubs/subs that\n  //    have unsuccessful format negotiation\n  // 2. the format combinations that match all the compatible formats for those pubs/subs that\n  //    have unsuccessful format negotiation and have `use_flexible_compatible_format` set as\n  //    `false` (i.e., those with `use_flexible_compatible_format = true` don't have to match).\n  std::vector<std::map<ComponentKey, std::string>> valid_final_format_combination_maps;\n  std::vector<std::map<ComponentKey, std::string>> valid_flexible_format_combination_maps;\n  for (const auto & supported_data_type_map : valid_negotiated_format_combination_maps) {\n    bool is_valid_format_combination = true;\n    bool is_valid_flexible_format_combination = true;\n    for (auto & nitros_pubsub : all_nitros_pubsubs_) {\n      if (!nitros_pubsub->getNegotiatedDataFormat().empty()) {\n        continue;\n      }\n      std::string compatible_format = nitros_pubsub->getCompatibleDataFormat();\n      ComponentKey comp_key = GenerateComponentKey(nitros_pubsub->getComponentInfo());\n      if (supported_data_type_map.at(comp_key) != compatible_format) {\n        is_valid_format_combination = false;\n        if (nitros_pub_sub_configs_[comp_key].use_flexible_compatible_format == false) {\n          is_valid_flexible_format_combination = false;\n          break;\n        }\n      }\n    }\n    if (is_valid_format_combination) {\n      valid_final_format_combination_maps.push_back(supported_data_type_map);\n    }\n    if (is_valid_flexible_format_combination) {\n      valid_flexible_format_combination_maps.push_back(supported_data_type_map);\n    }\n  }\n  if (valid_final_format_combination_maps.size() > 0) {\n    // The current negotiated formats + compatible formats combination is valid, so\n    // continue using the existing selections.\n    return;\n  }\n\n  if (valid_flexible_format_combination_maps.size() == 0) {\n    // No valid format combination exists even with considering flexible compatible formats\n    // for those pubs/subs that have `use_flexible_compatible_format = true`\n    std::string error_msg = \"[NitrosPublisherSubscriberGroup] Detected invalid format \"\n      \"combination due to inflexible compatible formats.\";\n    RCLCPP_ERROR(node_.get_logger(), error_msg.c_str());\n    throw std::runtime_error(error_msg.c_str());\n  }\n\n  // A valid format combination has been found with some pubs/subs being configured with\n  // flexible compatible format (i.e., `use_flexible_compatible_format = true`).\n  // Adjust the compatible formats for those pubs/subs based on the first (top priority)\n  // valid format combination.\n  for (auto & nitros_pubsub : all_nitros_pubsubs_) {\n    ComponentKey comp_key = GenerateComponentKey(nitros_pubsub->getComponentInfo());\n\n    if (!nitros_pubsub->getNegotiatedDataFormat().empty() ||\n      (nitros_pub_sub_configs_[comp_key].use_flexible_compatible_format == false))\n    {\n      // This pub/sub is either negotiated successfully or configured to have a fixed\n      // compatible format, so skip it.\n      continue;\n    }\n\n    std::string old_compatible_format = nitros_pubsub->getCompatibleDataFormat();\n    std::string valid_compatible_format = valid_flexible_format_combination_maps[0].at(comp_key);\n\n    if (old_compatible_format == valid_compatible_format) {\n      // The valid format remains the same\n      continue;\n    }\n\n    // Adjust the compatible format to be the one from the top valid format combination\n    nitros_pubsub->setCompatibleDataFormat(valid_compatible_format);\n    nitros_pub_sub_configs_[comp_key].compatible_data_format = valid_compatible_format;\n\n    auto comp_info = nitros_pubsub->getComponentInfo();\n    RCLCPP_INFO(\n      node_.get_logger(),\n      \"[NitrosPublisherSubscriberGroup] Adjusted the compatible format of the component \"\n      \"\\\"%s/%s\\\" (type=\\\"%s\\\") from \\\"%s\\\" to \\\"%s\\\"\",\n      comp_info.entity_name.c_str(),\n      comp_info.component_name.c_str(),\n      comp_info.component_type_name.c_str(),\n      old_compatible_format.c_str(),\n      valid_compatible_format.c_str());\n  }\n}\n\nvoid NitrosPublisherSubscriberGroup::createNitrosSubscribers()\n{\n  // Create subscribers\n  for (const auto & ingress_comp_info : gxf_io_supported_data_formats_info_.ingress_infos) {\n    const std::string component_key = gxf::optimizer::GenerateComponentKey(ingress_comp_info);\n    if (nitros_pub_sub_configs_.count(component_key) == 0) {\n      RCLCPP_ERROR(\n        node_.get_logger(),\n        \"[NitrosPublisherSubscriberGroup] Topic name was not specified for a \"\n        \"GXF graph IO. Component type: %s | Component Name: %s | Entity Name: %s\",\n        ingress_comp_info.component_type_name.c_str(),\n        ingress_comp_info.component_name.c_str(),\n        ingress_comp_info.entity_name.c_str());\n    }\n\n    std::vector<std::string> supported_data_formats =\n      gxf::optimizer::GetSupportedDataTypes(gxf_io_supported_data_formats_info_, ingress_comp_info);\n\n    NitrosPublisherSubscriberConfig component_config = nitros_pub_sub_configs_[component_key];\n\n    auto nitros_sub = std::make_shared<NitrosSubscriber>(\n      node_,\n      context_,\n      nitros_type_manager_,\n      ingress_comp_info,\n      supported_data_formats,\n      component_config,\n      diagnostics_config_,\n      true);\n\n    nitros_sub->setFrameIdMap(frame_id_map_ptr_);\n\n    nitros_subs_.push_back(nitros_sub);\n  }\n}\n\nvoid NitrosPublisherSubscriberGroup::createNitrosPublishers()\n{\n  // Create publishers\n  for (const auto & egress_comp_info : gxf_io_supported_data_formats_info_.egress_infos) {\n    const std::string component_key = gxf::optimizer::GenerateComponentKey(egress_comp_info);\n\n    if (nitros_pub_sub_configs_.count(component_key) == 0) {\n      RCLCPP_ERROR(\n        node_.get_logger(),\n        \"[NitrosPublisherSubscriberGroup] Topic name was not specified for a \"\n        \"GXF graph IO. Component type: %s | Component Name: %s | Entity Name: %s\",\n        egress_comp_info.component_type_name.c_str(),\n        egress_comp_info.component_name.c_str(),\n        egress_comp_info.entity_name.c_str());\n    }\n\n    std::vector<std::string> supported_data_formats =\n      gxf::optimizer::GetSupportedDataTypes(gxf_io_supported_data_formats_info_, egress_comp_info);\n\n    // Set up callback functions for the negotiated publisher\n    negotiated::NegotiatedPublisherOptions negotiated_pub_options;\n    negotiated_pub_options.update_downstream_cb =\n      std::bind(\n      &NitrosPublisherSubscriberGroup::updateUpstreamSubscriberDownstreamSupportedFormatsCallback,\n      this,\n      std::placeholders::_1,\n      std::placeholders::_2,\n      std::placeholders::_3,\n      std::placeholders::_4,\n      std::placeholders::_5,\n      std::placeholders::_6,\n      egress_comp_info);\n\n    negotiated_pub_options.negotiation_cb =\n      std::bind(\n      &NitrosPublisherSubscriberGroup::publisherNegotiationCallback,\n      this,\n      std::placeholders::_1,\n      std::placeholders::_2,\n      std::placeholders::_3,\n      std::placeholders::_4,\n      egress_comp_info);\n\n    NitrosPublisherSubscriberConfig component_config = nitros_pub_sub_configs_[component_key];\n\n    auto nitros_pub = std::make_shared<NitrosPublisher>(\n      node_,\n      context_,\n      nitros_type_manager_,\n      egress_comp_info,\n      supported_data_formats,\n      component_config,\n      negotiated_pub_options,\n      diagnostics_config_);\n\n    nitros_pub->setFrameIdMap(frame_id_map_ptr_);\n\n    // Add all subscribers in the same group as upstream dependency\n    for (auto & nitros_sub : nitros_subs_) {\n      RCLCPP_DEBUG(\n        node_.get_logger(),\n        \"[NitrosPublisherSubscriberGroup] An upstream subscriber added.\");\n      nitros_pub->getNegotiatedPublisher()->add_upstream_negotiated_subscription(\n        nitros_sub->getNegotiatedSubscriber());\n    }\n\n    nitros_pubs_.push_back(nitros_pub);\n  }\n}\n\nstd::shared_ptr<NitrosPublisher> NitrosPublisherSubscriberGroup::findNitrosPublisher(\n  const gxf::optimizer::ComponentInfo & comp_info)\n{\n  for (auto & nitros_pub : nitros_pubs_) {\n    if (nitros_pub->getComponentInfo() == comp_info) {\n      return nitros_pub;\n    }\n  }\n  return nullptr;\n}\n\nstd::shared_ptr<NitrosSubscriber> NitrosPublisherSubscriberGroup::findNitrosSubscriber(\n  const gxf::optimizer::ComponentInfo & comp_info)\n{\n  for (auto & nitros_sub : nitros_subs_) {\n    if (nitros_sub->getComponentInfo() == comp_info) {\n      return nitros_sub;\n    }\n  }\n  return nullptr;\n}\n\nstd::shared_ptr<NitrosSubscriber> NitrosPublisherSubscriberGroup::findNitrosSubscriber(\n  const std::shared_ptr<negotiated::NegotiatedSubscription> negotiated_sub)\n{\n  for (auto & nitros_sub : nitros_subs_) {\n    if (nitros_sub->getNegotiatedSubscriber() == negotiated_sub) {\n      return nitros_sub;\n    }\n  }\n  return nullptr;\n}\n\nvoid NitrosPublisherSubscriberGroup::start()\n{\n  for (auto & nitros_pub : nitros_pubs_) {\n    nitros_pub->start();\n  }\n  for (auto & nitros_sub : nitros_subs_) {\n    nitros_sub->start();\n  }\n}\n\ngxf::optimizer::GraphIOGroupDataTypeConfigurations\nNitrosPublisherSubscriberGroup::getDataFormatConfigurations()\n{\n  nvidia::gxf::optimizer::GraphIOGroupDataTypeConfigurations io_group_config;\n  io_group_config.ingress_infos = gxf_io_supported_data_formats_info_.ingress_infos;\n  io_group_config.egress_infos = gxf_io_supported_data_formats_info_.egress_infos;\n\n  // Publishers\n  for (auto & nitros_pub : nitros_pubs_) {\n    auto comp_key = gxf::optimizer::GenerateComponentKey(nitros_pub->getComponentInfo());\n    if (nitros_pub->getNegotiatedDataFormat().empty()) {\n      io_group_config.data_type_configurations[comp_key] = nitros_pub->getCompatibleDataFormat();\n    } else {\n      io_group_config.data_type_configurations[comp_key] = nitros_pub->getNegotiatedDataFormat();\n    }\n  }\n  // Subscribers\n  for (auto & nitros_sub : nitros_subs_) {\n    auto comp_key = gxf::optimizer::GenerateComponentKey(nitros_sub->getComponentInfo());\n    if (nitros_sub->getNegotiatedDataFormat().empty()) {\n      io_group_config.data_type_configurations[comp_key] = nitros_sub->getCompatibleDataFormat();\n    } else {\n      io_group_config.data_type_configurations[comp_key] = nitros_sub->getNegotiatedDataFormat();\n    }\n  }\n  return io_group_config;\n}\n\nvoid NitrosPublisherSubscriberGroup::updateUpstreamSubscriberDownstreamSupportedFormatsCallback(\n  const std::map<std::string, negotiated::detail::SupportedTypeInfo> & key_to_supported_types,\n  const std::shared_ptr<std::map<\n    negotiated::detail::PublisherGid,\n    std::vector<std::string>>> & negotiated_subscription_type_gids,\n  const std::unordered_set<std::shared_ptr<\n    negotiated::detail::UpstreamNegotiatedSubscriptionHandle>> & upstream_negotiated_subscriptions,\n  const negotiated_interfaces::msg::SupportedTypes & downstream_types_to_add,\n  const negotiated_interfaces::msg::SupportedTypes & downstream_types_to_remove,\n  negotiated::detail::PublisherGid gid_key,\n  gxf::optimizer::ComponentInfo comp_info)\n{\n  (void)key_to_supported_types;\n  (void)negotiated_subscription_type_gids;\n\n  negotiated_interfaces::msg::SupportedTypes upstream_types_to_add;\n  negotiated_interfaces::msg::SupportedTypes upstream_types_to_remove;\n\n  auto nitros_pub = findNitrosPublisher(comp_info);\n\n  for (const std::shared_ptr<negotiated::detail::UpstreamNegotiatedSubscriptionHandle> & handle :\n    upstream_negotiated_subscriptions)\n  {\n    // Map downstream types to the upstream subscriber's types\n    auto nitros_sub = findNitrosSubscriber(handle->subscription);\n\n    std::unordered_set<std::string> upstream_format_names_to_add;\n    for (auto & downstream_type : downstream_types_to_add.supported_types) {\n      auto upstream_formats = mapDownstreamToUpstreamDataFormats(\n        comp_info,\n        nitros_sub->getComponentInfo(),\n        downstream_type.supported_type_name);\n      for (auto upstream_format : upstream_formats) {\n        if (upstream_format_names_to_add.count(upstream_format) == 0) {\n          upstream_format_names_to_add.insert(upstream_format);\n\n          negotiated_interfaces::msg::SupportedType upstream_type_to_add(downstream_type);\n          // Supported format name\n          upstream_type_to_add.supported_type_name = upstream_format;\n          // The corresponding ROS type name\n          upstream_type_to_add.ros_type_name =\n            nitros_type_manager_->getFormatCallbacks(\n            upstream_type_to_add.supported_type_name).getROSTypeName();\n\n          upstream_types_to_add.supported_types.push_back(upstream_type_to_add);\n        }\n      }\n    }\n\n    std::unordered_set<std::string> upstream_format_names_to_remove;\n    for (auto & downstream_type : downstream_types_to_remove.supported_types) {\n      auto upstream_formats = mapDownstreamToUpstreamDataFormats(\n        comp_info,\n        nitros_sub->getComponentInfo(),\n        downstream_type.supported_type_name);\n      for (auto upstream_format : upstream_formats) {\n        if (upstream_format_names_to_add.count(upstream_format) == 0) {\n          upstream_format_names_to_add.insert(upstream_format);\n\n          negotiated_interfaces::msg::SupportedType upstream_type_to_remove(downstream_type);\n          // Supported format name\n          upstream_type_to_remove.supported_type_name = upstream_format;\n          // The corresponding ROS type name\n          upstream_type_to_remove.ros_type_name =\n            nitros_type_manager_->getFormatCallbacks(\n            upstream_type_to_remove.supported_type_name).getROSTypeName();\n\n          upstream_types_to_remove.supported_types.push_back(upstream_type_to_remove);\n        }\n      }\n    }\n\n    handle->subscription->update_downstream_supported_types(\n      upstream_types_to_add,\n      upstream_types_to_remove,\n      gid_key);\n  }\n}\n\nstd::unordered_set<std::string> NitrosPublisherSubscriberGroup::mapDownstreamToUpstreamDataFormats(\n  const gxf::optimizer::ComponentInfo & downstream_comp_info,\n  const gxf::optimizer::ComponentInfo & upstream_comp_info,\n  const std::string & downstream_data_format) const\n{\n  std::unordered_set<std::string> upstream_data_formats;\n  for (auto & data_format_map : gxf_io_supported_data_formats_info_.supported_data_types) {\n    if (data_format_map.at(gxf::optimizer::GenerateComponentKey(downstream_comp_info)) ==\n      downstream_data_format)\n    {\n      upstream_data_formats.insert(\n        data_format_map.at(\n          gxf::optimizer::GenerateComponentKey(\n            upstream_comp_info)));\n    }\n  }\n  return upstream_data_formats;\n}\n\nstd::unordered_set<std::string> NitrosPublisherSubscriberGroup::mapUpstreamToDownstreamDataFormats(\n  const gxf::optimizer::ComponentInfo & upstream_comp_info,\n  const gxf::optimizer::ComponentInfo & downstream_comp_info,\n  const std::string & upstream_data_format) const\n{\n  std::unordered_set<std::string> downstream_data_formats;\n  for (auto & data_format_map : gxf_io_supported_data_formats_info_.supported_data_types) {\n    if (data_format_map.at(gxf::optimizer::GenerateComponentKey(upstream_comp_info)) ==\n      upstream_data_format)\n    {\n      downstream_data_formats.insert(\n        data_format_map.at(\n          gxf::optimizer::GenerateComponentKey(\n            downstream_comp_info)));\n    }\n  }\n  return downstream_data_formats;\n}\n\nstd::vector<negotiated_interfaces::msg::SupportedType>\nNitrosPublisherSubscriberGroup::publisherNegotiationCallback(\n  const std::map<negotiated::detail::PublisherGid,\n  std::vector<std::string>> & negotiated_sub_gid_to_keys,\n  const std::map<std::string,\n  negotiated::detail::SupportedTypeInfo> & key_to_supported_types,\n  const std::unordered_set<\n    std::shared_ptr<negotiated::detail::UpstreamNegotiatedSubscriptionHandle>> &\n  upstream_negotiated_subscriptions,\n  size_t maximum_solutions,\n  gxf::optimizer::ComponentInfo pub_info)\n{\n  // What the negotiation algorithm does is to try to find the minimum number of publishers with\n  // the maximum amount of weight to satisfy all of the subscriptions.  This is approximately\n  // equivalent to the Cutting-stock problem (https://en.wikipedia.org/wiki/Cutting_stock_problem).\n  // To do this, we examine all combinations at every level (a level being the number of publishers\n  // to choose), finding the highest weight one.  If there is at least one at that level, we stop\n  // processing.  If there are no solutions at that level, we increment the number of publishers to\n  // choose by one and try again at the next level.  If we exhaust all levels, then we have failed\n  // to find a match and negotiation fails.\n  //\n  // Some examples will help illustrate the process.\n  //\n  // Scenario 1:\n  //   Assume there are 3 subscribers, S1, S2, and S3.\n  //   Further assume that all 3 subscribers support one ros_type/supported_type_name combination,\n  //    and that combination (F1) is the same across all 3.\n  //   Finally assume that the publisher also supports the same ros_type/supported_type_name\n  //    combination (F1)\n  //   When negotiation happens the publisher will try to find a solution that can satisify all of\n  //    S1, S2, and S3.  It starts by examining all of the solutions that involve one publisher.\n  //    Since all of the subscriptions and the publisher support F1, then a \"level 1\" solution\n  //    exists, and the algorithm chooses that.\n  //\n  // Scenario 2:\n  //   Assume there are 3 subscribers, S1, S2, and S3.\n  //   Further assume that S1 and S2 support one ros_type/supported_type_name combination (F1), and\n  //    S3 supports a different ros_type/supported_type_name combination (F2).\n  //   Finally assume that the publisher supports both F1 and F2 ros_type/supported_type_name\n  //    combinations.\n  //   When negotiation happens the publisher will try to find a solution that can satisify all of\n  //    S1, S2, and S3.  It starts by examining all of the solutions that involve one publisher.\n  //    The publisher and S1 and S2 support F1, but S3 does not, so there is no one publisher\n  //    solution.  Next the algorithm tries all combinations of 2 publisher solutions.  In this\n  //    case we can make 2 publishers, one to satisify F1 and F2, so that algorithm chooses that.\n  //\n  // Scenario 3:\n  //   Assume there are 3 subscribers, S1, S2, and S3.\n  //   Further assume that S1 and S2 support one ros_type/supported_type_name combination (F1), and\n  //    S3 supports a different ros_type/supported_type_name combination (F2).\n  //   Finally assume that the publisher supports only the F1 ros_type/supported_type_name\n  //     combinations.\n  //   When negotiation happens the publisher will try to find a solution that can satisify all of\n  //    S1, S2, and S3.  It starts by examining all of the solutions that involve one publisher.\n  //    The publisher and S1 and S2 support F1, but S3 does not, so there is no one publisher\n  //    solution.  Next the algorithm tries all combinations of 2 publisher solutions.  Since the\n  //    publisher doesn't support F2, there are no 2 publisher solutions.  Finally the algorithm\n  //    tries the 3 publisher solution, but since the publisher doesn't support F2 this can't\n  //    work either.  So the negotiation fails in this case.\n\n  std::vector<negotiated_interfaces::msg::SupportedType> matched_subs;\n\n  // If there are upstream subscriptions that we should wait on before negotiating with our\n  // downstream subscriptions, we'll discover it here.\n  std::map<std::string, negotiated::detail::SupportedTypeInfo> upstream_filtered_supported_types;\n  if (upstream_negotiated_subscriptions.size() > 0) {\n    std::map<ComponentKey, std::string> nitros_sub_negotiated_format_map;  // NVIDIA\n    for (const std::shared_ptr<negotiated::detail::UpstreamNegotiatedSubscriptionHandle> & handle :\n      upstream_negotiated_subscriptions)\n    {\n      negotiated_interfaces::msg::NegotiatedTopicsInfo topics_info =\n        handle->subscription->get_negotiated_topics_info();\n      if (!topics_info.success || topics_info.negotiated_topics.size() == 0) {\n        // NVIDIA\n        continue;\n        // NVIDIA\n      }\n\n      // NVIDIA\n      auto nitros_sub = findNitrosSubscriber(handle->subscription);\n      nitros_sub_negotiated_format_map[GenerateComponentKey(nitros_sub->getComponentInfo())] =\n        topics_info.negotiated_topics[0].supported_type_name;\n      // NVIDIA\n    }\n\n    // NVIDIA\n    if (nitros_sub_negotiated_format_map.size() == 0) {\n      // No upstream subscriber has done its negotiation.\n      // The downstream negotiation still needs to continue with all supported data formats.\n      upstream_filtered_supported_types = key_to_supported_types;\n    } else {\n      // Determine the publisher's current supported formats based on the valid format\n      // combinations filtered by the upstream subscriber's negotiated formats\n      ComponentKey pub_comp_key = GenerateComponentKey(pub_info);\n      for (const auto & supported_data_type_map :\n        gxf_io_supported_data_formats_info_.supported_data_types)\n      {\n        bool is_valid_format_combination = true;\n        for (auto & nitros_sub : nitros_subs_) {\n          ComponentKey comp_key = GenerateComponentKey(nitros_sub->getComponentInfo());\n          std::string negotiated_format = nitros_sub_negotiated_format_map[comp_key];\n          if (supported_data_type_map.at(comp_key) != negotiated_format) {\n            is_valid_format_combination = false;\n            break;\n          }\n        }\n        if (is_valid_format_combination) {\n          std::string pub_valid_format = supported_data_type_map.at(pub_comp_key);\n          std::string key = negotiated::detail::generate_key(\n            nitros_type_manager_->getFormatCallbacks(pub_valid_format).getROSTypeName(),\n            pub_valid_format);\n          if (key_to_supported_types.count(key) > 0) {\n            upstream_filtered_supported_types[key] = key_to_supported_types.at(key);\n          }\n        }\n      }\n    }\n    // NVIDIA\n\n  } else {\n    upstream_filtered_supported_types = key_to_supported_types;\n  }\n\n  std::set<negotiated::detail::PublisherGid> gid_set;\n  for (const std::pair<const negotiated::detail::PublisherGid,\n    std::vector<std::string>> & gid : negotiated_sub_gid_to_keys)\n  {\n    gid_set.insert(gid.first);\n  }\n\n  std::vector<std::string> keys;\n  std::vector<negotiated::detail::SupportedTypeInfo> compatible_supported_types;\n  for (const std::pair<const std::string,\n    negotiated::detail::SupportedTypeInfo> & supported_info : upstream_filtered_supported_types)\n  {\n    keys.push_back(supported_info.first);\n    if (supported_info.second.is_compat) {\n      compatible_supported_types.push_back(supported_info.second);\n    }\n  }\n\n  for (size_t i = 1; i <= upstream_filtered_supported_types.size(); ++i) {\n    double max_weight = 0.0;\n\n    auto check_combination = [\n      & upstream_filtered_supported_types = std::as_const(upstream_filtered_supported_types),\n      & gid_set = std::as_const(gid_set),\n      & compatible_supported_types = std::as_const(compatible_supported_types),\n      & negotiated_sub_gid_to_keys = std::as_const(negotiated_sub_gid_to_keys),\n      &max_weight,\n      &matched_subs](\n      std::vector<std::string>::iterator first,\n      std::vector<std::string>::iterator last) -> bool\n      {\n        std::set<negotiated::detail::PublisherGid> gids_needed = gid_set;\n\n        double sum_of_weights = 0.0;\n\n        for (std::vector<std::string>::iterator it = first; it != last; ++it) {\n          // The iterator should *always* be available in the upstream_filtered_supported_types\n          // map, since we are iterating over that same map.  But we use .at just\n          // to be safe.\n          negotiated::detail::SupportedTypeInfo\n            supported_type_info = upstream_filtered_supported_types.at(*it);\n\n          for (const std::pair<negotiated::detail::PublisherGid,\n            double> gid_to_weight : supported_type_info.gid_to_weight)\n          {\n            sum_of_weights += gid_to_weight.second;\n\n            gids_needed.erase(gid_to_weight.first);\n          }\n        }\n\n        std::vector<negotiated_interfaces::msg::SupportedType> compatible_subs;\n        if (!compatible_supported_types.empty()) {\n          // We've removed all of the ones we could above in this iteration.  Now we go through\n          // the remaining list of GIDs, seeing if any of the \"compatible\" supported types satisfy\n          // the requirements.\n          for (std::set<negotiated::detail::PublisherGid>::const_iterator it = gids_needed.begin();\n            it != gids_needed.end(); )\n          {\n            const std::vector<std::string> key_list = negotiated_sub_gid_to_keys.at(*it);\n\n            bool found_key = false;\n            for (const negotiated::detail::SupportedTypeInfo & compat_info :\n              compatible_supported_types)\n            {\n              std::string key = negotiated::detail::generate_key(\n                compat_info.ros_type_name,\n                compat_info.supported_type_name);\n              if (std::find(key_list.begin(), key_list.end(), key) != key_list.end()) {\n                negotiated_interfaces::msg::SupportedType match;\n                match.ros_type_name = compat_info.ros_type_name;\n                match.supported_type_name = compat_info.supported_type_name;\n                compatible_subs.push_back(match);\n\n                found_key = true;\n                break;\n              }\n            }\n\n            if (found_key) {\n              it = gids_needed.erase(it);\n            } else {\n              ++it;\n            }\n          }\n        }\n\n        if (gids_needed.empty()) {\n          // Hooray!  We found a solution at this level.  We don't interrupt processing at this\n          // level because there may be another combination that is more favorable, but we know\n          // we don't need to descend to further levels.\n\n          if (sum_of_weights > max_weight) {\n            max_weight = sum_of_weights;\n\n            matched_subs.clear();\n            matched_subs = compatible_subs;\n            for (std::vector<std::string>::iterator it = first; it != last; ++it) {\n              negotiated::detail::SupportedTypeInfo supported_type_info =\n                upstream_filtered_supported_types.at(*it);\n              negotiated_interfaces::msg::SupportedType match;\n              match.ros_type_name = supported_type_info.ros_type_name;\n              match.supported_type_name = supported_type_info.supported_type_name;\n              matched_subs.push_back(match);\n            }\n          }\n        }\n\n        return false;\n      };\n\n    for_each_combination(keys.begin(), keys.begin() + i, keys.end(), check_combination);\n\n    if (!matched_subs.empty()) {\n      break;\n    }\n\n    if (i == maximum_solutions) {\n      break;\n    }\n  }\n\n  RCLCPP_DEBUG(\n    node_.get_logger(),\n    \"[NitrosPublisherSubscriberGroup] Returned matched_subs: %ld\",\n    matched_subs.size());\n\n  return matched_subs;\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros/src/nitros_subscriber.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"gxf/core/gxf.h\"\n\n#include \"isaac_ros_nitros/nitros_subscriber.hpp\"\n\n#include \"rclcpp/logger.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char LOGGER_SUFFIX[] = \"NitrosSubscriber\";\nconstexpr uint32_t kGxfReceiverPushPollPeriodNs = 100000;  // 100us\nconstexpr uint32_t kGxfReceiverPushPollWarningPeriodLoopCount = 100000;  // 10s\n\nNitrosSubscriber::NitrosSubscriber(\n  rclcpp::Node & node,\n  const gxf_context_t context,\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n  const gxf::optimizer::ComponentInfo & gxf_component_info,\n  const std::vector<std::string> & supported_data_formats,\n  const NitrosPublisherSubscriberConfig & config,\n  const NitrosDiagnosticsConfig & diagnostics_config,\n  const bool use_callback_group)\n: NitrosPublisherSubscriberBase(\n    node, context, nitros_type_manager, gxf_component_info, supported_data_formats, config),\n  use_callback_group_{use_callback_group}\n{\n  if (config_.type == NitrosPublisherSubscriberType::NOOP) {\n    return;\n  }\n\n  diagnostics_config_ = diagnostics_config;\n  if (diagnostics_config_.enable_all_topic_diagnostics ||\n    (diagnostics_config_.enable_diagnostics &&\n    diagnostics_config_.topic_name_expected_dt_map.find(config_.topic_name) !=\n    diagnostics_config_.topic_name_expected_dt_map.end()))\n  {\n    // Initialize diagnostics variables and message fields\n    initDiagnostics();\n  }\n\n  if (use_callback_group_) {\n    callback_group_ = node_.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);\n    RCLCPP_DEBUG(\n      node_.get_logger(),\n      \"[NitrosSubscriber] Created a MutuallyExclusive callback group\");\n  }\n\n  // Check and throw error if the specified compatible data format is not supported\n  // from the underlying graph\n  throwOnUnsupportedCompatibleDataFormat();\n\n  // Create the compatible data format subscriber\n  createCompatibleSubscriber();\n\n  if (config_.type == NitrosPublisherSubscriberType::NON_NEGOTIATED) {\n    return;\n  }\n\n  // Create a negotiated subscriber object\n  negotiated_sub_ = std::make_shared<negotiated::NegotiatedSubscription>(\n    node_,\n    compatible_sub_->get_topic_name() + std::string(\"/nitros\"));\n\n  // Add supported data formats (which also adds the compatible subscriber)\n  double weight = 1.0;\n  for (std::string data_format : supported_data_formats_) {\n    weight -= 0.1;\n    addSupportedDataFormat(\n      data_format,\n      weight);\n  }\n}\n\nNitrosSubscriber::NitrosSubscriber(\n  rclcpp::Node & node,\n  const gxf_context_t context,\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n  const gxf::optimizer::ComponentInfo & gxf_component_info,\n  const std::vector<std::string> & supported_data_formats,\n  const NitrosPublisherSubscriberConfig & config)\n: NitrosSubscriber(\n    node, context, nitros_type_manager, gxf_component_info, supported_data_formats, config, {})\n{}\n\nNitrosSubscriber::NitrosSubscriber(\n  rclcpp::Node & node,\n  const gxf_context_t context,\n  std::shared_ptr<NitrosTypeManager> nitros_type_manager,\n  const std::vector<std::string> & supported_data_formats,\n  const NitrosPublisherSubscriberConfig & config,\n  const NitrosDiagnosticsConfig & diagnostics_config)\n: NitrosSubscriber(\n    node, context, nitros_type_manager, {}, supported_data_formats, config, diagnostics_config)\n{\n  use_gxf_receiver_ = false;\n}\n\nstd::shared_ptr<negotiated::NegotiatedSubscription> NitrosSubscriber::getNegotiatedSubscriber()\n{\n  return negotiated_sub_;\n}\n\nvoid NitrosSubscriber::addSupportedDataFormat(\n  const std::string & data_format,\n  const double weight)\n{\n  rclcpp::SubscriptionOptions sub_options;\n  sub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;\n  if (use_callback_group_) {\n    sub_options.callback_group = callback_group_;\n  }\n\n  if (!nitros_type_manager_->hasFormat(data_format)) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[NitrosSubscriber] Could not identify the supported data foramt: \" <<\n      \"\\\"\" << data_format.c_str() << \"\\\"\";\n    RCLCPP_ERROR(\n      node_.get_logger(), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  if (data_format == config_.compatible_data_format) {\n    nitros_type_manager_->getFormatCallbacks(data_format).addCompatibleSubscriberCallback(\n      node_,\n      negotiated_sub_,\n      compatible_sub_,\n      weight\n    );\n\n    RCLCPP_DEBUG(\n      node_.get_logger(),\n      \"[NitrosSubscriber] Added a compatible subscriber: \"\n      \"topic_name=\\\"%s\\\", data_format=\\\"%s\\\"\",\n      compatible_sub_->get_topic_name(), config_.compatible_data_format.c_str());\n  } else {\n    std::function<void(NitrosTypeBase &, const std::string data_format_name)>\n    subscriber_callback =\n      std::bind(\n      &NitrosSubscriber::subscriberCallback,\n      this,\n      std::placeholders::_1,\n      std::placeholders::_2);\n\n    nitros_type_manager_->getFormatCallbacks(data_format).addSubscriberSupportedFormatCallback(\n      node_,\n      negotiated_sub_,\n      weight,\n      config_.qos,\n      subscriber_callback,\n      sub_options);\n\n    RCLCPP_DEBUG(\n      node_.get_logger(),\n      \"[NitrosSubscriber] Added a supported data format: \"\n      \"topic_name=\\\"%s\\\", data_format=\\\"%s\\\"\",\n      compatible_sub_->get_topic_name(), data_format.c_str());\n  }\n}\n\nvoid NitrosSubscriber::start()\n{\n  if (config_.type == NitrosPublisherSubscriberType::NEGOTIATED) {\n    negotiated_sub_->start();\n  }\n}\n\nvoid NitrosSubscriber::setIsGxfRunning(const bool is_gxf_running)\n{\n  is_gxf_running_ = is_gxf_running;\n}\n\nvoid NitrosSubscriber::createCompatibleSubscriber()\n{\n  rclcpp::SubscriptionOptions sub_options;\n  sub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;\n  if (use_callback_group_) {\n    sub_options.callback_group = callback_group_;\n  }\n\n  if (!nitros_type_manager_->hasFormat(config_.compatible_data_format)) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[NitrosSubscriber] Could not identify the compatible data foramt: \" <<\n      \"\\\"\" << config_.compatible_data_format.c_str() << \"\\\"\";\n    RCLCPP_ERROR(\n      node_.get_logger(), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  std::function<void(NitrosTypeBase &, const std::string)>\n  subscriber_callback =\n    std::bind(\n    &NitrosSubscriber::subscriberCallback,\n    this,\n    std::placeholders::_1,\n    std::placeholders::_2);\n\n  nitros_type_manager_->getFormatCallbacks(config_.compatible_data_format).\n  createCompatibleSubscriberCallback(\n    node_,\n    compatible_sub_,\n    config_.topic_name,\n    config_.qos,\n    subscriber_callback,\n    sub_options\n  );\n}\n\nvoid NitrosSubscriber::postNegotiationCallback()\n{\n  if (config_.type != NitrosPublisherSubscriberType::NEGOTIATED) {\n    return;\n  }\n\n  auto topics_info = negotiated_sub_->get_negotiated_topics_info();\n  if (!topics_info.success || topics_info.negotiated_topics.size() == 0) {\n    RCLCPP_INFO(\n      node_.get_logger(),\n      \"[NitrosSubscriber] Negotiation ended with no results\");\n    RCLCPP_INFO(\n      node_.get_logger(),\n      \"[NitrosSubscriber] Use the compatible subscriber: \"\n      \"topic_name=\\\"%s\\\", data_format=\\\"%s\\\"\",\n      compatible_sub_->get_topic_name(), config_.compatible_data_format.c_str());\n    negotiated_data_format_ = \"\";\n  } else {\n    negotiated_data_format_ = topics_info.negotiated_topics[0].supported_type_name;\n\n    RCLCPP_INFO(\n      node_.get_logger(),\n      \"[NitrosSubscriber] Use the negotiated data format: \\\"%s\\\"\",\n      negotiated_data_format_.c_str());\n\n    if (negotiated_data_format_ != config_.compatible_data_format) {\n      // Delete the compatible subscriber as we don't use it anymore\n      nitros_type_manager_->getFormatCallbacks(config_.compatible_data_format).\n      removeCompatibleSubscriberCallback(\n        node_,\n        negotiated_sub_,\n        compatible_sub_);\n\n      RCLCPP_DEBUG(\n        node_.get_logger(),\n        \"[NitrosSubscriber] Removed a compatible subscriber: \"\n        \"topic_name=\\\"%s\\\", data_format=\\\"%s\\\"\",\n        compatible_sub_->get_topic_name(), config_.compatible_data_format.c_str());\n\n      compatible_sub_ = nullptr;\n    }\n  }\n}\n\nvoid NitrosSubscriber::setReceiverPointer(void * gxf_receiver_ptr)\n{\n  gxf_receiver_ptr_ = reinterpret_cast<nvidia::gxf::Receiver *>(gxf_receiver_ptr);\n}\n\nvoid NitrosSubscriber::setReceiverPolicy(const size_t policy)\n{\n  if (gxf_receiver_ptr_ == nullptr) {\n    RCLCPP_ERROR(\n      node_.get_logger().get_child(LOGGER_SUFFIX),\n      \"The underlying receiver pointer (\\\"%s\\\") is not set when setting its policy.\",\n      GenerateComponentKey(gxf_component_info_).c_str());\n    return;\n  }\n\n  gxf_result_t code;\n  code = GxfParameterSetUInt64(context_, gxf_receiver_ptr_->cid(), \"policy\", policy);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      node_.get_logger().get_child(LOGGER_SUFFIX),\n      \"Failed to set policy for the underlying receiver (\\\"%s\\\"): %s\",\n      GenerateComponentKey(gxf_component_info_).c_str(),\n      GxfResultStr(code));\n    return;\n  }\n\n  RCLCPP_DEBUG(\n    node_.get_logger().get_child(LOGGER_SUFFIX),\n    \"Set policy to %zu for the underlying receiver (\\\"%s\\\")\",\n    policy, GenerateComponentKey(gxf_component_info_).c_str());\n}\n\n// Set the capacity for the underlying GXF receiver\nvoid NitrosSubscriber::setReceiverCapacity(const size_t capacity)\n{\n  if (gxf_receiver_ptr_ == nullptr) {\n    RCLCPP_ERROR(\n      node_.get_logger().get_child(LOGGER_SUFFIX),\n      \"The underlying receiver pointer (\\\"%s\\\") is not set when setting its capacity.\",\n      GenerateComponentKey(gxf_component_info_).c_str());\n    return;\n  }\n\n  gxf_result_t code;\n  code = GxfParameterSetUInt64(context_, gxf_receiver_ptr_->cid(), \"capacity\", capacity);\n  if (code != GXF_SUCCESS) {\n    RCLCPP_ERROR(\n      node_.get_logger().get_child(LOGGER_SUFFIX),\n      \"Failed to set capacity for the underlying receiver (\\\"%s\\\"): %s\",\n      GenerateComponentKey(gxf_component_info_).c_str(),\n      GxfResultStr(code));\n    return;\n  }\n\n  RCLCPP_DEBUG(\n    node_.get_logger().get_child(LOGGER_SUFFIX),\n    \"Set capacity to %zu for the underlying receiver (\\\"%s\\\")\",\n    capacity, GenerateComponentKey(gxf_component_info_).c_str());\n}\n\nbool NitrosSubscriber::pushEntity(const int64_t eid, bool should_block)\n{\n  gxf_entity_status_t entity_status;\n  gxf_result_t code;\n  gxf_uid_t gxf_receiver_eid = gxf_receiver_ptr_->eid();\n  int loop_count = 0;\n  if (should_block) {\n    while ((gxf_receiver_ptr_->back_size() + 1) >\n      (gxf_receiver_ptr_->capacity() - gxf_receiver_ptr_->size()))\n    {\n      if (loop_count > 0 && loop_count % kGxfReceiverPushPollWarningPeriodLoopCount == 0) {\n        RCLCPP_WARN(\n          node_.get_logger().get_child(LOGGER_SUFFIX),\n          \"%.1fs passed while waiting to push a message entity (eid=%ld) \"\n          \"to the receiver %s\",\n          (loop_count * (kGxfReceiverPushPollPeriodNs / 1000000000.0)),\n          eid, gxf_receiver_ptr_->name());\n      }\n      rclcpp::sleep_for(std::chrono::nanoseconds(kGxfReceiverPushPollPeriodNs));\n      loop_count++;\n      // Check the status of the receiver entity in case the graph is terminated.\n      code = GxfEntityGetStatus(context_, gxf_receiver_eid, &entity_status);\n      if (code != GXF_SUCCESS) {\n        RCLCPP_ERROR(\n          node_.get_logger(),\n          \"[NitrosSubscriber] Failed to get the receiver entity (eid=%ld) status: %s\",\n          gxf_receiver_eid, GxfResultStr(code));\n        return false;\n      }\n    }\n  }\n\n  auto msg_entity = nvidia::gxf::Entity::Shared(context_, eid);\n  gxf_receiver_ptr_->push(std::move(msg_entity.value()));\n  GxfEntityNotifyEventType(context_, gxf_receiver_ptr_->eid(), GXF_EVENT_MESSAGE_SYNC);\n\n  RCLCPP_DEBUG(\n    node_.get_logger(),\n    \"[NitrosSubscriber] Pushed a message entity (eid=%ld) to \"\n    \"the appliation\", eid);\n\n  return true;\n}\n\nvoid NitrosSubscriber::subscriberCallback(\n  NitrosTypeBase & msg_base,\n  const std::string data_format_name)\n{\n  std::stringstream nvtx_tag_name;\n  nvtx_tag_name <<\n    \"[\" << node_.get_name() << \"] NitrosSubscriber::subscriberCallback(\" <<\n    config_.topic_name << \", t=\" <<\n    getTimestamp(msg_base) << \")\";\n  nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_PURPLE);\n\n  // Only enable diagnostics if the ROS parameter flag is enabled and\n  // the topic has been specified in the topics_list ROS parameter\n  if (diagnostics_config_.enable_all_topic_diagnostics ||\n    (diagnostics_config_.enable_diagnostics &&\n    diagnostics_config_.topic_name_expected_dt_map.find(config_.topic_name) !=\n    diagnostics_config_.topic_name_expected_dt_map.end()))\n  {\n    updateDiagnostics(getTimestamp(msg_base));\n  }\n\n  RCLCPP_DEBUG(node_.get_logger(), \"[NitrosSubscriber] Received a Nitros-typed messgae\");\n  RCLCPP_DEBUG(node_.get_logger(), \"[NitrosSubscriber] \\teid: %ld\", msg_base.handle);\n  RCLCPP_DEBUG(\n    node_.get_logger(), \"[NitrosSubscriber] \\tdata_format_name: %s\",\n    data_format_name.c_str());\n  RCLCPP_DEBUG(\n    node_.get_logger(), \"[NitrosSubscriber] \\tmsg_base: %s\",\n    msg_base.data_format_name.c_str());\n  RCLCPP_DEBUG(\n    node_.get_logger(), \"[NitrosSubscriber] \\tReceiver's pointer: %p\",\n    (void *)gxf_receiver_ptr_);\n\n  if (use_gxf_receiver_ && (!is_gxf_running_ || gxf_receiver_ptr_ == nullptr)) {\n    RCLCPP_DEBUG(\n      node_.get_logger(),\n      \"[NitrosSubscriber] Received a message but the underlying GXF graph is\"\n      \"not yet ready.\");\n    nvtxRangePopWrapper();\n    return;\n  }\n\n  if (frame_id_map_ptr_ != nullptr) {\n    std::string frame_id_source_key = config_.frame_id_source_key.empty() ?\n      GenerateComponentKey(gxf_component_info_) : config_.frame_id_source_key;\n    (*frame_id_map_ptr_.get())[frame_id_source_key] = msg_base.frame_id;\n\n    RCLCPP_DEBUG(\n      node_.get_logger(),\n      \"[NitrosSubscriber] Updated frame_id=%s\",\n      (*frame_id_map_ptr_.get())[frame_id_source_key].c_str());\n  }\n\n  if (config_.callback != nullptr) {\n    RCLCPP_DEBUG(\n      node_.get_logger(),\n      \"[NitrosSubscriber] Calling user-defined callback for an Nitros-typed \"\n      \"message (eid=%ld)\", msg_base.handle);\n    config_.callback(context_, msg_base);\n  }\n\n  if (use_gxf_receiver_ && gxf_receiver_ptr_ != nullptr && is_gxf_running_) {\n    // Push the message to the associated gxf receiver if existed\n    pushEntity(msg_base.handle, false);\n  }\n\n  nvtxRangePopWrapper();\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros/src/types/nitros_empty.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <string>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros/types/nitros_empty.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosEmpty,\n  std_msgs::msg::Empty>::convert_to_ros_message(\n  const custom_type & source,\n  ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosEmpty::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosEmpty\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  (void)source;\n  (void)destination;\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto gxf_message = nvidia::gxf::Entity::Shared(context, source.handle);\n  if (!gxf_message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Error getting message entity: \" <<\n      GxfResultStr(gxf_message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosEmpty\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Nothing to populate for empty message\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosEmpty,\n  std_msgs::msg::Empty>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosEmpty::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosEmpty\"),\n    \"[convert_to_custom] Conversion started\");\n\n  (void)source;\n\n  // Create an entity for storing the actual data in the context\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto gxf_message = nvidia::gxf::Entity::New(context);\n  if (!gxf_message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error initializing new message entity: \" <<\n      GxfResultStr(gxf_message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosEmpty\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // No payload to transfer\n  // Set the entity's pointer (EID) in a Nitros type data struct\n  destination.handle = gxf_message->eid();\n  destination.data_format_name = \"nitros_empty\";\n\n  // Increase the reference count for the created entity so it doesn't get destroyed after\n  // exiting this function\n  GxfEntityRefCountInc(context, gxf_message->eid());\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros/src/types/nitros_type_base.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nNitrosTypeBase::NitrosTypeBase(\n  const int64_t handle,\n  const std::string data_format_name,\n  const std::string compatible_data_format_name,\n  const std::string frame_id,\n  const cudaStream_t cuda_stream)\n: handle(handle),\n  data_format_name(data_format_name),\n  compatible_data_format_name(compatible_data_format_name),\n  frame_id(frame_id),\n  cuda_stream(cuda_stream)\n{\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTypeBase\"),\n    \"[Constructor] Creating a Nitros-typed object for handle = %ld\",\n    handle);\n  GxfEntityRefCountInc(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(),\n    handle);\n}\n\nNitrosTypeBase::NitrosTypeBase(const NitrosTypeBase & other)\n: handle(other.handle),\n  data_format_name(other.data_format_name),\n  compatible_data_format_name(other.compatible_data_format_name),\n  frame_id(other.frame_id),\n  cuda_stream(other.cuda_stream)\n{\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTypeBase\"),\n    \"[Copy Constructor] Copying a Nitros-typed object for handle = %ld\",\n    other.handle);\n  GxfEntityRefCountInc(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(),\n    other.handle);\n}\n\nNitrosTypeBase::~NitrosTypeBase()\n{\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTypeBase\"),\n    \"[Destructor]Dstroying a Nitros-typed object for handle = %ld\",\n    handle);\n  GxfEntityRefCountDec(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(),\n    handle);\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros/src/types/type_adapter_nitros_context.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <ament_index_cpp/get_package_share_directory.hpp>\n\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char TYPE_ADAPTER_CONTEXT_YAML[] =\n  \"config/type_adapter_nitros_context_graph.yaml\";\n\nconst std::vector<std::pair<std::string, std::string>> TYPE_ADAPTER_EXTENSIONS = {\n  {\"isaac_ros_gxf\", \"gxf/lib/std/libgxf_std.so\"},\n  {\"isaac_ros_gxf\", \"gxf/lib/cuda/libgxf_cuda.so\"},\n  {\"gxf_isaac_gxf_helpers\", \"gxf/lib/libgxf_isaac_gxf_helpers.so\"},\n  {\"gxf_isaac_sight\", \"gxf/lib/libgxf_isaac_sight.so\"},\n  {\"gxf_isaac_atlas\", \"gxf/lib/libgxf_isaac_atlas.so\"}\n};\n\nstd::unique_ptr<NitrosContext> g_type_adapter_nitros_context;\nstd::mutex g_type_adapter_nitros_context_mutex;\nbool g_type_adapter_nitros_context_initialized = false;\nbool g_type_adapter_nitros_context_destroyed = true;\n\nNitrosContext & GetTypeAdapterNitrosContext()\n{\n  // Mutex: g_type_adapter_nitros_context_mutex\n  const std::lock_guard<std::mutex> lock(g_type_adapter_nitros_context_mutex);\n  gxf_result_t code;\n  if (g_type_adapter_nitros_context_initialized == false) {\n    g_type_adapter_nitros_context = std::make_unique<NitrosContext>();\n    const std::string nitros_package_share_directory =\n      ament_index_cpp::get_package_share_directory(\"isaac_ros_nitros\");\n\n    // Load extensions\n    for (const auto & extension_pair : TYPE_ADAPTER_EXTENSIONS) {\n      const std::string package_directory =\n        ament_index_cpp::get_package_share_directory(extension_pair.first);\n      code = g_type_adapter_nitros_context->loadExtension(package_directory, extension_pair.second);\n      if (code != GXF_SUCCESS) {\n        std::stringstream error_msg;\n        error_msg << \"loadExtensions Error: \" << GxfResultStr(code);\n        RCLCPP_ERROR(rclcpp::get_logger(\"TypeAdapterNitrosContext\"), error_msg.str().c_str());\n        throw std::runtime_error(error_msg.str().c_str());\n      }\n    }\n\n    // Load application\n    code = g_type_adapter_nitros_context->loadApplication(\n      nitros_package_share_directory + \"/\" + TYPE_ADAPTER_CONTEXT_YAML);\n    if (code != GXF_SUCCESS) {\n      std::stringstream error_msg;\n      error_msg << \"loadApplication Error: \" << GxfResultStr(code);\n      RCLCPP_ERROR(rclcpp::get_logger(\"TypeAdapterNitrosContext\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n\n    // Run graph\n    code = g_type_adapter_nitros_context->runGraphAsync();\n    if (code != GXF_SUCCESS) {\n      std::stringstream error_msg;\n      error_msg << \"runGraphAsync Error: \" << GxfResultStr(code);\n      RCLCPP_ERROR(rclcpp::get_logger(\"TypeAdapterNitrosContext\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n\n    // Init CUDA Stream\n    code = g_type_adapter_nitros_context->initCudaStreamPool();\n    if (code != GXF_SUCCESS) {\n      std::stringstream error_msg;\n      error_msg << \"initCudaStreamPool Error: \" << GxfResultStr(code);\n      RCLCPP_ERROR(rclcpp::get_logger(\"TypeAdapterNitrosContext\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n\n    g_type_adapter_nitros_context_initialized = true;\n    g_type_adapter_nitros_context_destroyed = false;\n  }\n  return *g_type_adapter_nitros_context;\n  // End Mutex: g_type_adapter_nitros_context_mutex\n}\n\nvoid DestroyTypeAdapterNitrosContext()\n{\n  const std::lock_guard<std::mutex> lock(g_type_adapter_nitros_context_mutex);\n  if (!g_type_adapter_nitros_context_destroyed) {\n    g_type_adapter_nitros_context->destroy();\n    g_type_adapter_nitros_context.reset();\n    g_type_adapter_nitros_context_destroyed = true;\n    g_type_adapter_nitros_context_initialized = false;\n  }\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2024, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- name: clock\n  type: nvidia::gxf::RealtimeClock\n- type: nvidia::gxf::EventBasedScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    worker_thread_number: 2\n    worker_thread_name_id: \"test_forward_node\"\n- type: nvidia::gxf::JobStatistics\n  parameters:\n    clock: clock"
  },
  {
    "path": "isaac_ros_nitros/test/isaac_ros_nitros_diagnostics_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport time\n\nfrom diagnostic_msgs.msg import DiagnosticArray\nfrom isaac_ros_test import IsaacROSBaseTest\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\nimport pytest\nimport rclpy\nfrom std_msgs.msg import Empty\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with NitrosEmptyForwardNode test node.\"\"\"\n    test_ns = IsaacROSNitrosNodeTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='image_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros',\n                plugin='nvidia::isaac_ros::nitros::NitrosEmptyForwardNode',\n                name='isaac_ros_nitros',\n                namespace=test_ns,\n                parameters=[{\n                        'compatible_format': 'nitros_empty',\n                        'enable_all_diagnostics': True,\n                        'diagnostics_publish_rate': 5.0,\n                        'filter_window_size': 10,\n                        'topics_list': ['topic_forward_output'],\n                        'jitter_tolerance_us': 100,\n                        'expected_fps_list': [50.0]\n                }]\n            ),\n            ComposableNode(\n                package='isaac_ros_nitros',\n                plugin='nvidia::isaac_ros::nitros::NitrosEmptyForwardNode',\n                name='isaac_ros_nitros',\n                namespace=test_ns + '/mid1',\n                parameters=[{\n                        'compatible_format': 'nitros_empty',\n                        'enable_all_diagnostics': False,\n                }],\n                remappings=[\n                    (test_ns + '/mid1/topic_forward_input',\n                     test_ns + '/topic_forward_output'),\n                    (test_ns + '/mid1/topic_forward_output',\n                     test_ns + '/final/topic_forward_output'),\n                ]\n            )\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosNodeTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosNodeTest(IsaacROSBaseTest):\n    \"\"\"\n    Proof-of-Life Test for Isaac ROS Nitros Node.\n\n    1. Sets up ROS publisher to send Empty values\n    2. Sets up ROS subscriber to listen to output channel of NitrosNode\n    3. Verify received messages\n    \"\"\"\n\n    def test_forward_node(self) -> None:\n        self.node._logger.info('Starting Isaac ROS NitrosNode POL Test')\n\n        # Subscriber\n        received_messages = {}\n\n        test_subscribers = [\n            ('/diagnostics', DiagnosticArray)\n        ]\n\n        subs = self.create_logging_subscribers(\n            subscription_requests=test_subscribers,\n            received_messages=received_messages,\n            use_namespace_lookup=False,\n            accept_multiple_messages=True\n        )\n\n        # Publisher\n        publisher_topic_namespace = self.generate_namespace('topic_forward_input')\n        pub = self.node.create_publisher(\n            Empty,\n            publisher_topic_namespace,\n            self.DEFAULT_QOS)\n\n        try:\n            # Construct test message\n            msg = Empty()\n\n            # Testing frame rate\n            test_frame_rate = 50\n\n            # Start sending messages\n            self.node.get_logger().info('Start publishing messages')\n\n            # Publish messages at a fixed frame rate set by test_frame_rate\n            start_time = time.time()\n            end_time = time.time() + 2\n            sent_count = 0\n            while time.time() < end_time:\n                sent_count += 1\n                pub.publish(msg)\n                # Run the queued ROS subscriber callbacks\n                rclpy.spin_once(self.node, timeout_sec=0)\n                time.sleep(1 / test_frame_rate)\n                if sent_count == 10:\n                    # add a dropped frame be delaying here\n                    time.sleep(.5)\n\n            measured_frame_rate = sent_count / (time.time() - start_time)\n            self.node.get_logger().info(f'sent_count: {sent_count}')\n            self.node.get_logger().info(f'time: {time.time() - start_time}')\n            self.node.get_logger().info(f'frame rate: {measured_frame_rate}')\n\n            # Assert that we have received atleast one message on the output topic\n            self.assertNotEqual(\n                len(received_messages['/diagnostics']),\n                0)\n\n            if len(received_messages['/diagnostics']) > 0:\n                # Assert that the measured frame rate is within the acceptable error window\n                print('message len: ', len(received_messages['/diagnostics']))\n                values_received = received_messages['/diagnostics'][-1].status[0].values\n                diagnostics_values = {\n                    'frame_rate_node': 0,\n                    'num_non_increasing_msg': 0,\n                    'num_jitter_outliers_msg': 0,\n                    'num_jitter_outliers_node': 0,\n                    'max_abs_jitter_msg': 0,\n                    'max_abs_jitter_node': 0,\n                    'mean_abs_jitter_msg': 0,\n                    'mean_abs_jitter_node': 0,\n                    'frame_rate_msg': 0,\n                    'total_dropped_frames': 0\n                }\n\n                for kv in values_received:\n                    if kv.key in diagnostics_values:\n                        diagnostics_values[kv.key] = float(kv.value)\n\n                # We are not publishing that consitently above, and this test may be running\n                # on a busy machine so we allow a large error window\n                self.assertAlmostEqual(diagnostics_values['frame_rate_node'], test_frame_rate,\n                                       None, 'measured recv rate error is too high', 5.0)\n                # The forward node used for testing always publishes a 0 msg timestamp\n                self.assertEqual(diagnostics_values['frame_rate_msg'], 0)\n\n                self.assertEqual(diagnostics_values['total_dropped_frames'], 0)\n\n                # Additional assertions for other diagnostic values\n                self.assertEqual(diagnostics_values['num_non_increasing_msg'], 0)\n\n                self.assertEqual(diagnostics_values['num_jitter_outliers_msg'], 0)\n\n                # we added one delayed frame, so we should have at least one jitter outlier\n                # we have have more if the system was very busy\n                self.assertGreaterEqual(diagnostics_values['num_jitter_outliers_node'], 1)\n\n                self.assertGreaterEqual(diagnostics_values['max_abs_jitter_msg'], 0)\n                self.assertGreaterEqual(diagnostics_values['max_abs_jitter_node'], 0)\n                self.assertGreaterEqual(diagnostics_values['mean_abs_jitter_msg'], 0)\n                self.assertGreaterEqual(diagnostics_values['mean_abs_jitter_node'], 0)\n\n        finally:\n            [self.node.destroy_subscription(sub) for sub in subs]\n            self.assertTrue(self.node.destroy_publisher(pub))\n"
  },
  {
    "path": "isaac_ros_nitros/test/isaac_ros_nitros_multi_node_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\nimport pytest\nimport rclpy\nfrom std_msgs.msg import Empty\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with NitrosNode test node.\"\"\"\n    test_ns = IsaacROSNitrosNodeTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='image_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros',\n                plugin='nvidia::isaac_ros::nitros::NitrosEmptyForwardNode',\n                name='isaac_ros_nitros',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_empty'\n                }]\n            ),\n            ComposableNode(\n                package='isaac_ros_nitros',\n                plugin='nvidia::isaac_ros::nitros::NitrosEmptyForwardNode',\n                name='isaac_ros_nitros',\n                namespace=test_ns+'/mid1',\n                parameters=[{\n                        'compatible_format': 'nitros_empty'\n                }],\n                remappings=[\n                    (test_ns+'/mid1/topic_forward_input',\n                     test_ns+'/topic_forward_output'),\n                    (test_ns+'/mid1/topic_forward_output',\n                     test_ns+'/mid2/topic_forward_input'),\n                ]\n            ),\n            ComposableNode(\n                package='isaac_ros_nitros',\n                plugin='nvidia::isaac_ros::nitros::NitrosEmptyForwardNode',\n                name='isaac_ros_nitros',\n                namespace=test_ns+'/mid2',\n                parameters=[{\n                        'compatible_format': 'nitros_empty'\n                }],\n            ),\n            ComposableNode(\n                package='isaac_ros_nitros',\n                plugin='nvidia::isaac_ros::nitros::NitrosEmptyForwardNode',\n                name='isaac_ros_nitros',\n                namespace=test_ns+'/mid3',\n                parameters=[{\n                        'compatible_format': 'nitros_empty'\n                }],\n                remappings=[\n                    (test_ns+'/mid3/topic_forward_input',\n                     test_ns+'/mid2/topic_forward_output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosNodeTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosNodeTest(IsaacROSBaseTest):\n    \"\"\"\n    Proof-of-Life Test for Isaac ROS Nitros Node.\n\n    1. Sets up ROS publisher to send Empty values\n    2. Sets up ROS subscriber to listen to output channel of NitrosNode\n    3. Verify received messages\n    \"\"\"\n\n    def test_forward_node(self) -> None:\n        self.node._logger.info('Starting Isaac ROS NitrosNode POL Test')\n\n        # Subscriber\n        received_messages = {}\n\n        subscriber_topic_namespace = self.generate_namespace('mid3/topic_forward_output')\n        test_subscribers = [\n            (subscriber_topic_namespace, Empty)\n        ]\n\n        subs = self.create_logging_subscribers(\n            subscription_requests=test_subscribers,\n            received_messages=received_messages,\n            use_namespace_lookup=False,\n            accept_multiple_messages=True,\n            add_received_message_timestamps=True\n        )\n\n        # Publisher\n        publisher_topic_namespace = self.generate_namespace('topic_forward_input')\n        pub = self.node.create_publisher(\n            Empty,\n            publisher_topic_namespace,\n            self.DEFAULT_QOS)\n\n        try:\n            # Construct test message\n            msg = Empty()\n\n            # Start sending messages\n            self.node.get_logger().info('Start publishing messages')\n            sent_count = 0\n            end_time = time.time() + 0.5\n            while time.time() < end_time:\n                sent_count += 1\n                pub.publish(msg)\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n            # Conclude the test\n            received_count = len(received_messages[subscriber_topic_namespace])\n            self.node._logger.info(\n                f'Test Results:\\n'\n                f'# of Messages Sent: {sent_count}\\n'\n                f'# of Messages Received: {received_count}\\n'\n                f'# of Messages Dropped: {sent_count - received_count}\\n'\n                f'Message Drop Rate: {((sent_count-received_count)/sent_count)*100}%'\n            )\n\n            self.assertGreater(len(received_messages[subscriber_topic_namespace]), 0)\n\n        finally:\n            [self.node.destroy_subscription(sub) for sub in subs]\n            self.assertTrue(self.node.destroy_publisher(pub))\n"
  },
  {
    "path": "isaac_ros_nitros/test/isaac_ros_nitros_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\nimport pytest\nimport rclpy\nfrom std_msgs.msg import Empty\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with NitrosNode test node.\"\"\"\n    test_ns = IsaacROSNitrosNodeTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='image_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros',\n                plugin='nvidia::isaac_ros::nitros::NitrosEmptyForwardNode',\n                name='isaac_ros_nitros',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_empty'\n                }]\n            ),\n            ComposableNode(\n                package='isaac_ros_nitros',\n                plugin='nvidia::isaac_ros::nitros::NitrosEmptyForwardNode',\n                name='isaac_ros_nitros',\n                namespace=test_ns+'/mid1',\n                parameters=[{\n                        'compatible_format': 'nitros_empty'\n                }],\n                remappings=[\n                    (test_ns+'/mid1/topic_forward_input',\n                     test_ns+'/topic_forward_output'),\n                    (test_ns+'/mid1/topic_forward_output',\n                     test_ns+'/final/topic_forward_output'),\n                ]\n            )\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosNodeTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosNodeTest(IsaacROSBaseTest):\n    \"\"\"\n    Proof-of-Life Test for Isaac ROS Nitros Node.\n\n    1. Sets up ROS publisher to send Empty values\n    2. Sets up ROS subscriber to listen to output channel of NitrosNode\n    3. Verify received messages\n    \"\"\"\n\n    def test_forward_node(self) -> None:\n        self.node._logger.info('Starting Isaac ROS NitrosNode POL Test')\n\n        # Subscriber\n        received_messages = {}\n\n        subscriber_topic_namespace = self.generate_namespace('final/topic_forward_output')\n        test_subscribers = [\n            (subscriber_topic_namespace, Empty)\n        ]\n\n        subs = self.create_logging_subscribers(\n            subscription_requests=test_subscribers,\n            received_messages=received_messages,\n            use_namespace_lookup=False,\n            accept_multiple_messages=True,\n            add_received_message_timestamps=True\n        )\n\n        # Publisher\n        publisher_topic_namespace = self.generate_namespace('topic_forward_input')\n        pub = self.node.create_publisher(\n            Empty,\n            publisher_topic_namespace,\n            self.DEFAULT_QOS)\n\n        try:\n            # Construct test message\n            msg = Empty()\n\n            # Start sending messages\n            self.node.get_logger().info('Start publishing messages')\n\n            # Wait at most 2 seconds for subscriber to receive at least one message\n            end_time = time.time() + 2\n            sent_count = 0\n            while time.time() < end_time:\n                sent_count += 1\n                pub.publish(msg)\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                if len(received_messages[subscriber_topic_namespace]) > 0:\n                    break\n\n            self.node._logger.info(f'# of messages sent: {sent_count}')\n\n            self.assertGreater(len(received_messages[subscriber_topic_namespace]), 0,\n                               \"Didn't receive any output.\")\n\n            self.node._logger.info('At least one message was received.')\n\n        finally:\n            [self.node.destroy_subscription(sub) for sub in subs]\n            self.assertTrue(self.node.destroy_publisher(pub))\n"
  },
  {
    "path": "isaac_ros_nitros/test/src/nitros_empty_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros/types/nitros_empty.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_empty\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosEmptyForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosEmptyForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosEmpty>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosEmptyForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros/test/test_cases/nitros_image/profile/april_tag_detection_array.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"tf_camera\"\n  },\n  \"detections\":[\n    {\n      \"id\": 1,\n      \"family\": \"tag36h11\",\n      \"center\":{\n        \"x\": 2.0, \n        \"y\": 3.0\n      },\n      \"corners\":[\n        {\n          \"x\": 4.0, \n          \"y\": 5.0\n        },\n        {\n          \"x\": 6.0, \n          \"y\": 7.0\n        },\n        {\n          \"x\": 8.0, \n          \"y\": 9.0\n        },\n        {\n          \"x\": 10.0, \n          \"y\": 11.0\n        }\n      ],\n      \"pose\":{\n        \"header\": {\n          \"frame_id\": \"tf_camera_tag1\"\n        },\n        \"pose\": {\n          \"pose\":{\n            \"position\": {\n                \"x\": 1.0, \n                \"y\": 0.0, \n                \"z\": 0.0\n            }, \n            \"orientation\": {\n                \"x\": 0.0, \n                \"y\": 0.0, \n                \"z\": 0.0, \n                \"w\": 1.0\n            }\n          }\n        }\n      }\n    },\n    {\n      \"id\": 2,\n      \"family\": \"tag36h11\",\n      \"center\":{\n        \"x\": 12.0, \n        \"y\": 13.0\n      },\n      \"corners\":[\n        {\n          \"x\": 14.0, \n          \"y\": 15.0\n        },\n        {\n          \"x\": 16.0, \n          \"y\": 17.0\n        },\n        {\n          \"x\": 18.0, \n          \"y\": 19.0\n        },\n        {\n          \"x\": 20.0, \n          \"y\": 21.0\n        }\n      ],\n      \"pose\":{\n        \"header\": {\n          \"frame_id\": \"tf_camera_tag2\"\n        },\n        \"pose\": {\n          \"pose\":{\n            \"position\": {\n                \"x\": 0.0, \n                \"y\": 1.0, \n                \"z\": 0.0\n            }, \n            \"orientation\": {\n                \"x\": 0.0, \n                \"y\": 0.0, \n                \"z\": 0.0, \n                \"w\": 1.0\n            }\n          }\n        }\n      }\n    }\n  ]\n}"
  },
  {
    "path": "isaac_ros_nitros/test/test_cases/nitros_image/profile/camera_info.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"tf_camera\"\n  },\n  \"height\": 460,\n  \"width\": 460,\n  \"distortion_model\": \"plumb_bob\",\n  \"D\": [\n    -0.363528858080088,\n    0.16117037733986861,\n    -8.1109585007538829e-05,\n    -0.00044776712298447841,\n    0.0\n  ],\n  \"K\": [\n    430.15433020105519,\n    0.0,\n    311.71339830549732,\n    0.0,\n    430.60920415473657,\n    221.06824942698509,\n    0.0,\n    0.0,\n    1.0\n  ],\n  \"R\": [\n    0.99806560714807102,\n    0.0068562422224214027,\n    0.061790256276695904,\n    -0.0067522959054715113,\n    0.99997541519165112,\n    -0.0018909025066874664,\n    -0.061801701660692349,\n    0.0014700186639396652,\n    0.99808736527268516\n  ],\n  \"P\": [\n    295.53402059708782,\n    0.0,\n    285.55760765075684,\n    0.0,\n    0.0,\n    295.53402059708782,\n    223.29617881774902,\n    0.0,\n    0.0,\n    0.0,\n    1.0,\n    0.0\n  ]\n}"
  },
  {
    "path": "isaac_ros_nitros/test/test_cases/nitros_image/profile/image.json",
    "content": "{\n    \"image\": \"NVIDIAprofile.png\",\n    \"encoding\": \"bgr8\"\n}"
  },
  {
    "path": "isaac_ros_nitros/test/test_cases/nitros_image/profile/ketchup.pcd",
    "content": "# .PCD v0.7 - Point Cloud Data file format\nVERSION 0.7\nFIELDS x y z\nSIZE 4 4 4\nTYPE F F F\nCOUNT 1 1 1\nWIDTH 500\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0\nPOINTS 20\nDATA ascii\n-1.634617501 -4.290111561 -1.858700299\n2.853279639 -7.209591076 -0.6885781819\n-1.031189043 -0.7250457872 -1.98802603\n-0.8246446857 -7.238877545 -1.052616484\n-1.268758149 1.713223204 -1.907102878\n0.7747915949 -7.244047803 -0.8747465489\n0.1939418026 -7.285154885 -0.5047866621\n-1.677812876 -2.032508282 -1.813671809\n-0.5203375641 0.707792248 -2.066546191\n0.2619938229 3.527571041 -1.784421252\n3.149173217 -5.719117107 -0.5116390493\n2.333993901 -7.365552464 -0.398136829\n-0.4208003406 1.830185188 -2.065341214\n2.852645176 -0.4909255341 -0.6731062388\n-1.764735432 -3.413405289 -1.798219496\n-2.064404219 -4.691953551 -1.700539264\n-0.4341473384 2.614184337 -2.041596547\n-1.367775279 -1.257706668 -1.90602497\n2.51894068 0.5080515703 -1.072134684\n3.100033603 -1.851751732 -0.3858886334"
  },
  {
    "path": "isaac_ros_nitros_bridge/.gitattributes",
    "content": "# Ignore Python files in linguist\n*.py linguist-detectable=false\n\n# Images\n*.gif filter=lfs diff=lfs merge=lfs -text\n*.jpg filter=lfs diff=lfs merge=lfs -text\n*.png filter=lfs diff=lfs merge=lfs -text\n*.psd filter=lfs diff=lfs merge=lfs -text\n\n# Archives\n*.gz filter=lfs diff=lfs merge=lfs -text\n*.tar filter=lfs diff=lfs merge=lfs -text\n*.zip filter=lfs diff=lfs merge=lfs -text\n\n# Documents\n*.pdf filter=lfs diff=lfs merge=lfs -text\n\n# Shared libraries\n*.so filter=lfs diff=lfs merge=lfs -text\n*.so.* filter=lfs diff=lfs merge=lfs -text\n\n# ROS Bags\n**/resources/**/*.zstd filter=lfs diff=lfs merge=lfs -text\n**/resources/**/*.db3 filter=lfs diff=lfs merge=lfs -text\n**/resources/**/*.yaml filter=lfs diff=lfs merge=lfs -text\n**/resources/**/*.bag filter=lfs diff=lfs merge=lfs -text\n\n# DNN Model files\n*.onnx filter=lfs diff=lfs merge=lfs -text\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/.gitignore",
    "content": "# Ignore all pycache files\n**/__pycache__/**\n\n# Ignore TensorRT plan files\n*.plan\n*.engine\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/CONTRIBUTING.md",
    "content": "# Isaac ROS Contribution Rules\n\nAny contribution that you make to this repository will\nbe under the Apache 2 License, as dictated by that\n[license](http://www.apache.org/licenses/LICENSE-2.0.html):\n\n> **5. Submission of Contributions.** Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions.\n\nContributors must sign-off each commit by adding a `Signed-off-by: ...`\nline to commit messages to certify that they have the right to submit\nthe code they are contributing to the project according to the\n[Developer Certificate of Origin (DCO)](https://developercertificate.org/).\n\n[//]: # (202201002)\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/LICENSE",
    "content": "\n                                 Apache License\n                           Version 2.0, January 2004\n                        http://www.apache.org/licenses/\n\n   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION\n\n   1. Definitions.\n\n      \"License\" shall mean the terms and conditions for use, reproduction,\n      and distribution as defined by Sections 1 through 9 of this document.\n\n      \"Licensor\" shall mean the copyright owner or entity authorized by\n      the copyright owner that is granting the License.\n\n      \"Legal Entity\" shall mean the union of the acting entity and all\n      other entities that control, are controlled by, or are under common\n      control with that entity. For the purposes of this definition,\n      \"control\" means (i) the power, direct or indirect, to cause the\n      direction or management of such entity, whether by contract or\n      otherwise, or (ii) ownership of fifty percent (50%) or more of the\n      outstanding shares, or (iii) beneficial ownership of such entity.\n\n      \"You\" (or \"Your\") shall mean an individual or Legal Entity\n      exercising permissions granted by this License.\n\n      \"Source\" form shall mean the preferred form for making modifications,\n      including but not limited to software source code, documentation\n      source, and configuration files.\n\n      \"Object\" form shall mean any form resulting from mechanical\n      transformation or translation of a Source form, including but\n      not limited to compiled object code, generated documentation,\n      and conversions to other media types.\n\n      \"Work\" shall mean the work of authorship, whether in Source or\n      Object form, made available under the License, as indicated by a\n      copyright notice that is included in or attached to the work\n      (an example is provided in the Appendix below).\n\n      \"Derivative Works\" shall mean any work, whether in Source or Object\n      form, that is based on (or derived from) the Work and for which the\n      editorial revisions, annotations, elaborations, or other modifications\n      represent, as a whole, an original work of authorship. For the purposes\n      of this License, Derivative Works shall not include works that remain\n      separable from, or merely link (or bind by name) to the interfaces of,\n      the Work and Derivative Works thereof.\n\n      \"Contribution\" shall mean any work of authorship, including\n      the original version of the Work and any modifications or additions\n      to that Work or Derivative Works thereof, that is intentionally\n      submitted to Licensor for inclusion in the Work by the copyright owner\n      or by an individual or Legal Entity authorized to submit on behalf of\n      the copyright owner. For the purposes of this definition, \"submitted\"\n      means any form of electronic, verbal, or written communication sent\n      to the Licensor or its representatives, including but not limited to\n      communication on electronic mailing lists, source code control systems,\n      and issue tracking systems that are managed by, or on behalf of, the\n      Licensor for the purpose of discussing and improving the Work, but\n      excluding communication that is conspicuously marked or otherwise\n      designated in writing by the copyright owner as \"Not a Contribution.\"\n\n      \"Contributor\" shall mean Licensor and any individual or Legal Entity\n      on behalf of whom a Contribution has been received by Licensor and\n      subsequently incorporated within the Work.\n\n   2. Grant of Copyright License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      copyright license to reproduce, prepare Derivative Works of,\n      publicly display, publicly perform, sublicense, and distribute the\n      Work and such Derivative Works in Source or Object form.\n\n   3. Grant of Patent License. Subject to the terms and conditions of\n      this License, each Contributor hereby grants to You a perpetual,\n      worldwide, non-exclusive, no-charge, royalty-free, irrevocable\n      (except as stated in this section) patent license to make, have made,\n      use, offer to sell, sell, import, and otherwise transfer the Work,\n      where such license applies only to those patent claims licensable\n      by such Contributor that are necessarily infringed by their\n      Contribution(s) alone or by combination of their Contribution(s)\n      with the Work to which such Contribution(s) was submitted. If You\n      institute patent litigation against any entity (including a\n      cross-claim or counterclaim in a lawsuit) alleging that the Work\n      or a Contribution incorporated within the Work constitutes direct\n      or contributory patent infringement, then any patent licenses\n      granted to You under this License for that Work shall terminate\n      as of the date such litigation is filed.\n\n   4. Redistribution. You may reproduce and distribute copies of the\n      Work or Derivative Works thereof in any medium, with or without\n      modifications, and in Source or Object form, provided that You\n      meet the following conditions:\n\n      (a) You must give any other recipients of the Work or\n          Derivative Works a copy of this License; and\n\n      (b) You must cause any modified files to carry prominent notices\n          stating that You changed the files; and\n\n      (c) You must retain, in the Source form of any Derivative Works\n          that You distribute, all copyright, patent, trademark, and\n          attribution notices from the Source form of the Work,\n          excluding those notices that do not pertain to any part of\n          the Derivative Works; and\n\n      (d) If the Work includes a \"NOTICE\" text file as part of its\n          distribution, then any Derivative Works that You distribute must\n          include a readable copy of the attribution notices contained\n          within such NOTICE file, excluding those notices that do not\n          pertain to any part of the Derivative Works, in at least one\n          of the following places: within a NOTICE text file distributed\n          as part of the Derivative Works; within the Source form or\n          documentation, if provided along with the Derivative Works; or,\n          within a display generated by the Derivative Works, if and\n          wherever such third-party notices normally appear. The contents\n          of the NOTICE file are for informational purposes only and\n          do not modify the License. You may add Your own attribution\n          notices within Derivative Works that You distribute, alongside\n          or as an addendum to the NOTICE text from the Work, provided\n          that such additional attribution notices cannot be construed\n          as modifying the License.\n\n      You may add Your own copyright statement to Your modifications and\n      may provide additional or different license terms and conditions\n      for use, reproduction, or distribution of Your modifications, or\n      for any such Derivative Works as a whole, provided Your use,\n      reproduction, and distribution of the Work otherwise complies with\n      the conditions stated in this License.\n\n   5. Submission of Contributions. Unless You explicitly state otherwise,\n      any Contribution intentionally submitted for inclusion in the Work\n      by You to the Licensor shall be under the terms and conditions of\n      this License, without any additional terms or conditions.\n      Notwithstanding the above, nothing herein shall supersede or modify\n      the terms of any separate license agreement you may have executed\n      with Licensor regarding such Contributions.\n\n   6. Trademarks. This License does not grant permission to use the trade\n      names, trademarks, service marks, or product names of the Licensor,\n      except as required for reasonable and customary use in describing the\n      origin of the Work and reproducing the content of the NOTICE file.\n\n   7. Disclaimer of Warranty. Unless required by applicable law or\n      agreed to in writing, Licensor provides the Work (and each\n      Contributor provides its Contributions) on an \"AS IS\" BASIS,\n      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or\n      implied, including, without limitation, any warranties or conditions\n      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A\n      PARTICULAR PURPOSE. You are solely responsible for determining the\n      appropriateness of using or redistributing the Work and assume any\n      risks associated with Your exercise of permissions under this License.\n\n   8. Limitation of Liability. In no event and under no legal theory,\n      whether in tort (including negligence), contract, or otherwise,\n      unless required by applicable law (such as deliberate and grossly\n      negligent acts) or agreed to in writing, shall any Contributor be\n      liable to You for damages, including any direct, indirect, special,\n      incidental, or consequential damages of any character arising as a\n      result of this License or out of the use or inability to use the\n      Work (including but not limited to damages for loss of goodwill,\n      work stoppage, computer failure or malfunction, or any and all\n      other commercial damages or losses), even if such Contributor\n      has been advised of the possibility of such damages.\n\n   9. Accepting Warranty or Additional Liability. While redistributing\n      the Work or Derivative Works thereof, You may choose to offer,\n      and charge a fee for, acceptance of support, warranty, indemnity,\n      or other liability obligations and/or rights consistent with this\n      License. However, in accepting such obligations, You may act only\n      on Your own behalf and on Your sole responsibility, not on behalf\n      of any other Contributor, and only if You agree to indemnify,\n      defend, and hold each Contributor harmless for any liability\n      incurred by, or claims asserted against, such Contributor by reason\n      of your accepting any such warranty or additional liability.\n\n   END OF TERMS AND CONDITIONS\n\n   APPENDIX: How to apply the Apache License to your work.\n\n      To apply the Apache License to your work, attach the following\n      boilerplate notice, with the fields enclosed by brackets \"[]\"\n      replaced with your own identifying information. (Don't include\n      the brackets!)  The text should be enclosed in the appropriate\n      comment syntax for the file format. We also recommend that a\n      file or class name and description of purpose be included on the\n      same \"printed page\" as the copyright notice for easier\n      identification within third-party archives.\n\n   Copyright [yyyy] [name of copyright owner]\n\n   Licensed under the Apache License, Version 2.0 (the \"License\");\n   you may not use this file except in compliance with the License.\n   You may obtain a copy of the License at\n\n       http://www.apache.org/licenses/LICENSE-2.0\n\n   Unless required by applicable law or agreed to in writing, software\n   distributed under the License is distributed on an \"AS IS\" BASIS,\n   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n   See the License for the specific language governing permissions and\n   limitations under the License."
  },
  {
    "path": "isaac_ros_nitros_bridge/config/nitros_bridge_convert_forward.yaml",
    "content": "topics:\n  -\n    topic: /r2b/ros1_input_bridge_image  # Topic name for ros1 bridge\n    type: isaac_ros_nitros_bridge_interfaces/msg/NitrosBridgeImage # Type type for ros1 bridge\n    queue_size: 10 \n  -\n    topic: /r2b/ros1_output_bridge_image # Topic name for ros1 bridge\n    type: isaac_ros_nitros_bridge_interfaces/msg/NitrosBridgeImage # Type type for ros1 bridge\n    queue_size: 10 \n"
  },
  {
    "path": "isaac_ros_nitros_bridge/config/nitros_bridge_forward.yaml",
    "content": "topics:\n  -\n    topic: /r2b/ros2_input_image  # Topic name for ros1 bridge\n    type: sensor_msgs/msg/Image # Type type for ros1 bridge\n    queue_size: 10\n  -\n    topic: /r2b/ros2_output_image # Topic name for ros1 bridge\n    type: sensor_msgs/msg/Image # Type type for ros1 bridge\n    queue_size: 10\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/config/nitros_bridge_image_converter.yaml",
    "content": "topics:\n  -\n    topic: /ros1_input_bridge_image  # Topic name for ros1 bridge\n    type: isaac_ros_nitros_bridge_interfaces/msg/NitrosBridgeImage # Type type for ros1 bridge\n    queue_size: 10 \n  -\n    topic: /ros1_output_bridge_image # Topic name for ros1 bridge\n    type: isaac_ros_nitros_bridge_interfaces/msg/NitrosBridgeImage # Type type for ros1 bridge\n    queue_size: 10\n  - \n    topic: /camera_info # Topic name for ros1 bridge\n    type: sensor_msgs/msg/CameraInfo # Type type for ros1 bridge\n    queue_size: 10  \n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_bridge_ros2 LANGUAGES C CXX)\n\nif($ENV{ROS_VERSION} EQUAL 1)\n  message(STATUS \"Skip package ${PROJECT_NAME} for ROS 1\")\n  return()\nendif()\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nfind_package(CUDAToolkit REQUIRED)\nament_auto_find_build_dependencies()\n\n# Image converter node\nament_auto_add_library(image_converter_node SHARED src/image_converter_node.cpp)\ntarget_link_libraries(image_converter_node CUDA::cuda_driver)\nset_target_properties(image_converter_node PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE\n)\n\nrclcpp_components_register_nodes(image_converter_node \"nvidia::isaac_ros::nitros_bridge::ImageConverterNode\")\nset(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros_bridge::ImageConverterNode;$<TARGET_FILE:image_converter_node>\\n\")\n\n\n# TensorList converter node\nament_auto_add_library(tensor_list_converter_node SHARED src/tensor_list_converter_node.cpp)\ntarget_link_libraries(tensor_list_converter_node CUDA::cuda_driver)\nset_target_properties(tensor_list_converter_node PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE\n)\n\nrclcpp_components_register_nodes(tensor_list_converter_node \"nvidia::isaac_ros::nitros_bridge::TensorListConverterNode\")\nset(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros_bridge::TensorListConverterNode;$<TARGET_FILE:tensor_list_converter_node>\\n\")\n\nif(BUILD_TESTING)\n  find_package(ament_lint_auto REQUIRED)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_bridge_image_pol.py TIMEOUT \"15\")\n  add_launch_test(test/isaac_ros_nitros_bridge_tensor_list_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE launch)\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/include/isaac_ros_nitros_bridge_ros2/image_converter_node.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_BRIDGE_ROS2__IMAGE_CONVERTER_NODE_HPP_\n#define ISAAC_ROS_NITROS_BRIDGE_ROS2__IMAGE_CONVERTER_NODE_HPP_\n\n#include <map>\n#include <memory>\n#include <string>\n#include <vector>\n\n#include \"rclcpp/rclcpp.hpp\"\n\n#include \"isaac_ros_common/qos.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_publisher.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_subscriber.hpp\"\n\n#include \"ipc_buffer_manager.hpp\"\n#include \"sensor_msgs/msg/image.hpp\"\n#include \"isaac_ros_nitros_bridge_interfaces/msg/nitros_bridge_image.hpp\"\n#include \"isaac_ros_nitros_image_type/nitros_image.hpp\"\n#include \"isaac_ros_nitros_image_type/nitros_image_builder.hpp\"\n#include \"isaac_ros_nitros_image_type/nitros_image_view.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros_bridge\n{\n\nclass ImageConverterNode : public rclcpp::Node\n{\npublic:\n  explicit ImageConverterNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions());\n\n  ~ImageConverterNode();\n\nprivate:\n  // Convert stub message into managed NITROS message\n  void BridgeToROSCallback(\n    const isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeImage::SharedPtr msg);\n\n  // Copy managed NITROS message data into IPC memory pool and convert to bridge message\n  void ROSToBridgeCallback(const nvidia::isaac_ros::nitros::NitrosImageView msg);\n\n  // Publisher for output NitrosImage messages\n  std::shared_ptr<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n      nvidia::isaac_ros::nitros::NitrosImage>> nitros_pub_;\n  // Publisher for output bridge messages\n  rclcpp::Publisher<isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeImage>::SharedPtr\n    bridge_image_pub_;\n  // Subscription to input NitrosImage messages\n  std::shared_ptr<nvidia::isaac_ros::nitros::ManagedNitrosSubscriber<\n      nvidia::isaac_ros::nitros::NitrosImageView>> nitros_sub_;\n  // Subscription to input bridge messages\n  rclcpp::Subscription<isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeImage>::SharedPtr\n    bridge_image_sub_;\n\n  // Type of NITROS image to publish\n  std::string pub_nitros_image_type_;\n  // Type of NITROS image to subscribe\n  std::string sub_nitros_image_type_;\n\n  // Number of blocks of the device memory pool\n  int64_t num_blocks_;\n  // Timeout in microsec to waiting for a buffer to be available\n  int64_t timeout_;\n  // Map between FD and device memory pointer\n  std::map<int32_t, CUdeviceptr> handle_ptr_map_;\n  // CUDA IPC memory pool manager\n  std::shared_ptr<IPCBufferManager> ipc_buffer_manager_;\n  // Shared memory based IPC buffer for refcount and UID\n  std::shared_ptr<HostIPCBuffer> host_ipc_buffer_;\n  // If received the first message\n  bool first_msg_received_ = false;\n  // CUDA driver context\n  CUcontext ctx_;\n  // CUDA IPC event handle sent by the sender\n  cudaIpcEventHandle_t ipc_event_handle_;\n  // CUDA event export from IPC event to synchronize the upstream\n  cudaEvent_t event_;\n  // QoS for NITROS bridge publishers and subscribers\n  rclcpp::QoS bridge_pub_qos_;\n  rclcpp::QoS bridge_sub_qos_;\n  // QoS for NITROS publishers and subscribers\n  rclcpp::QoS nitros_pub_qos_;\n  rclcpp::QoS nitros_sub_qos_;\n};\n\n}  // namespace nitros_bridge\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_BRIDGE_ROS2__IMAGE_CONVERTER_NODE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/include/isaac_ros_nitros_bridge_ros2/ipc_buffer_manager.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_BRIDGE_ROS2__IPC_BUFFER_MANAGER_HPP_\n#define ISAAC_ROS_NITROS_BRIDGE_ROS2__IPC_BUFFER_MANAGER_HPP_\n\n#include <cuda_runtime_api.h>\n#include <cuda.h>\n#include <sys/syscall.h>\n#include <sys/types.h>\n#include <unistd.h>\n\n#include <memory>\n#include <string>\n#include <vector>\n#include <boost/uuid/uuid.hpp>\n#include <boost/uuid/uuid_generators.hpp>\n#include <boost/uuid/uuid_io.hpp>\n#include <boost/interprocess/sync/scoped_lock.hpp>\n#include <boost/interprocess/sync/named_mutex.hpp>\n#include <boost/interprocess/shared_memory_object.hpp>\n#include <boost/interprocess/mapped_region.hpp>\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\n\nnamespace nitros_bridge\n{\n\n// Size of shared memory object in bytes\nconstexpr size_t kSharedMemorySize = 1024;\n\nstruct DeviceIPCBuffer\n{\n  CUdeviceptr d_ptr;\n  CUmemGenericAllocationHandle generic_allocation_handle;\n  int pid;\n  int fd;\n  std::string uid;\n  size_t index;\n};\n\nstruct HostIPCBuffer\n{\n  enum class Mode {CREATE, OPEN};\n  std::shared_ptr<boost::interprocess::shared_memory_object> shm_object_ = nullptr;\n  std::shared_ptr<boost::interprocess::named_mutex> mutex_ = nullptr;\n  std::shared_ptr<boost::interprocess::mapped_region> shm_ptr_ = nullptr;\n  boost::uuids::uuid * uid_ = nullptr;\n  int32_t * refcount_ = nullptr;\n  int start_time_ = 0;\n\n  HostIPCBuffer(const std::string & shm_name, const Mode mode)\n  {\n    if (mode == Mode::CREATE) {\n      shm_object_ = std::make_shared<boost::interprocess::shared_memory_object>(\n        boost::interprocess::open_or_create,\n        shm_name.c_str(), boost::interprocess::read_write);\n      shm_object_->truncate(kSharedMemorySize);\n      mutex_ = std::make_shared<boost::interprocess::named_mutex>(\n        boost::interprocess::open_or_create, (shm_name).c_str());\n      shm_ptr_ = std::make_shared<boost::interprocess::mapped_region>(\n        *shm_object_, boost::interprocess::read_write);\n      refcount_ = static_cast<int *>(shm_ptr_->get_address());\n      uid_ = reinterpret_cast<boost::uuids::uuid *>(\n        static_cast<char *>(shm_ptr_->get_address()) + sizeof(int));\n\n      // Initialize refcount and UID\n      *refcount_ = 0;\n      *uid_ = boost::uuids::random_generator()();\n    } else {\n      try {\n        shm_object_ = std::make_shared<boost::interprocess::shared_memory_object>(\n          boost::interprocess::open_only, shm_name.c_str(), boost::interprocess::read_write);\n        mutex_ = std::make_shared<boost::interprocess::named_mutex>(\n          boost::interprocess::open_only, (shm_name).c_str());\n        shm_ptr_ = std::make_shared<boost::interprocess::mapped_region>(\n          *shm_object_, boost::interprocess::read_write);\n        refcount_ = static_cast<int *>(shm_ptr_->get_address());\n        uid_ = reinterpret_cast<boost::uuids::uuid *>(\n          static_cast<char *>(shm_ptr_->get_address()) + sizeof(int));\n      } catch (const boost::interprocess::interprocess_exception & ex) {\n        RCLCPP_ERROR(\n          rclcpp::get_logger(\"SharedMem\"), \"Failed to open shared memory object %s\",\n          ex.what());\n        throw std::runtime_error(\"Failed to open shared memory object\");\n      }\n    }\n  }\n\n  bool reset_if_refcount_zero()\n  {\n    boost::interprocess::scoped_lock<boost::interprocess::named_mutex> lock(*mutex_);\n    if (*refcount_ == 0) {\n      // Reset the start time and UID\n      *uid_ = boost::uuids::random_generator()();\n      start_time_ = std::chrono::duration_cast<std::chrono::microseconds>(\n        std::chrono::system_clock::now().time_since_epoch()).count();\n      return true;\n    }\n    return false;\n  }\n\n  bool refcoun_inc_if_uid_match(const std::string & received_uid)\n  {\n    // The UID compare and refcount increment should be atomic\n    boost::interprocess::scoped_lock<boost::interprocess::named_mutex> lock(*mutex_);\n    if (boost::uuids::to_string(*uid_) == received_uid) {\n      *refcount_ += 1;\n      return true;\n    }\n    return false;\n  }\n\n  void refcount_inc()\n  {\n    boost::interprocess::scoped_lock<boost::interprocess::named_mutex> lock(*mutex_);\n    *refcount_ += 1;\n  }\n\n  void refcount_dec()\n  {\n    boost::interprocess::scoped_lock<boost::interprocess::named_mutex> lock(*mutex_);\n    if (*refcount_ == 0) {\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"IPCBufferManager\"), \"Refcount is already zero.\");\n      return;\n    }\n    *refcount_ -= 1;\n  }\n\n  std::string get_uid()\n  {\n    boost::interprocess::scoped_lock<boost::interprocess::named_mutex> lock(*mutex_);\n    auto uid = boost::uuids::to_string(*uid_);\n    return uid;\n  }\n};\n\nclass IPCBufferManager\n{\npublic:\n  IPCBufferManager() = default;\n\n  // Constructor, create device memory buffers and export to FD\n  IPCBufferManager(size_t buffer_count, size_t buffer_step, int64_t timeout)\n  {\n    buffer_count_ = buffer_count;\n    buffer_step_ = buffer_step;\n    timeout_ = timeout;\n\n    CUmemAllocationProp prop = {};\n    prop.type = CU_MEM_ALLOCATION_TYPE_PINNED;\n    prop.location.type = CU_MEM_LOCATION_TYPE_DEVICE;\n    prop.location.id = 0;\n    prop.requestedHandleTypes = CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR;\n    size_t granularity = 0;\n    auto cuda_err = cuMemGetAllocationGranularity(\n      &granularity, &prop, CU_MEM_ALLOC_GRANULARITY_MINIMUM);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"IPCBufferManager\"), \"Failed to call cuMemGetAllocationGranularity %s\",\n        error_str);\n    }\n    alloc_size_ = buffer_step - (buffer_step % granularity) + granularity;\n\n    for (size_t index = 0; index < buffer_count_; index++) {\n      // Create shareable device memory buffer\n      CUmemGenericAllocationHandle generic_allocation_handle;\n      auto cuda_err = cuMemCreate(&generic_allocation_handle, alloc_size_, &prop, 0);\n      if (CUDA_SUCCESS != cuda_err) {\n        const char * error_str = NULL;\n        cuGetErrorString(cuda_err, &error_str);\n        RCLCPP_ERROR(\n          rclcpp::get_logger(\"IPCBufferManager\"), \"Failed to call cuMemCreate %s\",\n          error_str);\n      }\n\n      int fd = -1;\n      cuda_err = cuMemExportToShareableHandle(\n        reinterpret_cast<void *>(&fd),\n        generic_allocation_handle,\n        CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR, 0);\n      if (CUDA_SUCCESS != cuda_err) {\n        const char * error_str = NULL;\n        cuGetErrorString(cuda_err, &error_str);\n        RCLCPP_ERROR(\n          rclcpp::get_logger(\"IPCBufferManager\"), \"Failed to cuMemExportToShareableHandle %s\",\n          error_str);\n      }\n\n      CUdeviceptr d_ptr = 0ULL;\n      cuda_err = cuMemAddressReserve(&d_ptr, alloc_size_, 0, 0, 0);\n      if (CUDA_SUCCESS != cuda_err) {\n        const char * error_str = NULL;\n        cuGetErrorString(cuda_err, &error_str);\n        RCLCPP_ERROR(\n          rclcpp::get_logger(\"IPCBufferManager\"), \"Failed to call cuMemCreate %s\",\n          error_str);\n      }\n\n      cuda_err = cuMemMap(d_ptr, alloc_size_, 0, generic_allocation_handle, 0);\n      if (CUDA_SUCCESS != cuda_err) {\n        const char * error_str = NULL;\n        cuGetErrorString(cuda_err, &error_str);\n        RCLCPP_ERROR(\n          rclcpp::get_logger(\"IPCBufferManager\"), \"Failed to call cuMemCreate %s\",\n          error_str);\n      }\n\n      CUmemAccessDesc accessDesc = {};\n      accessDesc.location.type = CU_MEM_LOCATION_TYPE_DEVICE;\n      accessDesc.location.id = 0;\n      accessDesc.flags = CU_MEM_ACCESS_FLAGS_PROT_READWRITE;\n      cuda_err = cuMemSetAccess(d_ptr, alloc_size_, &accessDesc, 1);\n      if (CUDA_SUCCESS != cuda_err) {\n        const char * error_str = NULL;\n        cuGetErrorString(cuda_err, &error_str);\n        RCLCPP_ERROR(\n          rclcpp::get_logger(\"IPCBufferManager\"), \"Failed to call cuMemSetAccess %s\",\n          error_str);\n      }\n\n      // Create host shared memory object\n      std::string shm_name = std::to_string(getpid()) + std::to_string(fd);\n      std::shared_ptr<HostIPCBuffer> host_ipc_buffer =\n        std::make_shared<HostIPCBuffer>(shm_name, HostIPCBuffer::Mode::CREATE);\n      std::shared_ptr<DeviceIPCBuffer> device_ipc_buffer =\n        std::make_shared<DeviceIPCBuffer>(\n        DeviceIPCBuffer{\n              d_ptr, generic_allocation_handle, getpid(),\n              fd, host_ipc_buffer->get_uid(), index});\n      host_ipc_buffers_.push_back(host_ipc_buffer);\n      device_ipc_buffers_.push_back(device_ipc_buffer);\n    }\n  }\n\n  // Destructor, free the alloacted device memory pool\n  ~IPCBufferManager()\n  {\n    for (size_t i = 0; i < buffer_count_; i++) {\n      cuMemUnmap(device_ipc_buffers_[i]->d_ptr, alloc_size_);\n      cuMemAddressFree(device_ipc_buffers_[i]->d_ptr, alloc_size_);\n      cuMemRelease(device_ipc_buffers_[i]->generic_allocation_handle);\n      std::string shm_name =\n        std::to_string(device_ipc_buffers_[i]->pid) + std::to_string(device_ipc_buffers_[i]->fd);\n      boost::interprocess::named_mutex::remove(shm_name.c_str());\n      boost::interprocess::shared_memory_object::remove(shm_name.c_str());\n    }\n  }\n\n  // Move the index to next available device memory block\n  std::shared_ptr<DeviceIPCBuffer> find_next_available_buffer()\n  {\n    auto last_handle_index = current_handle_index_;\n    while (true) {\n      auto current_time = std::chrono::duration_cast<std::chrono::microseconds>(\n        std::chrono::system_clock::now().time_since_epoch()).count();\n      // TODO(yuankunz) ：Reset the timeout everytime the refount is 0\n      auto start_time = host_ipc_buffers_[current_handle_index_]->start_time_;\n      if (current_time - start_time > timeout_) {\n        if (host_ipc_buffers_[current_handle_index_]->reset_if_refcount_zero()) {\n          device_ipc_buffers_[current_handle_index_]->uid =\n            host_ipc_buffers_[current_handle_index_]->get_uid();\n          auto ret_device_buffer = device_ipc_buffers_[current_handle_index_];\n          current_handle_index_ = (current_handle_index_ + 1) % buffer_count_;\n          return ret_device_buffer;\n        }\n      } else {\n        current_handle_index_ = (current_handle_index_ + 1) % buffer_count_;\n      }\n      if (current_handle_index_ == last_handle_index) {\n        RCLCPP_INFO(\n          rclcpp::get_logger(\"IPCBufferManager\"), \"No available buffer, re-check after 1 ms\");\n        std::this_thread::sleep_for(std::chrono::milliseconds(1));\n      }\n    }\n  }\n\nprivate:\n  // Number of pre-allocated device memory buffers\n  size_t buffer_count_;\n  // requested size of each device memory buffer\n  size_t buffer_step_;\n  // Index of the current device memory buffer\n  size_t current_handle_index_ = 0;\n  // Allocated size of each device memory buffer\n  size_t alloc_size_;\n  // Timeout in microseconds to reset the buffer\n  int64_t timeout_;\n  // Vector of device memory buffers\n  std::vector<std::shared_ptr<DeviceIPCBuffer>> device_ipc_buffers_;\n  // Vector of shared memory buffers contain refcount and UID\n  std::vector<std::shared_ptr<HostIPCBuffer>> host_ipc_buffers_;\n};\n\n}  // namespace nitros_bridge\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_BRIDGE_ROS2__IPC_BUFFER_MANAGER_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/include/isaac_ros_nitros_bridge_ros2/tensor_list_converter_node.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_BRIDGE_ROS2__TENSOR_LIST_CONVERTER_NODE_HPP_\n#define ISAAC_ROS_NITROS_BRIDGE_ROS2__TENSOR_LIST_CONVERTER_NODE_HPP_\n\n#include <map>\n#include <memory>\n#include <vector>\n\n#include \"rclcpp/rclcpp.hpp\"\n\n#include \"isaac_ros_common/qos.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_publisher.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_subscriber.hpp\"\n\n#include \"ipc_buffer_manager.hpp\"\n#include \"sensor_msgs/msg/image.hpp\"\n#include \"isaac_ros_nitros_bridge_interfaces/msg/nitros_bridge_tensor_list.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list_builder.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list_view.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_builder.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros_bridge\n{\n\nclass TensorListConverterNode : public rclcpp::Node\n{\npublic:\n  explicit TensorListConverterNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions());\n\n  ~TensorListConverterNode();\n\nprivate:\n  // Convert stub message into managed NITROS message\n  void BridgeToROSCallback(\n    const isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeTensorList::SharedPtr msg);\n\n  // Copy managed NITROS message data into IPC memory pool and convert to bridge message\n  void ROSToBridgeCallback(const nvidia::isaac_ros::nitros::NitrosTensorListView msg);\n\n  // Publisher for output NitrosTensorList messages\n  std::shared_ptr<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n      nvidia::isaac_ros::nitros::NitrosTensorList>> nitros_pub_;\n  // Publisher for output bridge messages\n  rclcpp::Publisher<isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeTensorList>::SharedPtr\n    nitros_bridge_pub_;\n\n  // Subscription to input NitrosTensorList messages\n  std::shared_ptr<nvidia::isaac_ros::nitros::ManagedNitrosSubscriber<\n      nvidia::isaac_ros::nitros::NitrosTensorListView>> nitros_sub_;\n  // Subscription to input bridge messages\n  rclcpp::Subscription<isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeTensorList>::SharedPtr\n    nitros_bridge_sub_;\n\n  // Number of blocks of the device memory pool\n  int64_t num_blocks_;\n  // Timeout in microsec to waiting for a buffer to be available\n  int64_t timeout_;\n  // Map between FD and device memory pointer\n  std::map<int32_t, CUdeviceptr> handle_ptr_map_;\n  // CUDA IPC memory pool manager\n  std::shared_ptr<IPCBufferManager> ipc_buffer_manager_;\n  // Shared memory based IPC buffer for refcount and UID\n  std::shared_ptr<HostIPCBuffer> host_ipc_buffer_;\n  // If received the first message\n  bool first_msg_received_ = false;\n  // CUDA driver context\n  CUcontext ctx_;\n  // CUDA IPC event handle sent by the sender\n  cudaIpcEventHandle_t ipc_event_handle_;\n  // CUDA event export from IPC event to synchronize the upstream\n  cudaEvent_t event_;\n  // QoS for NITROS publishers and subscribers\n  rclcpp::QoS nitros_pub_qos_;\n  rclcpp::QoS nitros_sub_qos_;\n  // QoS for NITROS bridge publishers and subscribers\n  rclcpp::QoS bridge_pub_qos_;\n  rclcpp::QoS bridge_sub_qos_;\n};\n\n}  // namespace nitros_bridge\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_BRIDGE_ROS2__TENSOR_LIST_CONVERTER_NODE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/launch/isaac_ros_nitros_bridge_image_converter.launch.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport launch\n\nfrom launch.actions import DeclareLaunchArgument, OpaqueFunction\nfrom launch.substitutions import LaunchConfiguration\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing.actions\n\n\ndef launch_setup(context):\n    pub_image_name = LaunchConfiguration('pub_image_name').perform(context)\n    sub_image_name = LaunchConfiguration('sub_image_name').perform(context)\n\n    ros2_converter = ComposableNode(\n        name='ros2_converter',\n        namespace='',\n        package='isaac_ros_nitros_bridge_ros2',\n        plugin='nvidia::isaac_ros::nitros_bridge::ImageConverterNode',\n        remappings=[\n            ('ros2_output_bridge_image', 'ros1_input_bridge_image'),\n            ('ros2_output_image', pub_image_name),\n            ('ros2_input_bridge_image', 'ros1_output_bridge_image'),\n            ('ros2_input_image', sub_image_name),\n        ])\n\n    container = ComposableNodeContainer(\n        name='ros2_converter_container',\n        namespace='',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[ros2_converter],\n        output='screen',\n        arguments=['--ros-args', '--log-level', 'info']\n    )\n    return [container]\n\n\ndef generate_launch_description():\n    launch_args = [\n        DeclareLaunchArgument(\n            'pub_image_name',\n            description='Remapped publish image name of NITROS Bridge ROS2'),\n        DeclareLaunchArgument(\n            'sub_image_name',\n            description='Remapped subscribe image name of NITROS Bridge ROS2'),\n    ]\n\n    nodes = launch_args + [OpaqueFunction(function=launch_setup)]\n    return launch.LaunchDescription(\n        nodes + [\n            # Start tests after a fixed delay for node startup\n            launch.actions.TimerAction(\n                period=30.0,\n                actions=[launch_testing.actions.ReadyToTest()])\n        ]\n    )\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/launch/isaac_ros_nitros_bridge_quickstart.launch.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport launch\n\nfrom launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction\nfrom launch.substitutions import LaunchConfiguration\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing.actions\n\n\ndef launch_setup(context):\n    rosbag_path = LaunchConfiguration('rosbag_path').perform(context)\n\n    rosbag_play = ExecuteProcess(\n        cmd=['ros2', 'bag', 'play', rosbag_path, '--loop', '--remap',\n             '/image:=/r2b/ros2_input_image'],\n        output='screen')\n\n    # Follow the POL test: two containers connected via a bridge image topic\n    ros2_converter_1 = ComposableNode(\n        name='ros2_converter',\n        namespace='r2b',\n        package='isaac_ros_nitros_bridge_ros2',\n        plugin='nvidia::isaac_ros::nitros_bridge::ImageConverterNode',\n        parameters=[{\n            'num_blocks': 40\n        }],\n        remappings=[\n            ('ros2_output_bridge_image', 'ros2_converter_output')\n        ])\n\n    ros2_converter_2 = ComposableNode(\n        name='ros2_converter',\n        namespace='r2b',\n        package='isaac_ros_nitros_bridge_ros2',\n        plugin='nvidia::isaac_ros::nitros_bridge::ImageConverterNode',\n        parameters=[{\n            'num_blocks': 40\n        }],\n        remappings=[\n            ('ros2_input_bridge_image', 'ros2_converter_output')\n        ])\n\n    ros2_converter_1_container = ComposableNodeContainer(\n        name='ros2_converter_1_container',\n        namespace='r2b',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[ros2_converter_1],\n        output='screen',\n        arguments=['--ros-args', '--log-level', 'info']\n    )\n\n    ros2_converter_2_container = ComposableNodeContainer(\n        name='ros2_converter_2_container',\n        namespace='r2b',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[ros2_converter_2],\n        output='screen',\n        arguments=['--ros-args', '--log-level', 'info']\n    )\n    return [rosbag_play, ros2_converter_1_container, ros2_converter_2_container]\n\n\ndef generate_launch_description():\n    launch_args = [\n        DeclareLaunchArgument(\n            'rosbag_path',\n            description='Path of the r2b rosbag')\n    ]\n\n    nodes = launch_args + [OpaqueFunction(function=launch_setup)]\n    return launch.LaunchDescription(\n        nodes + [\n            # Start tests after a fixed delay for node startup\n            launch.actions.TimerAction(\n                period=30.0,\n                actions=[launch_testing.actions.ReadyToTest()])\n        ]\n    )\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/launch/isaac_ros_nitros_bridge_tensor_list_converter.launch.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport launch\n\nfrom launch.actions import DeclareLaunchArgument, OpaqueFunction\nfrom launch.substitutions import LaunchConfiguration\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing.actions\n\n\ndef launch_setup(context):\n    pub_tensor_list_name = LaunchConfiguration('pub_tensor_list_name').perform(context)\n    sub_tensor_list_name = LaunchConfiguration('sub_tensor_list_name').perform(context)\n\n    ros2_converter = ComposableNode(\n        name='ros2_converter',\n        namespace='',\n        package='isaac_ros_nitros_bridge_ros2',\n        plugin='nvidia::isaac_ros::nitros_bridge::TensorListConverterNode',\n        remappings=[\n            ('ros2_output_bridge_tensor_list', 'ros1_input_bridge_tensor_list'),\n            ('ros2_output_tensor_list', pub_tensor_list_name),\n            ('ros2_input_bridge_tensor_list', 'pynitros_tensor_list'),\n            ('ros2_input_tensor_list', sub_tensor_list_name),\n        ])\n\n    container = ComposableNodeContainer(\n        name='ros2_converter_container',\n        namespace='',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[ros2_converter],\n        output='screen',\n        arguments=['--ros-args', '--log-level', 'info']\n    )\n    return [container]\n\n\ndef generate_launch_description():\n    launch_args = [\n        DeclareLaunchArgument(\n            'pub_tensor_list_name',\n            description='Remapped publish image name of NITROS Bridge ROS2'),\n        DeclareLaunchArgument(\n            'sub_tensor_list_name',\n            description='Remapped subscribe image name of NITROS Bridge ROS2'),\n    ]\n\n    nodes = launch_args + [OpaqueFunction(function=launch_setup)]\n    return launch.LaunchDescription(\n        nodes + [\n            # Start tests after a fixed delay for node startup\n            launch.actions.TimerAction(\n                period=30.0,\n                actions=[launch_testing.actions.ReadyToTest()])\n        ]\n    )\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\n SPDX-FileCopyrightText: Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n\n Licensed under the Apache License, Version 2.0 (the \"License\");\n you may not use this file except in compliance with the License.\n You may obtain a copy of the License at\n\n http://www.apache.org/licenses/LICENSE-2.0\n\n Unless required by applicable law or agreed to in writing, software\n distributed under the License is distributed on an \"AS IS\" BASIS,\n WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n See the License for the specific language governing permissions and\n limitations under the License.\n\n SPDX-License-Identifier: Apache-2.0\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_bridge_ros2</name>\n  <version>4.4.0</version>\n  <description>Converter between NITROS bridge messages and ROS 2 messages</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache-2.0</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Yuankun Zhu</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n  <build_depend>isaac_ros_common</build_depend>\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>std_msgs</depend>\n  <depend>libboost-dev</depend>\n  <depend>isaac_ros_managed_nitros</depend>\n  <depend>isaac_ros_nitros_image_type</depend>\n  <depend>isaac_ros_nitros_tensor_list_type</depend>\n  <depend>isaac_ros_nitros_bridge_interfaces</depend>\n  <depend>isaac_ros_test</depend>\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/src/image_converter_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n\n#include <cuda_runtime_api.h>\n#include <sys/un.h>\n#include <string>\n\n#include \"isaac_ros_nitros_bridge_ros2/image_converter_node.hpp\"\n#include \"sensor_msgs/image_encodings.hpp\"\n\n#define SYS_pidfd_getfd_nitros_bridge 438\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros_bridge\n{\n\nImageConverterNode::ImageConverterNode(const rclcpp::NodeOptions options)\n: rclcpp::Node(\"image_converter_node\", options),\n  pub_nitros_image_type_(\n    declare_parameter<std::string>(\"pub_nitros_image_type\", \"nitros_image_rgb8\")),\n  sub_nitros_image_type_(\n    declare_parameter<std::string>(\"sub_nitros_image_type\", \"nitros_image_rgb8\")),\n  num_blocks_(declare_parameter<int64_t>(\"num_blocks\", 40)),\n  // Timeout in microseconds: duration to wait after refcount reaches 0 before recycling the buffer\n  timeout_(declare_parameter<int64_t>(\"timeout\", 500)),\n  bridge_pub_qos_{::isaac_ros::common::AddQosParameter(*this, \"DEFAULT\", \"bridge_pub_qos\")},\n  bridge_sub_qos_{::isaac_ros::common::AddQosParameter(*this, \"DEFAULT\", \"bridge_sub_qos\")},\n  nitros_pub_qos_{::isaac_ros::common::AddQosParameter(*this, \"DEFAULT\", \"nitros_pub_qos\")},\n  nitros_sub_qos_{::isaac_ros::common::AddQosParameter(*this, \"DEFAULT\", \"nitros_sub_qos\")}\n{\n  cudaSetDevice(0);\n  cuDevicePrimaryCtxRetain(&ctx_, 0);\n\n  cudaEventCreateWithFlags(&event_, cudaEventInterprocess | cudaEventDisableTiming);\n  cudaIpcGetEventHandle(reinterpret_cast<cudaIpcEventHandle_t *>(&ipc_event_handle_), event_);\n\n  bridge_image_pub_ = create_publisher<isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeImage>(\n    \"ros2_output_bridge_image\", bridge_pub_qos_);\n\n  bridge_image_sub_ = create_subscription<\n    isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeImage>(\n    \"ros2_input_bridge_image\", bridge_sub_qos_,\n    std::bind(&ImageConverterNode::BridgeToROSCallback, this, std::placeholders::_1));\n\n  nitros_pub_ = std::make_shared<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n        nvidia::isaac_ros::nitros::NitrosImage>>(\n    this, \"ros2_output_image\", pub_nitros_image_type_,\n    nvidia::isaac_ros::nitros::NitrosDiagnosticsConfig{}, nitros_pub_qos_);\n\n  nitros_sub_ = std::make_shared<nvidia::isaac_ros::nitros::ManagedNitrosSubscriber<\n        nvidia::isaac_ros::nitros::NitrosImageView>>(\n    this, \"ros2_input_image\", sub_nitros_image_type_,\n    std::bind(&ImageConverterNode::ROSToBridgeCallback, this, std::placeholders::_1),\n    nvidia::isaac_ros::nitros::NitrosDiagnosticsConfig{}, nitros_sub_qos_);\n}\n\nImageConverterNode::~ImageConverterNode() = default;\n\nvoid ImageConverterNode::BridgeToROSCallback(\n  const isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeImage::SharedPtr msg)\n{\n  cuCtxSetCurrent(ctx_);\n\n  CUdeviceptr gpu_buffer = 0ULL;\n  CUmemGenericAllocationHandle generic_allocation_handle;\n\n  auto pid = msg->data[0];\n  auto fd = msg->data[1];\n\n  auto msg_uid = msg->uid;\n  cudaEvent_t event;\n  cudaIpcEventHandle_t event_handle;\n\n  // Construct CUDA IPC event handle if it exists\n  if (msg->cuda_event_handle.size() != 0) {\n    if (msg->cuda_event_handle.size() != sizeof(cudaIpcEventHandle_t)) {\n      RCLCPP_ERROR(this->get_logger(), \"Invalid event handle size.\");\n      return;\n    }\n    memcpy(&event_handle, msg->cuda_event_handle.data(), sizeof(cudaIpcEventHandle_t));\n    auto err = cudaIpcOpenEventHandle(&event, event_handle);\n    if (err != cudaSuccess) {\n      RCLCPP_ERROR(\n        this->get_logger(), \"cudaIpcOpenEventHandle failed: %s\",\n        cudaGetErrorString(err));\n      return;\n    }\n\n    // The event may record the completion of the previous operation\n    err = cudaEventSynchronize(event);\n    if (err != cudaSuccess) {\n      RCLCPP_ERROR(\n        this->get_logger(), \"CUDA event synchronize failed: %s\",\n        cudaGetErrorString(err));\n      return;\n    }\n  }\n\n  // Compare UID if exists\n  if (!msg_uid.empty()) {\n    std::string shm_name = std::to_string(pid) + std::to_string(fd);\n    host_ipc_buffer_ = std::make_shared<HostIPCBuffer>(shm_name, HostIPCBuffer::Mode::OPEN);\n    if (!host_ipc_buffer_->refcoun_inc_if_uid_match(msg_uid)) {\n      RCLCPP_WARN(this->get_logger(), \"Failed to match UID, skip.\");\n      return;\n    }\n  }\n\n  if (handle_ptr_map_.find(msg->data.data()[1]) != handle_ptr_map_.end()) {\n    gpu_buffer = handle_ptr_map_[msg->data.data()[1]];\n    RCLCPP_DEBUG(this->get_logger(), \"Found FD in local map.\");\n  } else {\n    int pidfd = syscall(SYS_pidfd_open, msg->data.data()[0], 0);\n    if (pidfd <= 0) {\n      perror(\"SYS_pidfd_open failed\");\n    }\n    int fd = syscall(SYS_pidfd_getfd_nitros_bridge, pidfd, msg->data.data()[1], 0);\n    if (fd <= 0) {\n      perror(\"SYS_pidfd_getfd failed\");\n    }\n\n    auto cuda_err = cuMemImportFromShareableHandle(\n      &generic_allocation_handle,\n      reinterpret_cast<void *>((uintptr_t)fd),\n      CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(\n        this->get_logger(), \"Failed to call cuMemImportFromShareableHandle %s\",\n        error_str);\n      throw std::runtime_error(\"[NITROS Bridge] cuMemImportFromShareableHandle Error\");\n    }\n\n    CUmemAllocationProp prop = {};\n    prop.type = CU_MEM_ALLOCATION_TYPE_PINNED;\n    prop.location.type = CU_MEM_LOCATION_TYPE_DEVICE;\n    prop.location.id = 0;\n    prop.requestedHandleTypes = CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR;\n    size_t granularity = 0;\n\n    cuda_err = cuMemGetAllocationGranularity(\n      &granularity, &prop, CU_MEM_ALLOC_GRANULARITY_MINIMUM);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(\n        this->get_logger(), \"Failed to call cuMemGetAllocationGranularity %s\",\n        error_str);\n      throw std::runtime_error(\n              \"[NITROS Bridge] cuMemGetAllocationGranularity Error\");\n    }\n\n    auto alloc_size = msg->height * msg->step;\n    // The alloc size must be the integral multiple of granularity\n    alloc_size = alloc_size - (alloc_size % granularity) + granularity;\n\n    cuda_err = cuMemAddressReserve(&gpu_buffer, alloc_size, 0, 0, 0);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(this->get_logger(), \"Failed to call cuMemAddressReserve %s\", error_str);\n      throw std::runtime_error(\"[NITROS Bridge] cuMemAddressReserve Error\");\n    }\n\n    cuda_err = cuMemMap(gpu_buffer, alloc_size, 0, generic_allocation_handle, 0);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(this->get_logger(), \"Failed to call cuMemMap %s\", error_str);\n      throw std::runtime_error(\"[NITROS Bridge] cuMemMap Error\");\n    }\n\n    CUmemAccessDesc accessDesc = {};\n    accessDesc.location.type = CU_MEM_LOCATION_TYPE_DEVICE;\n    accessDesc.location.id = 0;\n    accessDesc.flags = CU_MEM_ACCESS_FLAGS_PROT_READWRITE;\n    cuda_err = cuMemSetAccess(gpu_buffer, alloc_size, &accessDesc, 1);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(this->get_logger(), \"Failed to call cuMemSetAccess %s\", error_str);\n      throw std::runtime_error(\"[NITROS Bridge] cuMemMap Error\");\n    }\n    handle_ptr_map_[msg->data.data()[1]] = gpu_buffer;\n  }\n\n  auto host_ipc_buffer_ptr = host_ipc_buffer_;\n  nvidia::isaac_ros::nitros::NitrosImage nitros_image =\n    nvidia::isaac_ros::nitros::NitrosImageBuilder()\n    .WithHeader(msg->header)\n    .WithEncoding(msg->encoding)\n    .WithDimensions(msg->height, msg->width)\n    .WithGpuData(reinterpret_cast<void *>(gpu_buffer))\n    .WithReleaseCallback(\n    [host_ipc_buffer_ptr]() {\n      if (host_ipc_buffer_ptr) {\n        host_ipc_buffer_ptr->refcount_dec();\n      }\n    })\n    .Build();\n\n  nitros_pub_->publish(nitros_image);\n\n  RCLCPP_DEBUG(this->get_logger(), \"NITROS Image is Published from NITROS Bridge.\");\n}\n\nvoid ImageConverterNode::ROSToBridgeCallback(const nvidia::isaac_ros::nitros::NitrosImageView view)\n{\n  cuCtxSetCurrent(ctx_);\n\n  if (first_msg_received_ == false) {\n    ipc_buffer_manager_ = std::make_shared<IPCBufferManager>(\n      num_blocks_, view.GetSizeInBytes(), timeout_);\n    first_msg_received_ = true;\n  }\n\n  auto ipc_buffer = ipc_buffer_manager_->find_next_available_buffer();\n\n  isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeImage img_msg;\n  img_msg.header.frame_id = view.GetFrameId();\n  img_msg.header.stamp.sec = view.GetTimestampSeconds();\n  img_msg.header.stamp.nanosec = view.GetTimestampNanoseconds();\n  img_msg.height = view.GetHeight();\n  img_msg.width = view.GetWidth();\n  img_msg.encoding = view.GetEncoding();\n  img_msg.step = view.GetSizeInBytes() / view.GetHeight();\n\n  auto cuda_err = cuMemcpyDtoD(\n    ipc_buffer->d_ptr,\n    (CUdeviceptr)(view.GetGpuData()),\n    view.GetSizeInBytes());\n  if (CUDA_SUCCESS != cuda_err) {\n    const char * error_str = NULL;\n    cuGetErrorString(cuda_err, &error_str);\n    RCLCPP_ERROR(\n      this->get_logger(), \"Failed to call cuMemcpyDtoD %s\",\n      error_str);\n    throw std::runtime_error(\"[NITROS Bridge] cuMemcpyDtoD Error\");\n  }\n\n  // cuMemcpyDtoD is an aysnchronize call, wait until it complete.\n  cuCtxSynchronize();\n\n  img_msg.data.push_back(ipc_buffer->pid);\n  img_msg.data.push_back(ipc_buffer->fd);\n  img_msg.uid = ipc_buffer->uid;\n  img_msg.device_id = 0;\n\n  bridge_image_pub_->publish(img_msg);\n}\n\n}  // namespace nitros_bridge\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n// Register as component\n#include \"rclcpp_components/register_node_macro.hpp\"\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros_bridge::ImageConverterNode)\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/src/tensor_list_converter_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n\n#include <cuda_runtime_api.h>\n#include <sys/un.h>\n#include <string>\n#include <atomic>\n\n#include \"isaac_ros_nitros_bridge_ros2/tensor_list_converter_node.hpp\"\n\n\n#define SYS_pidfd_getfd_nitros_bridge 438\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros_bridge\n{\n\nTensorListConverterNode::TensorListConverterNode(const rclcpp::NodeOptions options)\n: rclcpp::Node(\"tensor_list_converter_node\", options),\n  num_blocks_(declare_parameter<int64_t>(\"num_blocks\", 40)),\n  // Timeout in microseconds: duration to wait after refcount reaches 0 before recycling the buffer\n  timeout_(declare_parameter<int64_t>(\"timeout\", 500)),\n  nitros_pub_qos_{::isaac_ros::common::AddQosParameter(*this, \"DEFAULT\", \"nitros_pub_qos\")},\n  nitros_sub_qos_{::isaac_ros::common::AddQosParameter(*this, \"DEFAULT\", \"nitros_sub_qos\")},\n  bridge_pub_qos_{::isaac_ros::common::AddQosParameter(*this, \"DEFAULT\", \"bridge_pub_qos\")},\n  bridge_sub_qos_{::isaac_ros::common::AddQosParameter(*this, \"DEFAULT\", \"bridge_sub_qos\")}\n{\n  cudaSetDevice(0);\n  cuDevicePrimaryCtxRetain(&ctx_, 0);\n\n  cudaEventCreateWithFlags(&event_, cudaEventInterprocess | cudaEventDisableTiming);\n  cudaIpcGetEventHandle(reinterpret_cast<cudaIpcEventHandle_t *>(&ipc_event_handle_), event_);\n\n  nitros_pub_ = std::make_shared<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n        nvidia::isaac_ros::nitros::NitrosTensorList>>(\n    this, \"ros2_output_tensor_list\",\n    nvidia::isaac_ros::nitros::nitros_tensor_list_nhwc_t::supported_type_name,\n    nvidia::isaac_ros::nitros::NitrosDiagnosticsConfig{}, nitros_pub_qos_);\n\n  nitros_sub_ = std::make_shared<nvidia::isaac_ros::nitros::ManagedNitrosSubscriber<\n        nvidia::isaac_ros::nitros::NitrosTensorListView>>(\n    this, \"ros2_input_tensor_list\",\n    nvidia::isaac_ros::nitros::nitros_tensor_list_nhwc_t::supported_type_name,\n    std::bind(&TensorListConverterNode::ROSToBridgeCallback, this, std::placeholders::_1),\n    nvidia::isaac_ros::nitros::NitrosDiagnosticsConfig{}, nitros_sub_qos_);\n\n  nitros_bridge_pub_ = create_publisher<\n    isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeTensorList>(\n    \"ros2_output_bridge_tensor_list\", bridge_pub_qos_);\n  nitros_bridge_sub_ = create_subscription<\n    isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeTensorList>(\n    \"ros2_input_bridge_tensor_list\", bridge_sub_qos_,\n    std::bind(&TensorListConverterNode::BridgeToROSCallback, this, std::placeholders::_1));\n}\n\nTensorListConverterNode::~TensorListConverterNode() = default;\n\nvoid TensorListConverterNode::BridgeToROSCallback(\n  const isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeTensorList::SharedPtr msg)\n{\n  cuCtxSetCurrent(ctx_);\n\n  CUdeviceptr gpu_buffer = 0ULL;\n  CUmemGenericAllocationHandle generic_allocation_handle;\n\n  auto msg_pid = msg->pid;\n  auto msg_fd = msg->fd;\n  auto msg_uid = msg->uid;\n\n  cudaEvent_t event;\n  cudaIpcEventHandle_t event_handle;\n  // Construct CUDA IPC event handle if it exists\n  if (msg->cuda_event_handle.size() != 0) {\n    if (msg->cuda_event_handle.size() != sizeof(cudaIpcEventHandle_t)) {\n      RCLCPP_ERROR(this->get_logger(), \"Invalid event handle size.\");\n      return;\n    }\n    memcpy(&event_handle, msg->cuda_event_handle.data(), sizeof(cudaIpcEventHandle_t));\n    auto err = cudaIpcOpenEventHandle(&event, event_handle);\n    if (err != cudaSuccess) {\n      fprintf(stderr, \"cudaIpcOpenEventHandle failed: %s\\n\", cudaGetErrorString(err));\n      return;\n    }\n\n    // The event may record the completion of the previous operation\n    err = cudaEventSynchronize(event);\n    if (err != cudaSuccess) {\n      fprintf(stderr, \"CUDA event synchronize failed: %s\\n\", cudaGetErrorString(err));\n      return;\n    }\n  }\n\n  // Get total size of all tensors\n  size_t total_size = 0;\n  for (size_t i = 0; i < msg->tensors.size(); i++) {\n    auto tensor = msg->tensors[i];\n    if (tensor.shape.rank == 0) {\n      RCLCPP_INFO(this->get_logger(), \"Received tensor with rank 0, skip.\");\n      continue;\n    }\n    if (tensor.shape.dims.size() == 0) {\n      RCLCPP_ERROR(this->get_logger(), \"Invalid tensor shape.\");\n      return;\n    }\n    if (tensor.strides.size() == 0) {\n      RCLCPP_ERROR(this->get_logger(), \"Invalid tensor strides.\");\n      return;\n    }\n    total_size += tensor.strides[0] * tensor.shape.dims[0];\n  }\n\n  // Compare UID if exists\n  if (!msg_uid.empty()) {\n    std::string shm_name = std::to_string(msg_pid) + std::to_string(msg_fd);\n    host_ipc_buffer_ = std::make_shared<HostIPCBuffer>(shm_name, HostIPCBuffer::Mode::OPEN);\n    if (!host_ipc_buffer_->refcoun_inc_if_uid_match(msg_uid)) {\n      RCLCPP_WARN(this->get_logger(), \"Failed to match UID, skip.\");\n      return;\n    }\n  }\n\n  if (handle_ptr_map_.find(msg_fd) != handle_ptr_map_.end()) {\n    gpu_buffer = handle_ptr_map_[msg_fd];\n    RCLCPP_DEBUG(this->get_logger(), \"Found FD in local map.\");\n  } else {\n    int pidfd = syscall(SYS_pidfd_open, msg_pid, 0);\n    if (pidfd <= 0) {\n      perror(\"SYS_pidfd_open failed\");\n    }\n    int fd = syscall(SYS_pidfd_getfd_nitros_bridge, pidfd, msg_fd, 0);\n    if (fd <= 0) {\n      perror(\"SYS_pidfd_getfd failed\");\n    }\n\n    auto cuda_err = cuMemImportFromShareableHandle(\n      &generic_allocation_handle,\n      reinterpret_cast<void *>((uintptr_t)fd),\n      CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(\n        this->get_logger(), \"Failed to call cuMemImportFromShareableHandle %s\",\n        error_str);\n      throw std::runtime_error(\n              \"[NITROS Bridge] cuMemImportFromShareableHandle Error\");\n    }\n\n    CUmemAllocationProp prop = {};\n    prop.type = CU_MEM_ALLOCATION_TYPE_PINNED;\n    prop.location.type = CU_MEM_LOCATION_TYPE_DEVICE;\n    prop.location.id = 0;\n    prop.requestedHandleTypes = CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR;\n    size_t granularity = 0;\n\n    cuda_err = cuMemGetAllocationGranularity(\n      &granularity, &prop, CU_MEM_ALLOC_GRANULARITY_MINIMUM);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(\n        this->get_logger(), \"Failed to call cuMemGetAllocationGranularity %s\",\n        error_str);\n      throw std::runtime_error(\n              \"[NITROS Bridge] cuMemGetAllocationGranularity Error\");\n    }\n\n    // The alloc size must be the integral multiple of granularity\n    auto alloc_size = total_size - (total_size % granularity) + granularity;\n\n    cuda_err = cuMemAddressReserve(&gpu_buffer, alloc_size, 0, 0, 0);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(this->get_logger(), \"Failed to call cuMemAddressReserve %s\", error_str);\n      throw std::runtime_error(\n              \"[NITROS Bridge] cuMemAddressReserve Error\");\n    }\n\n    cuda_err = cuMemMap(gpu_buffer, alloc_size, 0, generic_allocation_handle, 0);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(this->get_logger(), \"Failed to call cuMemMap %s\", error_str);\n      throw std::runtime_error(\n              \"[NITROS Bridge] cuMemMap Error\");\n    }\n\n    CUmemAccessDesc accessDesc = {};\n    accessDesc.location.type = CU_MEM_LOCATION_TYPE_DEVICE;\n    accessDesc.location.id = 0;\n    accessDesc.flags = CU_MEM_ACCESS_FLAGS_PROT_READWRITE;\n    cuda_err = cuMemSetAccess(gpu_buffer, alloc_size, &accessDesc, 1);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(this->get_logger(), \"Failed to call cuMemSetAccess %s\", error_str);\n      throw std::runtime_error(\n              \"[NITROS Bridge] cuMemSetAccess Error\");\n    }\n    handle_ptr_map_[msg_fd] = gpu_buffer;\n  }\n\n  auto nitros_tensor_list_builder =\n    nvidia::isaac_ros::nitros::NitrosTensorListBuilder()\n    .WithHeader(msg->header);\n\n  // Create a shared counter for all tensors from this message\n  auto tensor_list_counter = std::make_shared<std::atomic<int>>(msg->tensors.size());\n\n  int offset = 0;\n  auto host_ipc_buffer_ptr = host_ipc_buffer_;\n  for (size_t i = 0; i < msg->tensors.size(); i++) {\n    auto ros_tensor = msg->tensors[i];\n    auto tensor_shape = std::vector<int32_t>{\n      ros_tensor.shape.dims.begin(), ros_tensor.shape.dims.end()};\n\n    auto cur_tensor = nvidia::isaac_ros::nitros::NitrosTensorBuilder()\n      .WithShape(nvidia::isaac_ros::nitros::NitrosTensorShape(tensor_shape))\n      .WithDataType(nvidia::isaac_ros::nitros::NitrosDataType::kFloat32)\n      .WithData(reinterpret_cast<void *>(gpu_buffer + offset))\n      .WithReleaseCallback(\n      [host_ipc_buffer_ptr, tensor_list_counter]() {\n        if (host_ipc_buffer_ptr) {\n          if (tensor_list_counter->fetch_sub(1) == 1) {\n            host_ipc_buffer_ptr->refcount_dec();\n          }\n        }\n      })\n      .Build();\n    nitros_tensor_list_builder.AddTensor(ros_tensor.name.c_str(), cur_tensor);\n    offset += ros_tensor.data.size();\n  }\n\n  auto nitros_tensor_list = nitros_tensor_list_builder.Build();\n\n  nitros_pub_->publish(nitros_tensor_list);\n\n  RCLCPP_DEBUG(this->get_logger(), \"NITROS Tensor List is Published from NITROS Bridge.\");\n}\n\nvoid TensorListConverterNode::ROSToBridgeCallback(\n  const nvidia::isaac_ros::nitros::NitrosTensorListView view)\n{\n  cuCtxSetCurrent(ctx_);\n\n  auto tensor_count = view.GetTensorCount();\n  // Get total size first\n  if (tensor_count == 0) {\n    RCLCPP_INFO(this->get_logger(), \"No tensor found in the list.\");\n    return;\n  }\n  auto tensor_list = view.GetAllTensor();\n  size_t total_size = 0;\n\n  // Get total size of all tensors\n  for (size_t i = 0; i < tensor_count; i++) {\n    auto tensor = tensor_list[i];\n    auto tensor_bytes_per_element = tensor.GetBytesPerElement();\n    auto tensor_element_count = tensor.GetElementCount();\n    total_size += tensor_element_count * tensor_bytes_per_element;\n  }\n\n  // Create IPC buffer manager\n  if (first_msg_received_ == false) {\n    ipc_buffer_manager_ = std::make_shared<IPCBufferManager>(\n      num_blocks_, total_size, timeout_);\n    first_msg_received_ = true;\n  }\n\n  auto ipc_buffer = ipc_buffer_manager_->find_next_available_buffer();\n  isaac_ros_nitros_bridge_interfaces::msg::NitrosBridgeTensorList tensor_list_msg;\n  tensor_list_msg.header.frame_id = view.GetFrameId();\n  tensor_list_msg.header.stamp.sec = view.GetTimestampSeconds();\n  tensor_list_msg.header.stamp.nanosec = view.GetTimestampNanoseconds();\n\n  size_t offset = 0;\n  for (size_t i = 0; i < tensor_count; i++) {\n    auto tensor = tensor_list[i];\n    auto tensor_shape = tensor.GetShape().shape();\n    auto tensor_rank = tensor.GetRank();\n    auto tensor_element_type = tensor.GetElementType();\n    auto tensor_name = tensor.GetName();\n    auto tensor_bytes_per_element = tensor.GetBytesPerElement();\n    auto tensor_element_count = tensor.GetElementCount();\n    std::vector<uint32_t> tensor_dims;\n    std::vector<uint64_t> tensor_strides;\n    for (uint32_t i = 0; i < tensor_rank; ++i) {\n      tensor_dims.push_back(tensor_shape.dimension(i));\n    }\n\n    isaac_ros_tensor_list_interfaces::msg::Tensor ros2_tensor;\n    ros2_tensor.name = tensor_name;\n    ros2_tensor.shape.dims = tensor_dims;\n    ros2_tensor.data_type = static_cast<uint8_t>(tensor_element_type);\n    ros2_tensor.shape.rank = tensor_rank;\n    ros2_tensor.strides = tensor.GetStrides();\n\n    auto cuda_err = cuMemcpyDtoD(\n      ipc_buffer->d_ptr + offset,\n      (CUdeviceptr)(tensor.GetBuffer()),\n      tensor_element_count * tensor_bytes_per_element);\n    if (CUDA_SUCCESS != cuda_err) {\n      const char * error_str = NULL;\n      cuGetErrorString(cuda_err, &error_str);\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"\"), \"Failed to call cuMemcpyDtoD %s\",\n        error_str);\n      throw std::runtime_error(\"[NITROS Bridge] cuMemcpyDtoD Error\");\n    }\n    offset += tensor_element_count * tensor_bytes_per_element;\n\n    tensor_list_msg.tensors.push_back(ros2_tensor);\n  }\n\n  // cuMemcpyDtoD is an aysnchronize call, wait until it complete.\n  cuCtxSynchronize();\n\n  tensor_list_msg.pid = ipc_buffer->pid;\n  tensor_list_msg.fd = ipc_buffer->fd;\n  tensor_list_msg.uid = ipc_buffer->uid;\n  tensor_list_msg.device_id = 0;\n\n  nitros_bridge_pub_->publish(tensor_list_msg);\n}\n\n}  // namespace nitros_bridge\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n// Register as component\n#include \"rclcpp_components/register_node_macro.hpp\"\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros_bridge::TensorListConverterNode)\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/test/isaac_ros_nitros_bridge_image_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\"\"\"\nProof-of-Life test for interprocess NITROS bridge image on ROS 2.\n\nNITROSBridgeImageConverter1(NITROSImage->NITROSBridgeImage)\nNITROSBridgeImageConverter2(NITROSBridgeImage->NITROSImage)\n\"\"\"\n\nimport os\nimport pathlib\nimport subprocess\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import Image\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    ptace_result = subprocess.run(['cat', '/proc/sys/kernel/yama/ptrace_scope'],\n                                  stdout=subprocess.PIPE,\n                                  stderr=subprocess.PIPE,\n                                  text=True)\n    pidfd_getfd_result = subprocess.run(['grep', 'pidfd_getfd', '/proc/kallsyms'],\n                                        stdout=subprocess.PIPE,\n                                        stderr=subprocess.PIPE,\n                                        text=True)\n    if (ptace_result.stdout.strip() == '0' and pidfd_getfd_result.returncode == 0):\n        ros2_converter_1 = ComposableNode(\n            name='ros2_converter',\n            namespace='r2b',\n            package='isaac_ros_nitros_bridge_ros2',\n            plugin='nvidia::isaac_ros::nitros_bridge::ImageConverterNode',\n            parameters=[{\n                'num_blocks': 5\n            }],\n            remappings=[\n                ('ros2_output_bridge_image', 'ros2_converter_output'),\n            ])\n\n        ros2_converter_2 = ComposableNode(\n            name='ros2_converter',\n            namespace='r2b',\n            package='isaac_ros_nitros_bridge_ros2',\n            plugin='nvidia::isaac_ros::nitros_bridge::ImageConverterNode',\n            parameters=[{\n                'num_blocks': 5\n            }],\n            remappings=[('ros2_input_bridge_image', 'ros2_converter_output')])\n\n        ros2_converter_1_container = ComposableNodeContainer(\n            name='ros2_converter_1_container',\n            namespace='r2b',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[ros2_converter_1],\n            output='screen',\n            arguments=['--ros-args', '--log-level', 'info'])\n\n        ros2_converter_2_container = ComposableNodeContainer(\n            name='ros2_converter_2_container',\n            namespace='r2b',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[ros2_converter_2],\n            output='screen',\n            arguments=['--ros-args', '--log-level', 'info'])\n\n        return IsaacROSNitrosBridgeTest.generate_test_description([\n            ros2_converter_1_container, ros2_converter_2_container,\n            launch.actions.TimerAction(period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n        ])\n    else:\n        IsaacROSNitrosBridgeTest.skip_test = True\n        return IsaacROSNitrosBridgeTest.generate_test_description(\n            [launch_testing.actions.ReadyToTest()])\n\n\nclass IsaacROSNitrosBridgeTest(IsaacROSBaseTest):\n    \"\"\"Validate Nitros Bridge on Image type.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n    skip_test = False\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_image')\n    def test_nitros_bridge_image(self, test_folder) -> None:\n        \"\"\"Expect the image received from NitrosImage type conversion to be identical to source.\"\"\"\n        if self.skip_test:\n            self.skipTest('No ptrace permission! Skipping test.')\n        else:\n            IsaacROSNitrosBridgeTest.DEFAULT_NAMESPACE = 'r2b'\n            self.generate_namespace_lookup(['ros2_input_image', 'ros2_output_image'])\n            received_messages = {}\n\n            received_image_sub = self.create_logging_subscribers(\n                subscription_requests=[('ros2_output_image', Image)],\n                received_messages=received_messages)\n\n            image_pub = self.node.create_publisher(Image, self.namespaces['ros2_input_image'],\n                                                   self.DEFAULT_QOS)\n\n            try:\n                image = JSONConversion.load_image_from_json(test_folder / 'image.json')\n                timestamp = self.node.get_clock().now().to_msg()\n                image.header.stamp = timestamp\n\n                # Wait at most TIMEOUT seconds for subscriber to respond\n                TIMEOUT = 30\n                end_time = time.time() + TIMEOUT\n\n                done = False\n                while time.time() < end_time:\n                    image_pub.publish(image)\n                    rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                    # If we have received a message on the output topic, break\n                    if 'ros2_output_image' in received_messages:\n                        done = True\n                        break\n\n                self.assertTrue(done, \"Didn't receive output on output_image topic!\")\n\n                received_image = received_messages['ros2_output_image']\n\n                print(f'Source image data size: {len(image.data)}')\n                print(f'Received image data size: {len(received_image.data)}')\n\n                self.assertEqual(str(timestamp), str(received_image.header.stamp),\n                                 'Timestamps do not match.')\n\n                self.assertEqual(\n                    len(image.data), len(received_image.data),\n                    'Source and received image sizes do not match: ' +\n                    f'{len(image.data)} != {len(received_image.data)}')\n\n                # Make sure that the source and received images are the same\n                self.assertEqual(\n                    received_image.height, image.height,\n                    'Source and received image heights do not match: ' +\n                    f'{image.height} != {received_image.height}')\n                self.assertEqual(\n                    received_image.width, image.width,\n                    'Source and received image widths do not match: ' +\n                    f'{image.width} != {received_image.width}')\n\n                for i in range(len(image.data)):\n                    self.assertEqual(image.data[i], received_image.data[i],\n                                     'Source and received image pixels do not match')\n\n            finally:\n                self.node.destroy_subscription(received_image_sub)\n                self.node.destroy_publisher(image_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/test/isaac_ros_nitros_bridge_tensor_list_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\"\"\"\nProof-of-Life test for interprocess NITROS bridge tensor list on ROS 2.\n\nNITROSBridgeTensorListConverter1(NITROSTensorList->NITROSBridgeTensorList)\nNITROSBridgeTensorListConverter2(NITROSBridgeTensorList->NITROSTensorList)\n\"\"\"\n\nimport os\nimport pathlib\nimport subprocess\nimport time\n\nfrom isaac_ros_tensor_list_interfaces.msg import Tensor, TensorList, TensorShape\nfrom isaac_ros_test import IsaacROSBaseTest\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\nimport numpy as np\n\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    ptace_result = subprocess.run(['cat', '/proc/sys/kernel/yama/ptrace_scope'],\n                                  stdout=subprocess.PIPE,\n                                  stderr=subprocess.PIPE,\n                                  text=True)\n    pidfd_getfd_result = subprocess.run(['grep', 'pidfd_getfd', '/proc/kallsyms'],\n                                        stdout=subprocess.PIPE,\n                                        stderr=subprocess.PIPE,\n                                        text=True)\n    if (ptace_result.stdout.strip() == '0' and pidfd_getfd_result.returncode == 0):\n        tensor_list_converter_1 = ComposableNode(\n            name='ros2_converter',\n            namespace='r2b',\n            package='isaac_ros_nitros_bridge_ros2',\n            plugin='nvidia::isaac_ros::nitros_bridge::TensorListConverterNode',\n            parameters=[{\n                'num_blocks': 5\n            }],\n            remappings=[\n                ('ros2_output_bridge_tensor_list', 'ros2_converter_output'),\n            ])\n\n        tensor_list_converter_2 = ComposableNode(\n            name='ros2_converter',\n            namespace='r2b',\n            package='isaac_ros_nitros_bridge_ros2',\n            plugin='nvidia::isaac_ros::nitros_bridge::TensorListConverterNode',\n            parameters=[{\n                'num_blocks': 5\n            }],\n            remappings=[('ros2_input_bridge_tensor_list', 'ros2_converter_output')])\n\n        tensor_list_converter_container_1 = ComposableNodeContainer(\n            name='ros2_converter_1_container',\n            namespace='r2b',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[tensor_list_converter_1],\n            output='screen',\n            arguments=['--ros-args', '--log-level', 'info'])\n\n        tensor_list_converter_container_2 = ComposableNodeContainer(\n            name='ros2_converter_2_container',\n            namespace='r2b',\n            package='rclcpp_components',\n            executable='component_container_mt',\n            composable_node_descriptions=[tensor_list_converter_2],\n            output='screen',\n            arguments=['--ros-args', '--log-level', 'info'])\n\n        return IsaacROSNitrosBridgeTest.generate_test_description([\n            tensor_list_converter_container_1, tensor_list_converter_container_2,\n            launch.actions.TimerAction(period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n        ])\n    else:\n        IsaacROSNitrosBridgeTest.skip_test = True\n        return IsaacROSNitrosBridgeTest.generate_test_description(\n            [launch_testing.actions.ReadyToTest()])\n\n\nclass IsaacROSNitrosBridgeTest(IsaacROSBaseTest):\n    \"\"\"Validate Nitros Bridge on TensorList type.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n    skip_test = False\n\n    def test_nitros_bridge_tensor_list(self) -> None:\n        \"\"\"Expect the received tensor list to be identical to source.\"\"\"\n        if self.skip_test:\n            self.skipTest('No ptrace permission! Skipping test.')\n        else:\n            IsaacROSNitrosBridgeTest.DEFAULT_NAMESPACE = 'r2b'\n            self.generate_namespace_lookup(['ros2_input_tensor_list', 'ros2_output_tensor_list'])\n            received_messages = {}\n\n            received_tensor_list_sub = self.create_logging_subscribers(\n                subscription_requests=[('ros2_output_tensor_list', TensorList)],\n                received_messages=received_messages)\n\n            tensor_list_pub = self.node.create_publisher(TensorList,\n                                                         self.namespaces['ros2_input_tensor_list'],\n                                                         self.DEFAULT_QOS)\n\n            try:\n                DATA_TYPE = 9\n                INPUT_TENSOR_DIMENSIONS = [10, 3, 100, 100]\n                INPUT_TENSOR_NAME = 'input'\n                INPUT_TENSOR_STRIDE = 4\n\n                tensor_list = TensorList()\n                tensor = Tensor()\n                tensor_shape = TensorShape()\n\n                tensor_shape.rank = len(INPUT_TENSOR_DIMENSIONS)\n                tensor_shape.dims = INPUT_TENSOR_DIMENSIONS\n\n                tensor.shape = tensor_shape\n                tensor.name = INPUT_TENSOR_NAME\n                tensor.data_type = DATA_TYPE\n                tensor.strides = []\n\n                data_length = INPUT_TENSOR_STRIDE * np.prod(INPUT_TENSOR_DIMENSIONS)\n                tensor.data = np.random.randint(256, size=data_length).tolist()\n\n                tensor_list.tensors = [tensor, tensor]\n                timestamp = self.node.get_clock().now().to_msg()\n                tensor_list.header.stamp = timestamp\n\n                # Wait at most TIMEOUT seconds for subscriber to respond\n                TIMEOUT = 30\n                end_time = time.time() + TIMEOUT\n\n                done = False\n                while time.time() < end_time:\n                    tensor_list_pub.publish(tensor_list)\n                    rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                    # If we have received a message on the output topic, break\n                    if 'ros2_output_tensor_list' in received_messages:\n                        done = True\n                        break\n\n                self.assertTrue(done, \"Didn't receive output on output_tensor_list topic!\")\n\n                received_tensor_list = received_messages['ros2_output_tensor_list']\n                self.assertEqual(len(tensor_list.tensors), len(received_tensor_list.tensors),\n                                 'Source and received tensor list size do not match')\n\n                for i in range(len(tensor_list.tensors)):\n                    source_tensor = tensor_list.tensors[i]\n                    received_tensor = received_tensor_list.tensors[i]\n                    self.assertEqual(source_tensor.name, received_tensor.name,\n                                     'Source and received tensor names do not match')\n                    self.assertEqual(source_tensor.name, received_tensor.name,\n                                     'Source and received tensor names do not match')\n                    self.assertEqual(source_tensor.data_type, received_tensor.data_type,\n                                     'Source and received tensor data types do not match')\n                    self.assertEqual(source_tensor.shape.rank, received_tensor.shape.rank,\n                                     'Source and received tensor ranks do not match')\n                    self.assertEqual(source_tensor.shape.dims, received_tensor.shape.dims,\n                                     'Source and received tensor dimensions do not match')\n                    self.assertEqual(len(source_tensor.data), len(received_tensor.data),\n                                     'Source and received tensor data do not match')\n                    self.assertEqual(str(timestamp), str(received_tensor_list.header.stamp),\n                                     'Timestamps do not match.')\n\n                    for j in range(len(source_tensor.data)):\n                        self.assertEqual(source_tensor.data[j], received_tensor.data[j],\n                                         'Source and received image pixels do not match')\n\n            finally:\n                self.node.destroy_subscription(received_tensor_list_sub)\n                self.node.destroy_publisher(tensor_list_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/isaac_ros_nitros_bridge_ros2/test/test_cases/nitros_image/profile/image.json",
    "content": "{\n    \"image\": \"NVIDIAprofile.png\",\n    \"encoding\": \"bgr8\"\n}"
  },
  {
    "path": "isaac_ros_nitros_bridge/resources/quickstart.bag/metadata.yaml",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:5dd7b3038c4e9a03311d6123502a6f4224417c2360c039c61148e15328b97d5e\nsize 1476\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/resources/quickstart.bag/quickstart.bag_0.db3",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:aca0bfa2cbb013092f93fe3e83dd6608090d88d8b34c0c80dcff5e464f41357e\nsize 418136064\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/resources/quickstart.bag/quickstart.bag_0.db3.zstd",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:14a4d829dc8baf05c2e1c60d7d66d1e924add3cf5d313124deca670050cea989\nsize 232126462\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/resources/unet_sample_data_ros1.bag",
    "content": "version https://git-lfs.github.com/spec/v1\noid sha256:da28d6e18a7c7d2cd7d0a607ae64df097e09e03d104c733a04457ed96f984a96\nsize 2300750\n"
  },
  {
    "path": "isaac_ros_nitros_bridge/scripts/workspace-entrypoint.sh",
    "content": "#!/bin/bash\n#\n# Copyright (c) 2024, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n\n\n# Restart udev daemon\nsudo service udev restart\n\n$@\n"
  },
  {
    "path": "isaac_ros_nitros_topic_tools/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_topic_tools LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\nament_auto_add_library(isaac_ros_nitros_camera_drop_node SHARED\n  src/isaac_ros_nitros_camera_drop_node.cpp\n)\ntarget_compile_definitions(isaac_ros_nitros_camera_drop_node PRIVATE \"TOPIC_TOOLS_BUILDING_LIBRARY\")\n\nrclcpp_components_register_nodes(isaac_ros_nitros_camera_drop_node \"nvidia::isaac_ros::nitros::NitrosCameraDropNode\")\n\nset_target_properties(isaac_ros_nitros_camera_drop_node PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n\nfind_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_topic_tools_camera_drop_node_mode_0_test.py TIMEOUT \"15\")\n  add_launch_test(test/isaac_ros_nitros_topic_tools_camera_drop_node_mode_1_test.py TIMEOUT \"15\")\n  add_launch_test(test/isaac_ros_nitros_topic_tools_camera_drop_node_mode_2_test.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package(INSTALL_TO_SHARE launch)\n"
  },
  {
    "path": "isaac_ros_nitros_topic_tools/include/isaac_ros_nitros_topic_tools/isaac_ros_nitros_camera_drop_node.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_TOPIC_TOOLS__ISAAC_ROS_NITROS_CAMERA_DROP_NODE_HPP_\n#define ISAAC_ROS_NITROS_TOPIC_TOOLS__ISAAC_ROS_NITROS_CAMERA_DROP_NODE_HPP_\n\n#include <message_filters/subscriber.h>\n#include <message_filters/sync_policies/exact_time.h>\n#include <memory>\n#include <string>\n#include <vector>\n\n#include <rclcpp/rclcpp.hpp>\n#include <sensor_msgs/msg/camera_info.hpp>\n#include <sensor_msgs/msg/image.hpp>\n\n#include \"isaac_ros_managed_nitros/managed_nitros_message_filters_subscriber.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_publisher.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_subscriber.hpp\"\n#include \"isaac_ros_nitros_camera_info_type/nitros_camera_info.hpp\"\n#include \"isaac_ros_nitros_image_type/nitros_image.hpp\"\n#include \"isaac_ros_nitros_image_type/nitros_image_view.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n/**\n * @brief NitrosCameraDropNode class implements a node that\n *        synchronizes and drops X out of Y incoming messages.\n *        The node supports three modes:\n *        - Mode 0(mono): Camera + CameraInfo\n *        - Mode 1(stereo): Camera + CameraInfo + Camera + CameraInfo\n *        - Mode 2(mono+depth): Camera + CameraInfo + Depth\n *        The node is based on the ros-tooling/topic_tools::DropNode OSS class.\n *        But the node is modified to drop messages in an evenly spread out fashion.\n *        The node does this by creating a binary array that represents the drop pattern.\n *        The node then uses the binary array to determine if the current message should be\n *        dropped or not.\n */\nclass NitrosCameraDropNode : public rclcpp::Node\n{\npublic:\n  /**\n   * @brief Constructor for NitrosCameraDropNode class.\n   * @param options The node options.\n   */\n  explicit NitrosCameraDropNode(const rclcpp::NodeOptions & options);\n\nprivate:\n  // Subscribers\n  nvidia::isaac_ros::nitros::message_filters::Subscriber<nvidia::isaac_ros::nitros::NitrosImageView>\n  image_sub_1_;\n  ::message_filters::Subscriber<sensor_msgs::msg::CameraInfo> camera_info_sub_1_;\n  nvidia::isaac_ros::nitros::message_filters::Subscriber<nvidia::isaac_ros::nitros::NitrosImageView>\n  image_sub_2_;\n  ::message_filters::Subscriber<sensor_msgs::msg::CameraInfo> camera_info_sub_2_;\n  nvidia::isaac_ros::nitros::message_filters::Subscriber<nvidia::isaac_ros::nitros::NitrosImageView>\n  depth_sub_;\n\n  std::shared_ptr<\n    nvidia::isaac_ros::nitros::ManagedNitrosPublisher<nvidia::isaac_ros::nitros::NitrosImage>>\n  image_pub_1_;\n  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_pub_1_;\n  std::shared_ptr<\n    nvidia::isaac_ros::nitros::ManagedNitrosPublisher<nvidia::isaac_ros::nitros::NitrosImage>>\n  image_pub_2_;\n  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_pub_2_;\n  std::shared_ptr<\n    nvidia::isaac_ros::nitros::ManagedNitrosPublisher<nvidia::isaac_ros::nitros::NitrosImage>>\n  depth_pub_;\n  // Exact message sync policy\n  using ExactPolicyMode0 = ::message_filters::sync_policies::ExactTime<\n    nvidia::isaac_ros::nitros::NitrosImage, sensor_msgs::msg::CameraInfo>;\n  using ExactSyncMode0 = ::message_filters::Synchronizer<ExactPolicyMode0>;\n  std::shared_ptr<ExactSyncMode0> exact_sync_mode_0_;  // Exact sync mode 0\n\n  using ExactPolicyMode1 = ::message_filters::sync_policies::ExactTime<\n    nvidia::isaac_ros::nitros::NitrosImage, sensor_msgs::msg::CameraInfo,\n    nvidia::isaac_ros::nitros::NitrosImage, sensor_msgs::msg::CameraInfo>;\n  using ExactSyncMode1 = ::message_filters::Synchronizer<ExactPolicyMode1>;\n  std::shared_ptr<ExactSyncMode1> exact_sync_mode_1_;  // Exact sync mode 1\n\n  using ExactPolicyMode2 = ::message_filters::sync_policies::ExactTime<\n    nvidia::isaac_ros::nitros::NitrosImage, sensor_msgs::msg::CameraInfo,\n    nvidia::isaac_ros::nitros::NitrosImage>;\n  using ExactSyncMode2 = ::message_filters::Synchronizer<ExactPolicyMode2>;\n  std::shared_ptr<ExactSyncMode2> exact_sync_mode_2_;  // Exact sync mode 2\n\n  /**\n   * @brief Callback function for sync mode 0.\n   * @param image_ptr Pointer to the image message.\n   * @param camera_info_ptr Pointer to the camera info message.\n   */\n  void sync_callback_mode_0(\n    const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & image_ptr,\n    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_ptr);\n\n  /**\n   * @brief Callback function for sync mode 1.\n   * @param image_1_ptr Pointer to the first image message.\n   * @param camera_info_1_ptr Pointer to the first camera info message.\n   * @param image_2_ptr Pointer to the second image message.\n   * @param camera_info_2_ptr Pointer to the second camera info message.\n   */\n  void sync_callback_mode_1(\n    const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & image_1_ptr,\n    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_1_ptr,\n    const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & image_2_ptr,\n    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_2_ptr);\n\n  /**\n   * @brief Callback function for sync mode 2.\n   * @param image_ptr Pointer to the image message.\n   * @param camera_info_ptr Pointer to the camera info message.\n   * @param depth_ptr Pointer to the depth message.\n   */\n  void sync_callback_mode_2(\n    const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & image_ptr,\n    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_ptr,\n    const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & depth_ptr);\n\n  /**\n   * @brief Ticks the dropper to determine if the current message should be\n   * published.\n   *\n   * This method iterates through the dropping order array and returns true if the current message\n   * should be published.\n   *\n   * @return True if the message should be published; false otherwise.\n   */\n  bool tick_dropper();\n\n  /**\n   * @brief Create an evenly spread array of size array_size with num_ones ones.\n   * @param array_size The size of the array.\n   * @param num_ones The number of ones in the array.\n   * @return The evenly spread array.\n   */\n  std::vector<bool> create_evenly_spread_array(int array_size, int num_ones);\n\n  std::string mode_;                      // Input mode\n  std::string depth_format_string_;       // Depth format string\n  int input_queue_size_;                  // Input queue size\n  int output_queue_size_;                 // Output queue size\n  int sync_queue_size_;                   // Sync queue size\n  int x_;                                 // Drop X out of Y messages\n  int y_;                                 // Drop X out of Y messages\n  int count_;                             // Count of messages\n  std::vector<bool> dropping_order_arr_;  // Dropping order array\n\n  // Latency optimization parameters\n  double max_latency_threshold_;          // Maximum allowed latency in seconds\n  bool enforce_max_latency_;             // Enforce max latency\n  /**\n   * @brief Check if the synchronized messages are recent enough to publish.\n   * @param message_timestamp The timestamp of the synchronized message set.\n   * @return True if messages are recent enough to publish; false otherwise.\n   */\n  bool is_message_recent_enough(const rclcpp::Time & message_timestamp);\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_TOPIC_TOOLS__ISAAC_ROS_NITROS_CAMERA_DROP_NODE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_topic_tools/include/isaac_ros_nitros_topic_tools/isaac_ros_nitros_topic_tools_common.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_TOPIC_TOOLS__ISAAC_ROS_NITROS_TOPIC_TOOLS_COMMON_HPP_\n#define ISAAC_ROS_NITROS_TOPIC_TOOLS__ISAAC_ROS_NITROS_TOPIC_TOOLS_COMMON_HPP_\n\n#include <string>\n#include <unordered_map>\n\nnamespace\n{\nconstexpr const char kModeMono[] = \"mono\";\nconstexpr const char kModeStereo[] = \"stereo\";\nconstexpr const char kModeMonoDepth[] = \"mono+depth\";\n}\n\n// Define enum for different modes\nenum class CameraDropMode\n{\n  Mono,\n  Stereo,\n  MonoDepth\n};\n\n// Map enum values to string representations\nconst std::unordered_map<CameraDropMode, std::string> modeToStringMap = {\n  {CameraDropMode::Mono, kModeMono},\n  {CameraDropMode::MonoDepth, kModeMonoDepth},\n  {CameraDropMode::Stereo, kModeStereo}};\n\n#endif  // ISAAC_ROS_NITROS_TOPIC_TOOLS__ISAAC_ROS_NITROS_TOPIC_TOOLS_COMMON_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_topic_tools/launch/isaac_ros_nitros_camera_drop.launch.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\n\n\ndef generate_launch_description():\n    isaac_ros_nitros_camera_drop_node = ComposableNode(\n        package='isaac_ros_nitros_topic_tools',\n        plugin='nvidia::isaac_ros::nitros::NitrosCameraDropNode',\n        name='isaac_ros_nitros_camera_drop_node')\n\n    container = ComposableNodeContainer(\n        package='rclcpp_components',\n        name='container',\n        namespace='',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            isaac_ros_nitros_camera_drop_node,\n        ],\n        output='screen'\n    )\n\n    return launch.LaunchDescription([container])\n"
  },
  {
    "path": "isaac_ros_nitros_topic_tools/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2024-2025, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_topic_tools</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS Nitros Topic Tools</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Ashwin Varghese Kuruttukulam</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>negotiated</depend>\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>std_msgs</depend>\n  <depend>sensor_msgs</depend>\n  <depend>message_filters</depend>\n  <depend>isaac_ros_nitros_camera_info_type</depend>\n  <depend>isaac_ros_nitros_image_type</depend>\n  <depend>isaac_ros_managed_nitros</depend>\n  <depend>isaac_ros_common</depend>\n\n  <build_depend>ament_index_cpp</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_topic_tools/src/isaac_ros_nitros_camera_drop_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_topic_tools/isaac_ros_nitros_topic_tools_common.hpp\"\n#include \"isaac_ros_nitros_topic_tools/isaac_ros_nitros_camera_drop_node.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\n#include <isaac_ros_common/qos.hpp>\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nnamespace\n{\nconstexpr const char kDefaultQoS[] = \"DEFAULT\";\n}\n\nNitrosCameraDropNode::NitrosCameraDropNode(const rclcpp::NodeOptions & options)\n: rclcpp::Node(\"drop\", options)\n{\n  mode_ = declare_parameter<std::string>(\"mode\", modeToStringMap.at(CameraDropMode::Mono));\n  depth_format_string_ =\n    declare_parameter<std::string>(\"depth_format_string\", \"nitros_image_32FC1\");\n  sync_queue_size_ = declare_parameter<int>(\"sync_queue_size\", 10);\n  input_queue_size_ = declare_parameter<int>(\"input_queue_size\", 10);\n  output_queue_size_ = declare_parameter<int>(\"output_queue_size\", 10);\n  const rclcpp::QoS input_qos = ::isaac_ros::common::AddQosParameter(\n    *this, kDefaultQoS, \"input_qos\").keep_last(input_queue_size_);\n  const rclcpp::QoS output_qos = ::isaac_ros::common::AddQosParameter(\n    *this, kDefaultQoS, \"output_qos\").keep_last(output_queue_size_);\n  const rmw_qos_profile_t input_qos_profile = input_qos.get_rmw_qos_profile();\n\n  // Drop X out of Y messages\n  x_ = declare_parameter<int>(\"X\", 15);\n  y_ = declare_parameter<int>(\"Y\", 30);\n  if (x_ > y_) {\n    RCLCPP_ERROR(get_logger(), \"X cannot be greater than Y\");\n    return;\n  }\n  count_ = 0;\n\n  // Latency optimization parameters\n  max_latency_threshold_ = declare_parameter<double>(\"max_latency_threshold\", 0.1);\n  enforce_max_latency_ = declare_parameter<bool>(\"enforce_max_latency\", false);\n  if (max_latency_threshold_ <= 0.0) {\n    RCLCPP_FATAL(get_logger(),\n      \"Parameter 'max_latency_threshold' must be > 0 (got %.3f)\", max_latency_threshold_);\n    throw std::invalid_argument(\"max_latency_threshold <= 0\");\n  }\n\n  // Create an array that represents the dropping pattern\n  // The array contains even spread of 0s.\n  dropping_order_arr_ = create_evenly_spread_array(y_, x_);\n\n  // Initialize common subscribers and publishers for all modes\n  image_sub_1_.subscribe(this, \"image_1\", input_qos_profile);\n  camera_info_sub_1_.subscribe(this, \"camera_info_1\", input_qos_profile);\n  image_pub_1_ = std::make_shared<\n    nvidia::isaac_ros::nitros::ManagedNitrosPublisher<nvidia::isaac_ros::nitros::NitrosImage>>(\n    this, \"image_1_drop\",\n    nvidia::isaac_ros::nitros::nitros_image_rgb8_t::supported_type_name,\n    nvidia::isaac_ros::nitros::NitrosDiagnosticsConfig(), output_qos);\n  camera_info_pub_1_ = this->create_publisher<\n    sensor_msgs::msg::CameraInfo>(\"camera_info_1_drop\", output_qos);\n\n  if (mode_ == modeToStringMap.at(CameraDropMode::Mono)) {\n    // Mode 0: Camera + CameraInfo (mono)\n    // Initialize sync policy\n    exact_sync_mode_0_ = std::make_shared<ExactSyncMode0>(\n      ExactPolicyMode0(sync_queue_size_), image_sub_1_,\n      camera_info_sub_1_);  // Use GetDefaultCompatibleFormat\n    using namespace std::placeholders;\n    exact_sync_mode_0_->registerCallback(\n      std::bind(&NitrosCameraDropNode::sync_callback_mode_0, this, _1, _2));\n  } else if (mode_ == modeToStringMap.at(CameraDropMode::Stereo)) {\n    // Mode 1: Camera + CameraInfo + Camera + CameraInfo (stereo)\n    // Initialize second mono subscribers and publishers\n    image_sub_2_.subscribe(this, \"image_2\", input_qos_profile);\n    camera_info_sub_2_.subscribe(this, \"camera_info_2\", input_qos_profile);\n    image_pub_2_ = std::make_shared<\n      nvidia::isaac_ros::nitros::ManagedNitrosPublisher<nvidia::isaac_ros::nitros::NitrosImage>>(\n      this, \"image_2_drop\",\n      nvidia::isaac_ros::nitros::nitros_image_rgb8_t::supported_type_name,\n      nvidia::isaac_ros::nitros::NitrosDiagnosticsConfig(), output_qos);\n    camera_info_pub_2_ = this->create_publisher<\n      sensor_msgs::msg::CameraInfo>(\"camera_info_2_drop\", output_qos);\n    // Initialize sync policy\n    exact_sync_mode_1_ = std::make_shared<ExactSyncMode1>(\n      ExactPolicyMode1(sync_queue_size_), image_sub_1_, camera_info_sub_1_, image_sub_2_,\n      camera_info_sub_2_);\n    using namespace std::placeholders;\n    exact_sync_mode_1_->registerCallback(\n      std::bind(&NitrosCameraDropNode::sync_callback_mode_1, this, _1, _2, _3, _4));\n  } else if (mode_ == modeToStringMap.at(CameraDropMode::MonoDepth)) {\n    // Mode 2: Camera + CameraInfo + Depth (mono+depth)\n    // Initialize depth subscriber and publisher\n    depth_sub_.subscribe(\n      this, \"depth_1\", input_qos_profile, rclcpp::SubscriptionOptions(),\n      depth_format_string_);\n    depth_pub_ = std::make_shared<\n      nvidia::isaac_ros::nitros::ManagedNitrosPublisher<nvidia::isaac_ros::nitros::NitrosImage>>(\n      this, \"depth_1_drop\", depth_format_string_);\n    // Initialize sync policy\n    exact_sync_mode_2_ = std::make_shared<ExactSyncMode2>(\n      ExactPolicyMode2(sync_queue_size_), image_sub_1_, camera_info_sub_1_, depth_sub_);\n    using namespace std::placeholders;\n    exact_sync_mode_2_->registerCallback(\n      std::bind(&NitrosCameraDropNode::sync_callback_mode_2, this, _1, _2, _3));\n  } else {\n    RCLCPP_ERROR(get_logger(), \"Invalid mode: %s\", mode_.c_str());\n  }\n}\n\nstd::vector<bool> NitrosCameraDropNode::create_evenly_spread_array(int array_size, int num_zeroes)\n{\n  std::vector<bool> dropping_order_arr(array_size, true);  // Initialize an array with all ones\n  if (num_zeroes <= 0) {return dropping_order_arr;}\n  double spacing = static_cast<double>(array_size) / num_zeroes;  // Calculate spacing\n  // Start by dropping the first message, ie, i=0\n  for (int i = 0; i < array_size; i++) {\n    int index = static_cast<int>(i * spacing);\n    if (index < array_size) {  // Ensure index does not exceed the array size\n      dropping_order_arr[index] = false;  // Place a 0 at the calculated index\n    }\n  }\n  return dropping_order_arr;\n}\n\nbool NitrosCameraDropNode::is_message_recent_enough(const rclcpp::Time & message_timestamp)\n{\n  if (!enforce_max_latency_) {\n    return true;\n  }\n  rclcpp::Time current_time = this->now();\n  double latency = (current_time - message_timestamp).seconds();\n\n  // Check if message is too old\n  if (latency > max_latency_threshold_) {\n    RCLCPP_WARN(get_logger(), \"Synced messages have a latency %.3f seconds\", latency);\n    return false;\n  }\n  return true;\n}\n\nbool NitrosCameraDropNode::tick_dropper()\n{\n  bool publish = dropping_order_arr_[count_];\n  count_++;\n  if (count_ >= y_) {\n    count_ = 0;\n  }\n  return publish;\n}\n\nvoid NitrosCameraDropNode::sync_callback_mode_0(\n  const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & image_ptr,\n  const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_ptr)\n{\n  // Get the message timestamp (use camera_info timestamp as reference)\n  rclcpp::Time message_timestamp = camera_info_ptr->header.stamp;\n\n  // Check if message is recent enough and should be published\n  if (tick_dropper() && is_message_recent_enough(message_timestamp)) {\n    image_pub_1_->publish(*image_ptr);\n    camera_info_pub_1_->publish(*camera_info_ptr);\n  }\n}\n\nvoid NitrosCameraDropNode::sync_callback_mode_1(\n  const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & image_1_ptr,\n  const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_1_ptr,\n  const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & image_2_ptr,\n  const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_2_ptr)\n{\n  // Get the message timestamp (use first camera_info timestamp as reference)\n  rclcpp::Time message_timestamp = camera_info_1_ptr->header.stamp;\n\n  // Check if message is recent enough and should be published\n  if (tick_dropper() && is_message_recent_enough(message_timestamp)) {\n    image_pub_1_->publish(*image_1_ptr);\n    camera_info_pub_1_->publish(*camera_info_1_ptr);\n    image_pub_2_->publish(*image_2_ptr);\n    camera_info_pub_2_->publish(*camera_info_2_ptr);\n  }\n}\n\nvoid NitrosCameraDropNode::sync_callback_mode_2(\n  const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & image_ptr,\n  const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_ptr,\n  const nvidia::isaac_ros::nitros::NitrosImage::ConstSharedPtr & depth_ptr)\n{\n  // Get the message timestamp (use camera_info timestamp as reference)\n  rclcpp::Time message_timestamp = camera_info_ptr->header.stamp;\n\n  // Check if message is recent enough and should be published\n  if (tick_dropper() && is_message_recent_enough(message_timestamp)) {\n    image_pub_1_->publish(*image_ptr);\n    camera_info_pub_1_->publish(*camera_info_ptr);\n    depth_pub_->publish(*depth_ptr);\n  }\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#include \"rclcpp_components/register_node_macro.hpp\"\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosCameraDropNode)\n"
  },
  {
    "path": "isaac_ros_nitros_topic_tools/test/isaac_ros_nitros_topic_tools_camera_drop_node_mode_0_test.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest\n\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\n\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import CameraInfo, Image\n\n\nIMAGE_HEIGHT = 100\nIMAGE_WIDTH = 150\nTOTAL_COUNT = 10\nDROP_COUNT = 7\nEXPECTED_MESSAGE_COUNT = TOTAL_COUNT - DROP_COUNT\nRECEIVE_TIMEOUT = 10\nBRINGUP_TIMEOUT = 10\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    nitros_drop_node = ComposableNode(\n        package='isaac_ros_nitros_topic_tools',\n        plugin='nvidia::isaac_ros::nitros::NitrosCameraDropNode',\n        name='nitros_drop_node',\n        namespace=NitrosCameraDropNodeMode0Test.generate_namespace(),\n        parameters=[{\n                    'X': DROP_COUNT,\n                    'Y': TOTAL_COUNT,\n                    'mode': 'mono'\n                    }]\n    )\n\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[nitros_drop_node],\n        output='screen',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return NitrosCameraDropNodeMode0Test.generate_test_description([container])\n\n\nclass NitrosCameraDropNodeMode0Test(IsaacROSBaseTest):\n    \"\"\"Test NitrosCameraDropNode in Mode = mono.\"\"\"\n\n    def create_image(self, name):\n        image = Image()\n        image.height = IMAGE_HEIGHT\n        image.width = IMAGE_WIDTH\n        image.encoding = 'rgb8'\n        image.is_bigendian = False\n        image.step = IMAGE_WIDTH * 3\n        image.data = [0] * IMAGE_HEIGHT * IMAGE_WIDTH * 3\n        image.header.frame_id = name\n        return image\n\n    def test_drop_node(self) -> None:\n        \"\"\"\n        Test case for the drop_node function.\n\n        This test case verifies the behavior of the drop_node function by publishing a fixed\n        number of messages to the input topics and\n        checking if the correct number of messages are dropped.\n        \"\"\"\n        # Generate namespace lookup\n        self.generate_namespace_lookup(\n            ['image_1', 'camera_info_1', 'image_1_drop', 'camera_info_1_drop'])\n\n        received_messages = []\n        # Create exact time sync logging subscribers\n        self.create_exact_time_sync_logging_subscribers(\n            [('image_1_drop', Image), ('camera_info_1_drop', CameraInfo)],\n            received_messages, accept_multiple_messages=True)\n        # Publish to inputs\n        image_pub = self.node.create_publisher(\n            Image, self.namespaces['image_1'], self.DEFAULT_QOS)\n        camera_info_pub = self.node.create_publisher(\n            CameraInfo, self.namespaces['camera_info_1'], self.DEFAULT_QOS)\n        try:\n            image_msg = self.create_image('test_image')\n            camera_info_msg = CameraInfo()\n            camera_info_msg.distortion_model = 'plumb_bob'\n            self.node.get_logger().info('Starting to publish messages')\n\n            # Publish TOTAL_COUNT number of messages\n            counter = 0\n            # Ensure NitrosCameraDropNode subscriptions are connected before publishing.\n            # Waiting for subscription counts is more robust than a fixed delay and\n            # prevents message drops / flaky tests on loaded CI machines.\n            end_time = time.time() + BRINGUP_TIMEOUT\n            while time.time() < end_time:\n                if (image_pub.get_subscription_count() > 0 and\n                        camera_info_pub.get_subscription_count() > 0):\n                    break\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n            self.assertGreater(\n                image_pub.get_subscription_count(), 0,\n                'NitrosCameraDropNode did not subscribe to image_1 in time')\n            self.assertGreater(\n                camera_info_pub.get_subscription_count(), 0,\n                'NitrosCameraDropNode did not subscribe to camera_info_1 in time')\n            while counter < TOTAL_COUNT:\n                header = self.node.get_clock().now().to_msg()\n                image_msg.header.stamp = header\n                camera_info_msg.header.stamp = header\n                image_pub.publish(image_msg)\n                camera_info_pub.publish(camera_info_msg)\n                time.sleep(0.1)\n                counter += 1\n                rclpy.spin_once(self.node, timeout_sec=0.01)\n\n            # Wait for the expected number of messages to be received (polling with timeout)\n            end_time = time.time() + RECEIVE_TIMEOUT\n            while time.time() < end_time:\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n                if len(received_messages) >= EXPECTED_MESSAGE_COUNT:\n                    break\n\n            # Check if the correct number of messages are received\n            self.assertTrue(len(received_messages) > 0,\n                            'Did not receive output messages')\n            self.assertEqual(\n                len(received_messages), EXPECTED_MESSAGE_COUNT,\n                f'Expected {EXPECTED_MESSAGE_COUNT} messages but received '\n                f'{len(received_messages)} messages')\n        finally:\n            self.node.destroy_publisher(image_pub)\n            self.node.destroy_publisher(camera_info_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_topic_tools/test/isaac_ros_nitros_topic_tools_camera_drop_node_mode_1_test.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest\n\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\n\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import CameraInfo, Image\n\n\nIMAGE_HEIGHT = 100\nIMAGE_WIDTH = 150\nTOTAL_COUNT = 10\nDROP_COUNT = 7\nEXPECTED_MESSAGE_COUNT = TOTAL_COUNT - DROP_COUNT\nRECEIVE_TIMEOUT = 10\nBRINGUP_TIMEOUT = 10\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    nitros_drop_node = ComposableNode(\n        package='isaac_ros_nitros_topic_tools',\n        plugin='nvidia::isaac_ros::nitros::NitrosCameraDropNode',\n        name='nitros_drop_node',\n        namespace=NitrosCameraDropNodeMode1Test.generate_namespace(),\n        parameters=[{\n                    'X': DROP_COUNT,\n                    'Y': TOTAL_COUNT,\n                    'mode': 'stereo'\n                    }]\n    )\n\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[nitros_drop_node],\n        output='screen',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return NitrosCameraDropNodeMode1Test.generate_test_description([container])\n\n\nclass NitrosCameraDropNodeMode1Test(IsaacROSBaseTest):\n    \"\"\"Test NitrosCameraDropNode in Mode = stereo.\"\"\"\n\n    def create_image(self, name):\n        image = Image()\n        image.height = IMAGE_HEIGHT\n        image.width = IMAGE_WIDTH\n        image.encoding = 'rgb8'\n        image.is_bigendian = False\n        image.step = IMAGE_WIDTH * 3\n        image.data = [0] * IMAGE_HEIGHT * IMAGE_WIDTH * 3\n        image.header.frame_id = name\n        return image\n\n    def test_drop_node(self) -> None:\n        \"\"\"\n        Test case for the drop_node function.\n\n        This test case verifies the behavior of the drop_node function by publishing a fixed\n        number of messages to the input topics and\n        checking if the correct number of messages are dropped.\n        \"\"\"\n        # Generate namespace lookup\n        self.generate_namespace_lookup(\n            ['image_1', 'camera_info_1', 'image_2', 'camera_info_2',\n             'image_1_drop', 'camera_info_1_drop',\n             'image_2_drop', 'camera_info_2_drop'])\n\n        received_messages_1 = []\n        received_messages_2 = []\n        # Create exact time sync logging subscribers\n        self.create_exact_time_sync_logging_subscribers(\n            [('image_1_drop', Image), ('camera_info_1_drop', CameraInfo)],\n            received_messages_1, accept_multiple_messages=True)\n        self.create_exact_time_sync_logging_subscribers(\n            [('image_2_drop', Image), ('camera_info_2_drop', CameraInfo)],\n            received_messages_2, accept_multiple_messages=True)\n        # Publish to inputs\n        image_pub_1 = self.node.create_publisher(\n            Image, self.namespaces['image_1'], self.DEFAULT_QOS)\n        camera_info_pub_1 = self.node.create_publisher(\n            CameraInfo, self.namespaces['camera_info_1'], self.DEFAULT_QOS)\n        image_pub_2 = self.node.create_publisher(\n            Image, self.namespaces['image_2'], self.DEFAULT_QOS)\n        camera_info_pub_2 = self.node.create_publisher(\n            CameraInfo, self.namespaces['camera_info_2'], self.DEFAULT_QOS)\n        try:\n            image_msg_1 = self.create_image('test_image_1')\n            camera_info_msg_1 = CameraInfo()\n            camera_info_msg_1.distortion_model = 'plumb_bob'\n\n            image_msg_2 = self.create_image('test_image_2')\n            camera_info_msg_2 = CameraInfo()\n            camera_info_msg_2.distortion_model = 'plumb_bob'\n            self.node.get_logger().info('Starting to publish messages')\n\n            # Publish TOTAL_COUNT number of messages\n            counter = 0\n            # Ensure NitrosCameraDropNode subscriptions are connected before publishing.\n            # Waiting for subscription counts is more robust than a fixed delay and\n            # prevents message drops / flaky tests on loaded CI machines.\n            end_time = time.time() + BRINGUP_TIMEOUT\n            while time.time() < end_time:\n                if (image_pub_1.get_subscription_count() > 0 and\n                        camera_info_pub_1.get_subscription_count() > 0 and\n                        image_pub_2.get_subscription_count() > 0 and\n                        camera_info_pub_2.get_subscription_count() > 0):\n                    break\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n            self.assertGreater(\n                image_pub_1.get_subscription_count(), 0,\n                'NitrosCameraDropNode did not subscribe to image_1 in time')\n            self.assertGreater(\n                camera_info_pub_1.get_subscription_count(), 0,\n                'NitrosCameraDropNode did not subscribe to camera_info_1 in time')\n            self.assertGreater(\n                image_pub_2.get_subscription_count(), 0,\n                'NitrosCameraDropNode did not subscribe to image_2 in time')\n            self.assertGreater(\n                camera_info_pub_2.get_subscription_count(), 0,\n                'NitrosCameraDropNode did not subscribe to camera_info_2 in time')\n            while counter < TOTAL_COUNT:\n                header = self.node.get_clock().now().to_msg()\n                image_msg_1.header.stamp = header\n                camera_info_msg_1.header.stamp = header\n                image_msg_2.header.stamp = header\n                camera_info_msg_2.header.stamp = header\n                image_pub_1.publish(image_msg_1)\n                camera_info_pub_1.publish(camera_info_msg_1)\n                image_pub_2.publish(image_msg_2)\n                camera_info_pub_2.publish(camera_info_msg_2)\n                time.sleep(0.1)\n                counter += 1\n                rclpy.spin_once(self.node, timeout_sec=0.01)\n\n            # Wait for the expected number of messages to be received (polling with timeout)\n            end_time = time.time() + RECEIVE_TIMEOUT\n            while time.time() < end_time:\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n                if (len(received_messages_1) >= EXPECTED_MESSAGE_COUNT and\n                        len(received_messages_2) >= EXPECTED_MESSAGE_COUNT):\n                    break\n\n            # Check if the correct number of messages are received\n            self.assertTrue(len(received_messages_1) > 0,\n                            'Did not receive output messages on channel 1')\n            self.assertTrue(len(received_messages_2) > 0,\n                            'Did not receive output messages on channel 2')\n            self.assertEqual(\n                len(received_messages_1), EXPECTED_MESSAGE_COUNT,\n                f'Channel 1: Expected {EXPECTED_MESSAGE_COUNT} messages but received '\n                f'{len(received_messages_1)} messages')\n            self.assertEqual(\n                len(received_messages_2), EXPECTED_MESSAGE_COUNT,\n                f'Channel 2: Expected {EXPECTED_MESSAGE_COUNT} messages but received '\n                f'{len(received_messages_2)} messages')\n        finally:\n            self.node.destroy_publisher(image_pub_1)\n            self.node.destroy_publisher(camera_info_pub_1)\n            self.node.destroy_publisher(image_pub_2)\n            self.node.destroy_publisher(camera_info_pub_2)\n"
  },
  {
    "path": "isaac_ros_nitros_topic_tools/test/isaac_ros_nitros_topic_tools_camera_drop_node_mode_2_test.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest\n\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\n\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import CameraInfo, Image\n\n\nIMAGE_HEIGHT = 100\nIMAGE_WIDTH = 150\nTOTAL_COUNT = 10\nDROP_COUNT = 7\nEXPECTED_MESSAGE_COUNT = TOTAL_COUNT - DROP_COUNT\nRECEIVE_TIMEOUT = 10\nBRINGUP_TIMEOUT = 10\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    nitros_drop_node = ComposableNode(\n        package='isaac_ros_nitros_topic_tools',\n        plugin='nvidia::isaac_ros::nitros::NitrosCameraDropNode',\n        name='nitros_drop_node',\n        namespace=NitrosCameraDropNodeMode2Test.generate_namespace(),\n        parameters=[{\n                    'X': DROP_COUNT,\n                    'Y': TOTAL_COUNT,\n                    'mode': 'mono+depth'\n                    }]\n    )\n\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[nitros_drop_node],\n        output='screen',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return NitrosCameraDropNodeMode2Test.generate_test_description([container])\n\n\nclass NitrosCameraDropNodeMode2Test(IsaacROSBaseTest):\n    \"\"\"Test NitrosCameraDropNode in Mode = mono+depth.\"\"\"\n\n    def create_image(self, name):\n        image = Image()\n        image.height = IMAGE_HEIGHT\n        image.width = IMAGE_WIDTH\n        image.encoding = 'rgb8'\n        image.is_bigendian = False\n        image.step = IMAGE_WIDTH * 3\n        image.data = [0] * IMAGE_HEIGHT * IMAGE_WIDTH * 3\n        image.header.frame_id = name\n        return image\n\n    def test_drop_node(self) -> None:\n        \"\"\"\n        Test case for the drop_node function.\n\n        This test case verifies the behavior of the drop_node function by publishing a fixed\n        number of messages to the input topics and\n        checking if the correct number of messages are dropped.\n        \"\"\"\n        # Generate namespace lookup\n        self.generate_namespace_lookup(\n            ['image_1', 'camera_info_1', 'depth_1', 'image_1_drop',\n             'camera_info_1_drop', 'depth_1_drop'])\n\n        received_messages = []\n\n        self.create_exact_time_sync_logging_subscribers(\n            [('image_1_drop', Image), ('camera_info_1_drop', CameraInfo),\n                ('depth_1_drop', Image)],\n            received_messages, accept_multiple_messages=True)\n        # Publish to inputs\n        image_pub = self.node.create_publisher(\n            Image, self.namespaces['image_1'], self.DEFAULT_QOS)\n        camera_info_pub = self.node.create_publisher(\n            CameraInfo, self.namespaces['camera_info_1'], self.DEFAULT_QOS)\n        depth_pub = self.node.create_publisher(\n            Image, self.namespaces['depth_1'], self.DEFAULT_QOS)\n        try:\n            image_msg = self.create_image('test_image')\n            camera_info_msg = CameraInfo()\n            camera_info_msg.distortion_model = 'plumb_bob'\n            depth_msg = self.create_image('test_depth')\n            self.node.get_logger().info('Starting to publish messages')\n\n            # Publish TOTAL_COUNT number of messages\n            counter = 0\n            # Ensure NitrosCameraDropNode subscriptions are connected before publishing.\n            # Waiting for subscription counts is more robust than a fixed delay and\n            # prevents message drops / flaky tests on loaded CI machines.\n            end_time = time.time() + BRINGUP_TIMEOUT\n            while time.time() < end_time:\n                if (image_pub.get_subscription_count() > 0 and\n                        camera_info_pub.get_subscription_count() > 0 and\n                        depth_pub.get_subscription_count() > 0):\n                    break\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n            self.assertGreater(\n                image_pub.get_subscription_count(), 0,\n                'NitrosCameraDropNode did not subscribe to image_1 in time')\n            self.assertGreater(\n                camera_info_pub.get_subscription_count(), 0,\n                'NitrosCameraDropNode did not subscribe to camera_info_1 in time')\n            self.assertGreater(\n                depth_pub.get_subscription_count(), 0,\n                'NitrosCameraDropNode did not subscribe to depth_1 in time')\n            while counter < TOTAL_COUNT:\n                header = self.node.get_clock().now().to_msg()\n                image_msg.header.stamp = header\n                camera_info_msg.header.stamp = header\n                depth_msg.header.stamp = header\n                image_pub.publish(image_msg)\n                camera_info_pub.publish(camera_info_msg)\n                depth_pub.publish(depth_msg)\n                time.sleep(0.1)\n                counter += 1\n                rclpy.spin_once(self.node, timeout_sec=0.01)\n\n            # Wait for the expected number of messages to be received (polling with timeout)\n            end_time = time.time() + RECEIVE_TIMEOUT\n            while time.time() < end_time:\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n                if len(received_messages) >= EXPECTED_MESSAGE_COUNT:\n                    break\n\n            # Check if the correct number of messages are received\n            self.assertTrue(len(received_messages) > 0, 'Did not receive output messages')\n            self.assertEqual(\n                len(received_messages), EXPECTED_MESSAGE_COUNT,\n                f'Expected {EXPECTED_MESSAGE_COUNT} messages but received '\n                f'{len(received_messages)} messages')\n        finally:\n            self.node.destroy_publisher(image_pub)\n            self.node.destroy_publisher(camera_info_pub)\n            self.node.destroy_publisher(depth_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_battery_state_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_battery_state_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n  add_definitions(-DUSE_NVTX)\n  link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n  link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(yaml-cpp)\n\n# NitrosBatteryState\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_battery_state.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosBatteryStateForwardNode\n  ament_auto_add_library(isaac_ros_nitros_battery_state_forward_node SHARED test/src/nitros_battery_state_forward_node.cpp)\n  target_link_libraries(isaac_ros_nitros_battery_state_forward_node ${PROJECT_NAME})\n  set_target_properties(isaac_ros_nitros_battery_state_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(isaac_ros_nitros_battery_state_forward_node\n    \"nvidia::isaac_ros::nitros::NitrosBatteryStateForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosBatteryStateForwardNode;\\\n    $<TARGET_FILE:isaac_ros_nitros_battery_state_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_battery_state_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_battery_state_type/include/isaac_ros_nitros_battery_state_type/nitros_battery_state.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_BATTERY_STATE_TYPE__NITROS_BATTERY_STATE_HPP_\n#define ISAAC_ROS_NITROS_BATTERY_STATE_TYPE__NITROS_BATTERY_STATE_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosBatteryState\n *   ROS type:    sensor_msgs::msg::BatteryState\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"sensor_msgs/msg/battery_state.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosBatteryState;\n\n// Formats\nstruct nitros_battery_state_t\n{\n  using MsgT = NitrosBatteryState;\n  static const inline std::string supported_type_name = \"nitros_battery_state\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosBatteryState)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_battery_state_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_messages\", \"gxf/lib/libgxf_isaac_messages.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosBatteryState,\n  sensor_msgs::msg::BatteryState>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosBatteryState;\n  using ros_message_type = sensor_msgs::msg::BatteryState;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosBatteryState,\n  sensor_msgs::msg::BatteryState);\n\n#endif  // ISAAC_ROS_NITROS_BATTERY_STATE_TYPE__NITROS_BATTERY_STATE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_battery_state_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_battery_state_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS NITROS Battery State Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>sensor_msgs</depend>\n  <depend>isaac_ros_nova_interfaces</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>gxf_isaac_messages</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_battery_state_type/src/nitros_battery_state.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"extensions/messages/battery_state_message.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_battery_state_type/nitros_battery_state.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosBatteryState,\n  sensor_msgs::msg::BatteryState>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosBatteryState::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosBatteryState\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto maybe_battery_state_parts = nvidia::isaac::GetBatteryStateMessage(\n    msg_entity.value());\n  if (!maybe_battery_state_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get battery state gxf message\"\n      \" from message entity: \" <<\n      GxfResultStr(maybe_battery_state_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosBatteryState\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto battery_state_parts = maybe_battery_state_parts.value();\n  destination.voltage = battery_state_parts.battery_state->voltage;\n\n  destination.percentage = battery_state_parts.battery_state->charge / 100.0;\n  if (battery_state_parts.battery_state->is_charging) {\n    destination.power_supply_status = 1;  // POWER_SUPPLY_STATUS_CHARGING = 1\n  } else {\n    destination.power_supply_status = 2;  // POWER_SUPPLY_STATUS_DISCHARGING = 2\n  }\n\n  // The following fields are unable to convert:\n  // destination.temperature\n  // destination.current\n  // destination.charge\n  // destination.capacity\n  // destination.design_capacity\n  // destination.power_supply_health\n  // destination.power_supply_technology\n  // destination.present\n  // destination.cell_voltage\n  // destination.cell_temperature\n  // destination.location\n  // destination.serial_number\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = battery_state_parts.timestamp;\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Set frame ID\n  destination.header.frame_id = source.frame_id;\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosBatteryState\"),\n    \"[convert_to_ros_message] frame_id = %s\", destination.header.frame_id.c_str());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosBatteryState\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosBatteryState,\n  sensor_msgs::msg::BatteryState>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosBatteryState::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosBatteryState\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // create battery_state gxf msg\n  auto maybe_battery_state_parts = nvidia::isaac::CreateBatteryStateMessage(context);\n  if (!maybe_battery_state_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to create battery state gxf msg \" << GxfResultStr(\n      maybe_battery_state_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosBatteryState\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto battery_state_parts = maybe_battery_state_parts.value();\n\n  battery_state_parts.battery_state->charge = source.percentage * 100.0;\n  battery_state_parts.battery_state->voltage = source.voltage;\n  battery_state_parts.battery_state->health = (source.capacity / source.design_capacity) * 100;\n  if (source.power_supply_status == 1) {\n    // POWER_SUPPLY_STATUS_CHARGING = 1\n    battery_state_parts.battery_state->is_charging = true;\n  }\n  battery_state_parts.battery_state->reach = 0;\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  battery_state_parts.timestamp->acqtime = input_timestamp;\n\n  // Set frame ID\n  destination.frame_id = source.header.frame_id;\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosBatteryState\"),\n    \"[convert_to_custom] frame_id = %s\", destination.frame_id.c_str());\n\n  // Set Entity Id\n  destination.handle = battery_state_parts.message.eid();\n  GxfEntityRefCountInc(context, battery_state_parts.message.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosBatteryState\"),\n    \"[convert_to_custom] Conversion completed (handle=%ld)\", destination.handle);\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_battery_state_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_battery_state_type/test/isaac_ros_nitros_battery_state_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosBatteryState type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\n\nfrom sensor_msgs.msg import BatteryState\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosBatteryStateTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_battery_state_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosBatteryStateForwardNode',\n                name='NitrosBatteryStateForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_battery_state'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'NitrosBatteryState:=debug'],\n    )\n\n    return IsaacROSNitrosBatteryStateTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosBatteryStateTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosBatteryState type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_battery_state')\n    def test_nitros_battery_state_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the message from NitrosBatteryState type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', BatteryState)],\n            received_messages=received_messages\n        )\n\n        battery_state_pub = self.node.create_publisher(\n            BatteryState, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            battery_state = self.load_battery_state_from_json(test_folder / 'battery_state.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 10\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                battery_state.header.stamp = timestamp\n\n                battery_state_pub.publish(battery_state)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_battery_state = received_messages['output']\n\n            # Only test for data passed through gxf\n            # Header\n            self.assertEqual(battery_state.header.stamp.sec,\n                             received_battery_state.header.stamp.sec)\n            self.assertEqual(battery_state.header.stamp.nanosec,\n                             received_battery_state.header.stamp.nanosec)\n            self.assertEqual(battery_state.header.frame_id,\n                             received_battery_state.header.frame_id)\n            # Values\n            self.assertEqual(battery_state.voltage,\n                             round(received_battery_state.voltage, 1))\n            self.assertEqual(battery_state.percentage,\n                             round(received_battery_state.percentage, 1))\n            self.assertEqual(battery_state.power_supply_status,\n                             received_battery_state.power_supply_status)\n            print('The received battery_state message has been verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(battery_state_pub)\n\n    @staticmethod\n    def load_battery_state_from_json(\n            json_filepath: pathlib.Path) -> BatteryState:\n        \"\"\"\n        Load a BatteryState message from a JSON filepath.\n\n        Parameters\n        ----------\n        json_filepath : Path\n            The path to a JSON file containing the BatteryState fields\n\n        Returns\n        -------\n        BatteryState\n            Generated BatteryState message\n\n        \"\"\"\n        battery_state_json = JSONConversion.load_from_json(\n            json_filepath)\n\n        battery_state = BatteryState()\n        battery_state.header.frame_id = battery_state_json[\n            'header']['frame_id']\n        battery_state.header.stamp.sec = battery_state_json[\n            'header']['stamp']['sec']\n        battery_state.header.stamp.nanosec = battery_state_json[\n            'header']['stamp']['nanosec']\n        battery_state.voltage = battery_state_json['voltage']\n        battery_state.temperature = battery_state_json['temperature']\n        battery_state.current = battery_state_json['current']\n        battery_state.charge = battery_state_json['charge']\n        battery_state.capacity = battery_state_json['capacity']\n        battery_state.design_capacity = battery_state_json['design_capacity']\n        battery_state.percentage = battery_state_json['percentage']\n        battery_state.power_supply_status = battery_state_json['power_supply_status']\n        battery_state.power_supply_health = battery_state_json['power_supply_health']\n        battery_state.power_supply_technology = battery_state_json['power_supply_technology']\n        battery_state.present = battery_state_json['present']\n        for i in range(len(battery_state_json['cell_voltage'])):\n            battery_state.cell_voltage.append(battery_state_json['cell_voltage'][i])\n        for i in range(len(battery_state_json['cell_temperature'])):\n            battery_state.cell_temperature.append(battery_state_json['cell_temperature'][i])\n        battery_state.location = battery_state_json['location']\n        battery_state.serial_number = battery_state_json['serial_number']\n        return battery_state\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_battery_state_type/test/src/nitros_battery_state_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_battery_state_type/nitros_battery_state.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_battery_state_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_battery_state\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosBatteryStateForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosBatteryStateForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n              .frame_id_source_key = \"forward/input\",\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosBatteryState>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosBatteryStateForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_battery_state_type/test/test_cases/nitros_battery_state/profile/battery_state.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"battery_state_header\",\n    \"stamp\": {\n      \"sec\": 22125,\n      \"nanosec\": 45172\n    }\n  },\n  \"voltage\": 11.1,\n  \"temperature\": 25.0,\n  \"current\": 10.0,\n  \"charge\": 1.0,\n  \"capacity\": 5.0,\n  \"design_capacity\": 5.2,\n  \"percentage\": 0.2,\n  \"power_supply_status\": 1,\n  \"power_supply_health\": 1,\n  \"power_supply_technology\": 3,\n  \"present\": true,\n  \"cell_voltage\": [3.7, 3.7, 3.7],\n  \"cell_temperature\": [30.0, 30.0, 30.0],\n  \"location\": \"unknown_location\",\n  \"serial_number\": \"test_serial_number\"\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_camera_info_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_camera_info_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(yaml-cpp)\n\n# NitrosCameraInfo\nament_auto_add_library(${PROJECT_NAME} SHARED\n  src/nitros_camera_info.cpp\n  src/nitros_camera_info_view.cpp\n)\n\ntarget_link_libraries(${PROJECT_NAME}\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosCameraInfoForwardNode\n  ament_auto_add_library(nitros_camera_info_forward_node SHARED\n    test/src/nitros_camera_info_forward_node.cpp\n  )\n  target_link_libraries(nitros_camera_info_forward_node ${PROJECT_NAME})\n  set_target_properties(nitros_camera_info_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(nitros_camera_info_forward_node \"nvidia::isaac_ros::nitros::NitrosCameraInfoForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosCameraInfoForwardNode;$<TARGET_FILE:nitros_camera_info_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_camera_info_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_camera_info_type/include/isaac_ros_nitros_camera_info_type/nitros_camera_info.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_CAMERA_INFO_TYPE__NITROS_CAMERA_INFO_HPP_\n#define ISAAC_ROS_NITROS_CAMERA_INFO_TYPE__NITROS_CAMERA_INFO_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosCameraInfo\n *   ROS type:    sensor_msgs::msg::CameraInfo\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"gxf/core/gxf.h\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/multimedia/camera.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"sensor_msgs/msg/camera_info.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosCameraInfo;\n\n// Formats\nstruct nitros_camera_info_t\n{\n  using MsgT = NitrosCameraInfo;\n  static const inline std::string supported_type_name = \"nitros_camera_info\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosCameraInfo)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_camera_info_t)\nNITROS_FORMAT_FACTORY_END()\n// Default compatible data format\nNITROS_DEFAULT_COMPATIBLE_FORMAT(nitros_camera_info_t)\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/multimedia/libgxf_multimedia.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n// Helper for convert_to_custom, resusable in other nodes\nvoid copy_ros_to_gxf_camera_info(\n  sensor_msgs::msg::CameraInfo source,\n  nvidia::gxf::Expected<nvidia::gxf::Entity> & destination);\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCameraInfo,\n  sensor_msgs::msg::CameraInfo>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosCameraInfo;\n  using ros_message_type = sensor_msgs::msg::CameraInfo;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosCameraInfo,\n  sensor_msgs::msg::CameraInfo);\n\n#endif  // ISAAC_ROS_NITROS_CAMERA_INFO_TYPE__NITROS_CAMERA_INFO_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_camera_info_type/include/isaac_ros_nitros_camera_info_type/nitros_camera_info_view.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_CAMERA_INFO_TYPE__NITROS_CAMERA_INFO_VIEW_HPP_\n#define ISAAC_ROS_NITROS_CAMERA_INFO_TYPE__NITROS_CAMERA_INFO_VIEW_HPP_\n\n#include \"isaac_ros_nitros/types/nitros_type_view_factory.hpp\"\n#include \"isaac_ros_nitros_camera_info_type/nitros_camera_info.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nNITROS_TYPE_VIEW_FACTORY_BEGIN(NitrosCameraInfo)\nNITROS_TYPE_VIEW_FACTORY_END(NitrosCameraInfo)\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_CAMERA_INFO_TYPE__NITROS_CAMERA_INFO_VIEW_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_camera_info_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_camera_info_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS Nitros Camera Info Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n  <author>Swapnesh Wani</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>sensor_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>ament_index_cpp</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_camera_info_type/src/nitros_camera_info.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_camera_info_type/nitros_camera_info.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\n\nconstexpr char RAW_CAMERA_MODEL_GXF_NAME[] = \"intrinsics\";\nconstexpr char RECT_CAMERA_MODEL_GXF_NAME[] = \"target_camera\";\nconstexpr char EXTRINSICS_GXF_NAME[] = \"extrinsics\";\nconstexpr char TARGET_EXTRINSICS_DELTA_GXF_NAME[] = \"target_extrinsics_delta\";\nconstexpr int NUM_COFF_PLUMB_BOB = 5;\n\nnamespace\n{\nusing DistortionType = nvidia::gxf::DistortionType;\nconst std::unordered_map<std::string, DistortionType> g_ros_to_gxf_distortion_model({\n    {\"plumb_bob\", DistortionType::Brown},\n    {\"rational_polynomial\", DistortionType::Polynomial},\n    {\"equidistant\", DistortionType::FisheyeEquidistant}\n  });\n\nconst std::unordered_map<DistortionType, std::string> g_gxf_to_ros_distortion_model({\n    {DistortionType::Polynomial, \"rational_polynomial\"},\n    {DistortionType::FisheyeEquidistant, \"equidistant\"},\n    {DistortionType::Perspective, \"plumb_bob\"},\n    {DistortionType::Brown, \"plumb_bob\"}\n  });\n}  // namespace\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Convert between an existing ROS CameraInfo message and an existing GXF CameraModel object\n// Also used in argus_camera_node.cpp\nvoid copy_ros_to_gxf_camera_info(\n  sensor_msgs::msg::CameraInfo source,\n  nvidia::gxf::Expected<nvidia::gxf::Entity> & destination)\n{\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\n      \"NitrosCameraInfo\"),\n    \"[convert_to_custom] Overriding CameraModel with values loaded from URL\");\n\n  auto raw_gxf_camera_model = destination->get<nvidia::gxf::CameraModel>(RAW_CAMERA_MODEL_GXF_NAME);\n  auto rect_gxf_camera_model =\n    destination->get<nvidia::gxf::CameraModel>(RECT_CAMERA_MODEL_GXF_NAME);\n  auto extrinsics_gxf_pose_3d = destination->get<nvidia::gxf::Pose3D>(EXTRINSICS_GXF_NAME);\n  auto target_extrinsics_delta_gxf_pose_3d = destination->get<nvidia::gxf::Pose3D>(\n    TARGET_EXTRINSICS_DELTA_GXF_NAME);\n\n  if (!raw_gxf_camera_model) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get the existing CameraModel object: \" <<\n      GxfResultStr(raw_gxf_camera_model.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  if (!extrinsics_gxf_pose_3d) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Failed to get Pose3D object from message entity: \" <<\n      GxfResultStr(extrinsics_gxf_pose_3d.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  if (!rect_gxf_camera_model) {\n    std::stringstream warn_msg;\n    warn_msg << \"[convert_to_custom] Rectified camera model  from message entity: \" <<\n      GxfResultStr(rect_gxf_camera_model.error()) << \" this is expected for monocular cameras\";\n    RCLCPP_WARN_ONCE(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), warn_msg.str().c_str());\n    rect_gxf_camera_model = raw_gxf_camera_model;\n  } else {\n    // Only set this to perspective/0 if we did not fall back to the raw camera model\n    // otherwise we will overwrite the distortion model\n    rect_gxf_camera_model.value()->distortion_type = DistortionType::Perspective;\n    memset(\n      rect_gxf_camera_model.value()->distortion_coefficients.data(), 0,\n      rect_gxf_camera_model.value()->kMaxDistortionCoefficients * sizeof(float));\n  }\n\n  if (!target_extrinsics_delta_gxf_pose_3d) {\n    std::stringstream warn_msg;\n    warn_msg <<\n      \"[convert_to_custom] Failed to get the target extrinsics delta Pose3D object: \" <<\n      GxfResultStr(target_extrinsics_delta_gxf_pose_3d.error()) <<\n      \" this is expected for monocular cameras\";\n    RCLCPP_WARN_ONCE(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), warn_msg.str().c_str());\n    // Add a target extrinsics delta, we should only be taking this path\n    // in the case where we override camera info for a monocular camera.\n    // So this delta should be identity rotation and 0 translation, but\n    // we will set it to whatever the rectification matrix passed in is.\n    // This way overridden camera info messages behave exactly like the\n    // original eeprom case.\n    target_extrinsics_delta_gxf_pose_3d = destination->add<nvidia::gxf::Pose3D>(\n      TARGET_EXTRINSICS_DELTA_GXF_NAME);\n  }\n\n  raw_gxf_camera_model.value()->dimensions = {source.width, source.height};\n  raw_gxf_camera_model.value()->focal_length = {\n    static_cast<float>(source.k[0]), static_cast<float>(source.k[4])};\n  raw_gxf_camera_model.value()->principal_point = {\n    static_cast<float>(source.k[2]), static_cast<float>(source.k[5])};\n\n  const auto distortion = g_ros_to_gxf_distortion_model.find(source.distortion_model);\n  if (distortion == std::end(g_ros_to_gxf_distortion_model)) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Unsupported distortion model from ROS [\" <<\n      source.distortion_model.c_str() << \"].\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  } else {\n    raw_gxf_camera_model.value()->distortion_type = distortion->second;\n  }\n\n  memset(\n    raw_gxf_camera_model.value()->distortion_coefficients.data(), 0,\n    sizeof(raw_gxf_camera_model.value()->distortion_coefficients));\n\n  // If the distortion model is \"Brown\", check if all the distortion parameters are zero\n  // If yes, then set the distortion type to \"Perspective\"\n  if (distortion->second == DistortionType::Brown && source.d.size() == NUM_COFF_PLUMB_BOB) {\n    if (std::all_of(\n        source.d.begin(), source.d.end(),\n        [](const auto & value) {return value == 0;}))\n    {\n      raw_gxf_camera_model.value()->distortion_type = DistortionType::Perspective;\n    }\n  }\n\n  if (source.d.size() > nvidia::gxf::CameraModel::kMaxDistortionCoefficients) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] More number of coefficients for distortion model found [ # of coeff: \" <<\n      source.d.size() << \" > \" << nvidia::gxf::CameraModel::kMaxDistortionCoefficients << \"].\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  if (raw_gxf_camera_model.value()->distortion_type == DistortionType::Polynomial) {\n    // prevents distortion parameters array access if its empty\n    // simulators may send empty distortion parameter array since images are already rectified\n    if (!source.d.empty()) {\n      // distortion parameters in GXF: k1, k2, k3, k4, k5, k6, p1, p2\n      // distortion parameters in ROS message: k1, k2, p1, p2, k3 ...\n      raw_gxf_camera_model.value()->distortion_coefficients[0] = source.d[0];\n      raw_gxf_camera_model.value()->distortion_coefficients[1] = source.d[1];\n\n      for (uint16_t index = 2; index < source.d.size() - 2; index++) {\n        raw_gxf_camera_model.value()->distortion_coefficients[index] = source.d[index + 2];\n      }\n      raw_gxf_camera_model.value()->distortion_coefficients[6] = source.d[2];\n      raw_gxf_camera_model.value()->distortion_coefficients[7] = source.d[3];\n    }\n  } else if (raw_gxf_camera_model.value()->distortion_type == DistortionType::Brown) {\n    // prevents distortion parameters array access if its empty\n    // simulators may send empty distortion parameter array since images are already rectified\n    if (!source.d.empty()) {\n      // distortion parameters in GXF: k1, k2, k3, k4, k5, k6, p1, p2\n      // distortion parameters in ROS message: k1, k2, p1, p2, k3 ...\n      raw_gxf_camera_model.value()->distortion_coefficients[0] = source.d[0];\n      raw_gxf_camera_model.value()->distortion_coefficients[1] = source.d[1];\n      raw_gxf_camera_model.value()->distortion_coefficients[2] = source.d[4];\n      for (uint16_t index = 3;\n        index < nvidia::gxf::CameraModel::kMaxDistortionCoefficients - 2;\n        index++)\n      {\n        raw_gxf_camera_model.value()->distortion_coefficients[index] = 0;\n      }\n      raw_gxf_camera_model.value()->distortion_coefficients[6] = source.d[2];\n      raw_gxf_camera_model.value()->distortion_coefficients[7] = source.d[3];\n    }\n  } else {\n    std::copy(\n      std::begin(source.d), std::end(source.d),\n      std::begin(raw_gxf_camera_model.value()->distortion_coefficients));\n  }\n\n  rect_gxf_camera_model.value()->dimensions = {source.width, source.height};\n  rect_gxf_camera_model.value()->focal_length = {\n    static_cast<float>(source.p[0]), static_cast<float>(source.p[5])};\n  rect_gxf_camera_model.value()->principal_point = {\n    static_cast<float>(source.p[2]), static_cast<float>(source.p[6])};\n\n  // populate rectification rotation matrix into the TARGET_EXTRINSICS_DELTA_GXF_NAME\n  std::copy(\n    std::begin(source.r), std::end(source.r),\n    std::begin(target_extrinsics_delta_gxf_pose_3d.value()->rotation));\n\n  // Based on the comments for the camera info msg type, p[0] == 0 means the topic\n  // contains no calibration data, ie, its an uncalibrated camera\n  if (source.p[0] == 0.0f) {\n    RCLCPP_WARN(\n      rclcpp::get_logger(\"NitrosCameraInfo\"),\n      \"[convert_to_custom] Received an uncalibrated camera info msg.\");\n    extrinsics_gxf_pose_3d.value()->translation = {\n      0,\n      0,\n      0};\n  } else {\n    // populate extrinsics translation the EXTRINSICS_GXF_NAME\n    extrinsics_gxf_pose_3d.value()->translation = {\n      static_cast<float>(source.p[3]) / static_cast<float>(source.p[0]),\n      static_cast<float>(source.p[7]),\n      static_cast<float>(source.p[11])};\n  }\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCameraInfo,\n  sensor_msgs::msg::CameraInfo>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosCameraInfo::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCameraInfo\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto raw_gxf_camera_model = msg_entity->get<nvidia::gxf::CameraModel>(RAW_CAMERA_MODEL_GXF_NAME);\n  if (!raw_gxf_camera_model) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get the Raw CameraModel object: \" <<\n      GxfResultStr(raw_gxf_camera_model.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto rect_gxf_camera_model =\n    msg_entity->get<nvidia::gxf::CameraModel>(RECT_CAMERA_MODEL_GXF_NAME);\n  // Fallback to raw intrinsics incase rect camera model is not available\n  if (!rect_gxf_camera_model) {\n    RCLCPP_WARN_ONCE(\n      rclcpp::get_logger(\n        \"NitrosCameraInfo\"),\n      \"[convert_to_ros_message] Failed to get the Rectified CameraModel object: \"\n      \"Falling back to raw camera model\");\n    rect_gxf_camera_model = raw_gxf_camera_model;\n  }\n\n  // Setting camera info from gxf camera model\n  destination.height = raw_gxf_camera_model.value()->dimensions.y;\n  destination.width = raw_gxf_camera_model.value()->dimensions.x;\n\n  const auto distortion = g_gxf_to_ros_distortion_model.find(\n    raw_gxf_camera_model.value()->distortion_type);\n  if (distortion == std::end(g_gxf_to_ros_distortion_model)) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Unsupported distortion model from gxf [\" <<\n      static_cast<int>(raw_gxf_camera_model.value()->distortion_type) << \"].\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  } else {\n    destination.distortion_model = distortion->second;\n  }\n\n  if (raw_gxf_camera_model.value()->distortion_type == DistortionType::Polynomial) {\n    // Resize d buffer to the right size\n    destination.d.resize(\n      sizeof(raw_gxf_camera_model.value()->distortion_coefficients) /\n      sizeof(float));\n    destination.d[0] = raw_gxf_camera_model.value()->distortion_coefficients[0];\n    destination.d[1] = raw_gxf_camera_model.value()->distortion_coefficients[1];\n    destination.d[2] = raw_gxf_camera_model.value()->distortion_coefficients[6];\n    destination.d[3] = raw_gxf_camera_model.value()->distortion_coefficients[7];\n    destination.d[4] = raw_gxf_camera_model.value()->distortion_coefficients[2];\n    destination.d[5] = raw_gxf_camera_model.value()->distortion_coefficients[3];\n    destination.d[6] = raw_gxf_camera_model.value()->distortion_coefficients[4];\n    destination.d[7] = raw_gxf_camera_model.value()->distortion_coefficients[5];\n  } else if (raw_gxf_camera_model.value()->distortion_type == DistortionType::Brown) {\n    // Resize d buffer to the right size\n    destination.d.resize(NUM_COFF_PLUMB_BOB);\n    destination.d[0] = raw_gxf_camera_model.value()->distortion_coefficients[0];\n    destination.d[1] = raw_gxf_camera_model.value()->distortion_coefficients[1];\n    destination.d[2] = raw_gxf_camera_model.value()->distortion_coefficients[6];\n    destination.d[3] = raw_gxf_camera_model.value()->distortion_coefficients[7];\n    destination.d[4] = raw_gxf_camera_model.value()->distortion_coefficients[2];\n  } else if (raw_gxf_camera_model.value()->distortion_type == DistortionType::Perspective) {\n    // Resize d buffer to the right size\n    destination.d.resize(NUM_COFF_PLUMB_BOB);\n    destination.d[0] = 0.0;\n    destination.d[1] = 0.0;\n    destination.d[2] = 0.0;\n    destination.d[3] = 0.0;\n    destination.d[4] = 0.0;\n  } else {\n    // Resize d buffer to the right size\n    destination.d.resize(\n      sizeof(raw_gxf_camera_model.value()->distortion_coefficients) /\n      sizeof(float));\n    std::copy(\n      std::begin(raw_gxf_camera_model.value()->distortion_coefficients),\n      std::end(raw_gxf_camera_model.value()->distortion_coefficients), std::begin(destination.d));\n  }\n\n  destination.k[0] = raw_gxf_camera_model.value()->focal_length.x;\n  destination.k[1] = 0;\n  destination.k[2] = raw_gxf_camera_model.value()->principal_point.x;\n  destination.k[3] = 0;\n  destination.k[4] = raw_gxf_camera_model.value()->focal_length.y;\n  destination.k[5] = raw_gxf_camera_model.value()->principal_point.y;\n  destination.k[6] = 0;\n  destination.k[7] = 0;\n  destination.k[8] = 1;\n\n  // Setting extrinsic info from gxf pose 3D\n  auto extrinsics_gxf_pose_3d = msg_entity->get<nvidia::gxf::Pose3D>(EXTRINSICS_GXF_NAME);\n  if (!extrinsics_gxf_pose_3d) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get the extrinsics Pose3D object: \" <<\n      GxfResultStr(extrinsics_gxf_pose_3d.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto target_extrinsics_delta_gxf_pose_3d = msg_entity->get<nvidia::gxf::Pose3D>(\n    TARGET_EXTRINSICS_DELTA_GXF_NAME);\n\n  // Fallback to extrinsics Pose3D object\n  if (!target_extrinsics_delta_gxf_pose_3d) {\n    std::stringstream warn_msg;\n    warn_msg <<\n      \"[convert_to_ros_message] Failed to get the target extrinsics delta Pose3D object: \" <<\n      GxfResultStr(target_extrinsics_delta_gxf_pose_3d.error()) <<\n      \"Falling back to extrinsics Pose3D object\";\n    RCLCPP_WARN_ONCE(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), warn_msg.str().c_str());\n\n    target_extrinsics_delta_gxf_pose_3d = extrinsics_gxf_pose_3d;\n  }\n\n  std::copy(\n    std::begin(target_extrinsics_delta_gxf_pose_3d.value()->rotation),\n    std::end(target_extrinsics_delta_gxf_pose_3d.value()->rotation),\n    std::begin(destination.r));\n\n  // The left 3*3 portion of the P-matrix specifies the intrinsic of rectified image\n  // The right 1*3 vector specifies the tranlsation vector\n  destination.p[0] = rect_gxf_camera_model.value()->focal_length.x;\n  destination.p[1] = 0;\n  destination.p[2] = rect_gxf_camera_model.value()->principal_point.x;\n  destination.p[3] = extrinsics_gxf_pose_3d.value()->translation[0] * destination.p[0];\n  destination.p[4] = 0;\n  destination.p[5] = rect_gxf_camera_model.value()->focal_length.y;\n  destination.p[6] = rect_gxf_camera_model.value()->principal_point.y;\n  destination.p[7] = extrinsics_gxf_pose_3d.value()->translation[1];\n  destination.p[8] = 0;\n  destination.p[9] = 0;\n  destination.p[10] = 1;\n  destination.p[11] = extrinsics_gxf_pose_3d.value()->translation[2];\n\n  destination.binning_x = 1;    // No subsampling\n  destination.binning_y = 1;    // No subsampling\n  destination.roi.height = raw_gxf_camera_model.value()->dimensions.y;    // Full resolution\n  destination.roi.width = raw_gxf_camera_model.value()->dimensions.x;    // Full resolution\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!input_timestamp) {    // Fallback to label 'timestamp'\n    input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>();\n  }\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp.value()->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp.value()->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Set frame ID\n  destination.header.frame_id = source.frame_id;\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCameraInfo\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCameraInfo,\n  sensor_msgs::msg::CameraInfo>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosCameraInfo::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCameraInfo\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  auto message = nvidia::gxf::Entity::New(context);\n  if (!message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error initializing new message entity: \" <<\n      GxfResultStr(message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  auto raw_gxf_camera_model = message->add<nvidia::gxf::CameraModel>(RAW_CAMERA_MODEL_GXF_NAME);\n  auto rect_gxf_camera_model = message->add<nvidia::gxf::CameraModel>(RECT_CAMERA_MODEL_GXF_NAME);\n  // this pose3D entity is used to passthrough the translation between the two cameras\n  // this translation data is not used by the tensorops rectification library\n  // Note: The rotation of this EXTRINSICS_GXF_NAME pose3D entity is set identity\n  // since the information cannot be calculated from a single camera info msg\n  auto extrinsics_gxf_pose_3d = message->add<nvidia::gxf::Pose3D>(EXTRINSICS_GXF_NAME);\n  // this pose3D entity is used to send the rectification matrix\n  // Note: translation of this TARGET_EXTRINSICS_DELTA_GXF_NAME pose3D entity is always zero\n  auto target_extrinsics_delta_gxf_pose_3d = message->add<nvidia::gxf::Pose3D>(\n    TARGET_EXTRINSICS_DELTA_GXF_NAME);\n\n\n  nvidia::isaac_ros::nitros::copy_ros_to_gxf_camera_info(source, message);\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  auto output_timestamp = message->add<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!output_timestamp) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Failed to add a timestamp component to message: \" <<\n      GxfResultStr(output_timestamp.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCameraInfo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  output_timestamp.value()->acqtime = input_timestamp;\n\n  // Set frame ID\n  destination.frame_id = source.header.frame_id;\n\n  // Set Entity Id\n  destination.handle = message->eid();\n  GxfEntityRefCountInc(context, message->eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCameraInfo\"),\n    \"[convert_to_custom] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_camera_info_type/src/nitros_camera_info_view.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_camera_info_type/nitros_camera_info_view.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nvoid NitrosCameraInfoView::InitView() {}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_camera_info_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_camera_info_type/test/isaac_ros_nitros_camera_info_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosCameraInfo type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import CameraInfo\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosCameraInfoTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_camera_info_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosCameraInfoForwardNode',\n                name='NitrosCameraInfoForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_camera_info'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosCameraInfoTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosCameraInfoTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosCameraInfo type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_camera_info')\n    def test_nitros_camera_info_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the camera info from NitrosCameraInfo type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n        kMaxDistortionCoefficients = 8\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', CameraInfo)],\n            received_messages=received_messages\n        )\n\n        camera_info_pub = self.node.create_publisher(\n            CameraInfo, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            camera_info = JSONConversion.load_camera_info_from_json(\n                test_folder / 'camera_info.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 2\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                camera_info.header.stamp = timestamp\n\n                camera_info_pub.publish(camera_info)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_camera_info = received_messages['output']\n\n            # Header\n            # Frame ID is to be passed from NitrosImage to ROS message\n            # self.assertEqual(camera_info.header.frame_id, received_camera_info.header.frame_id)\n\n            # Size\n            self.assertEqual(camera_info.height, received_camera_info.height)\n            self.assertEqual(camera_info.width, received_camera_info.width)\n\n            # Distortion model\n            self.assertEqual(camera_info.distortion_model, received_camera_info.distortion_model)\n\n            self.assertLessEqual(len(received_camera_info.d), kMaxDistortionCoefficients)\n\n            # D\n            for i in range(min(len(camera_info.d), len(received_camera_info.d))):\n                self.assertEqual(\n                    round(camera_info.d[i], 2), round(received_camera_info.d[i], 2),\n                    f'{i+1}th D value does not match')\n\n            # K\n            self.assertEqual(len(camera_info.k), len(received_camera_info.k))\n            for i in range(min(len(camera_info.k), len(received_camera_info.k))):\n                self.assertEqual(\n                    round(camera_info.k[i], 2), round(received_camera_info.k[i], 2),\n                    f'{i+1}th K value does not match')\n\n            # R\n            self.assertEqual(len(camera_info.r), len(received_camera_info.r))\n            for i in range(len(received_camera_info.r)):\n                self.assertEqual(\n                    round(camera_info.r[i], 2), round(received_camera_info.r[i], 2),\n                    f'{i+1}th R value does not match')\n\n            # P\n            self.assertEqual(len(camera_info.p), len(received_camera_info.p))\n            # The P matrix is a 3*4 matrix\n            for i in range(3):\n                for j in range(4):\n                    self.assertEqual(\n                            round(camera_info.p[i*3+j], 2),\n                            round(received_camera_info.p[i*3+j], 2),\n                            f'{i+1}th P value does not match')\n\n            print('The received camera info is verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(camera_info_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_camera_info_type/test/src/nitros_camera_info_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_camera_info_type/nitros_camera_info.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_camera_info_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_camera_info\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosCameraInfoForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosCameraInfoForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosCameraInfo>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosCameraInfoForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_camera_info_type/test/test_cases/nitros_camera_info/profile/camera_info.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"tf_camera\"\n  },\n  \"height\": 460,\n  \"width\": 460,\n  \"distortion_model\": \"plumb_bob\",\n  \"D\": [\n    -0.363528858080088,\n    0.16117037733986861,\n    -8.1109585007538829e-05,\n    -0.00044776712298447841,\n    0.0\n  ],\n  \"K\": [\n    430.15433020105519,\n    0.0,\n    311.71339830549732,\n    0.0,\n    430.60920415473657,\n    221.06824942698509,\n    0.0,\n    0.0,\n    1.0\n  ],\n  \"R\": [\n    0.99806560714807102,\n    0.0068562422224214027,\n    0.061790256276695904,\n    -0.0067522959054715113,\n    0.99997541519165112,\n    -0.0018909025066874664,\n    -0.061801701660692349,\n    0.0014700186639396652,\n    0.99808736527268516\n  ],\n  \"P\": [\n    295.53402059708782,\n    0.0,\n    285.55760765075684,\n    0.0,\n    0.0,\n    295.53402059708782,\n    223.29617881774902,\n    0.0,\n    0.0,\n    0.0,\n    1.0,\n    0.0\n  ]\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_image_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_compressed_image_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(yaml-cpp)\n\n# NitrosCompressedImage\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_compressed_image.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES BUILD_RPATH_USE_ORIGIN TRUE)\nset_target_properties(${PROJECT_NAME} PROPERTIES INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosCompressedImageForwardNode\n  ament_auto_add_library(nitros_compressed_image_forward_node SHARED test/src/nitros_compressed_image_forward_node.cpp)\n  target_link_libraries(nitros_compressed_image_forward_node ${PROJECT_NAME})\n  set_target_properties(nitros_compressed_image_forward_node PROPERTIES BUILD_RPATH_USE_ORIGIN TRUE)\n  set_target_properties(nitros_compressed_image_forward_node PROPERTIES INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(nitros_compressed_image_forward_node \"nvidia::isaac_ros::nitros::NitrosCompressedImageForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosCompressedImage;$<TARGET_FILE:nitros_compressed_image_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_compressed_image_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_image_type/include/isaac_ros_nitros_compressed_image_type/nitros_compressed_image.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_COMPRESSED_IMAGE_TYPE__NITROS_COMPRESSED_IMAGE_HPP_\n#define ISAAC_ROS_NITROS_COMPRESSED_IMAGE_TYPE__NITROS_COMPRESSED_IMAGE_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosCompressedImage\n *   ROS type:    sensor_msgs::msg::CompressedImage\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"sensor_msgs/msg/compressed_image.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosCompressedImage;\n\n// Formats\nstruct nitros_compressed_image_t\n{\n  using MsgT = NitrosCompressedImage;\n  static const inline std::string supported_type_name = \"nitros_compressed_image\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosCompressedImage)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_compressed_image_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/std/libgxf_std.so\")\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/multimedia/libgxf_multimedia.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCompressedImage,\n  sensor_msgs::msg::CompressedImage>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosCompressedImage;\n  using ros_message_type = sensor_msgs::msg::CompressedImage;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosCompressedImage,\n  sensor_msgs::msg::CompressedImage);\n\n#endif  // ISAAC_ROS_NITROS_COMPRESSED_IMAGE_TYPE__NITROS_COMPRESSED_IMAGE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_image_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_compressed_image_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS NITROS Compressed Image Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n  <author>Yuankun Zhu</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>sensor_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_image_type/src/nitros_compressed_image.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include <cuda_runtime.h>\n\n#include <string>\n#include <unordered_map>\n#include <vector>\n#include <iostream>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/multimedia/camera.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#include \"gxf/std/tensor.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n#include \"isaac_ros_nitros_compressed_image_type/nitros_compressed_image.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace\n{\nconstexpr char kEntityName[] = \"memory_pool\";\nconstexpr char kComponentName[] = \"unbounded_allocator\";\nconstexpr char kComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\n}  // namespace\n\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCompressedImage,\n  sensor_msgs::msg::CompressedImage>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosCompressedImage::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCompressedImage\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto maybe_gxf_tensor = msg_entity->get<nvidia::gxf::Tensor>();\n  if (!maybe_gxf_tensor) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get tensor component from message entity: \" <<\n      GxfResultStr(maybe_gxf_tensor.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  auto gxf_tensor = maybe_gxf_tensor.value();\n\n  destination.format = \"h264\";\n  destination.data.resize(gxf_tensor->size());\n\n  // compressed results are reside on CPU\n  const cudaError_t cuda_error = cudaMemcpyAsync(\n    destination.data.data(),\n    gxf_tensor->data<uint8_t>().value(), gxf_tensor->size(),\n    cudaMemcpyHostToHost, source.cuda_stream);\n\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] cudaMemcpy failed for conversion from \"\n      \"NitrosCompressedImage to sensor_msgs::msg::CompressedImage: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  cudaError_t cuda_result = cudaStreamSynchronize(source.cuda_stream);\n  CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!input_timestamp) {    // Fallback to any 'timestamp'\n    input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>();\n  }\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp.value()->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp.value()->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Set frame ID\n  destination.header.frame_id = source.frame_id;\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCompressedImage\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCompressedImage,\n  sensor_msgs::msg::CompressedImage>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosCompressedImage::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCompressedImage\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  auto message = nvidia::gxf::Entity::New(context);\n  if (!message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error initializing new message entity: \" <<\n      GxfResultStr(message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  auto gxf_tensor = message->add<nvidia::gxf::Tensor>(\"compressed_image\");\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  // Allocate memory of gxf tensor.\n  auto gxf_tensor_result = gxf_tensor.value()->reshape<uint8_t>(\n    nvidia::gxf::Shape{static_cast<int>(source.data.size())}, nvidia::gxf::MemoryStorageType::kHost,\n    allocator_handle);\n  if (!gxf_tensor_result) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error Creating tensors: \" <<\n      GxfResultStr(gxf_tensor_result.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto nitros_cuda_stream =\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext()\n    .getCudaStreamFromNitrosGraph();\n  // Copy data from compressed image msg to gxf tensor.\n  const cudaError_t cuda_error = cudaMemcpyAsync(\n    gxf_tensor.value()->data<uint8_t>().value(),\n    source.data.data(), source.data.size(), cudaMemcpyHostToHost, nitros_cuda_stream);\n\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] cudaMemcpyAsync failed for copying data from \"\n      \"sensor_msgs::msg::CompressedImage to NitrosCompressedImage: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  cudaError_t cuda_result = cudaStreamSynchronize(nitros_cuda_stream);\n  CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  auto output_timestamp = message->add<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!output_timestamp) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Failed to add a timestamp component to message: \" <<\n      GxfResultStr(output_timestamp.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  output_timestamp.value()->acqtime = input_timestamp;\n\n  // Set frame ID\n  destination.frame_id = source.header.frame_id;\n\n  // Set Entity Id\n  destination.handle = message->eid();\n  GxfEntityRefCountInc(context, message->eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCompressedImage\"),\n    \"[convert_to_custom] Conversion completed (resulting handle=%ld)\", message->eid());\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_image_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_image_type/test/isaac_ros_nitros_compressed_image_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosCompressedImage type adapter.\"\"\"\n\nimport random\nimport time\n\n\nfrom isaac_ros_test import IsaacROSBaseTest\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import CompressedImage\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosCompressedImageTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='compressed_image_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_compressed_image_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosCompressedImageForwardNode',\n                name='NitrosCompressedImageForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_compressed_image'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosCompressedImageTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosCompressedImageTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosCompressedImage type adapter.\"\"\"\n\n    def test_nitros_compressed_image_type_conversions(self) -> None:\n        \"\"\"Expect data received to be identical to source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_compressed_img_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', CompressedImage)],\n            received_messages=received_messages\n        )\n\n        compressed_img_pub = self.node.create_publisher(\n            CompressedImage, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            compressed_img_msg = CompressedImage()\n            compressed_img_msg.format = 'h264'\n            compressed_img_msg.data = [random.randint(0, 255) for _ in range(100)]\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 2\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                compressed_img_msg.header.stamp = timestamp\n\n                compressed_img_pub.publish(compressed_img_msg)\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on output_image topic!\")\n\n            received_compressed_img = received_messages['output']\n\n            self.assertEqual(len(compressed_img_msg.data), len(received_compressed_img.data),\n                             'Source and received image sizes do not match: ' +\n                             f'{len(compressed_img_msg.data)} \\\n                             != {len(received_compressed_img.data)}')\n\n            print('Source and received images are identical.')\n        finally:\n            self.node.destroy_subscription(received_compressed_img_sub)\n            self.node.destroy_publisher(compressed_img_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_image_type/test/src/nitros_compressed_image_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_compressed_image_type/nitros_compressed_image.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_compressed_image_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_compressed_image\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosCompressedImageForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosCompressedImageForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosCompressedImage>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosCompressedImageForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_video_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_compressed_video_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(yaml-cpp)\n\n# NitrosCompressedVideo\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_compressed_video.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosCompressedVideoForwardNode\n  ament_auto_add_library(nitros_compressed_video_forward_node SHARED test/src/nitros_compressed_video_forward_node.cpp)\n  target_link_libraries(nitros_compressed_video_forward_node ${PROJECT_NAME})\n  set_target_properties(nitros_compressed_video_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(nitros_compressed_video_forward_node \"nvidia::isaac_ros::nitros::NitrosCompressedVideoForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosCompressedVideo;$<TARGET_FILE:nitros_compressed_video_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_compressed_video_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_video_type/include/isaac_ros_nitros_compressed_video_type/nitros_compressed_video.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_COMPRESSED_VIDEO_TYPE__NITROS_COMPRESSED_VIDEO_HPP_\n#define ISAAC_ROS_NITROS_COMPRESSED_VIDEO_TYPE__NITROS_COMPRESSED_VIDEO_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosCompressedVideo\n *   ROS type:    foxglove_msgs::msg::CompressedVideo\n */\n\n#include <string>\n\n#include <foxglove_msgs/msg/compressed_video.hpp>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosCompressedVideo;\n\n// Formats\nstruct nitros_compressed_video_t\n{\n  using MsgT = NitrosCompressedVideo;\n  static const inline std::string supported_type_name = \"nitros_compressed_video\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosCompressedVideo)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_compressed_video_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/std/libgxf_std.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCompressedVideo,\n  foxglove_msgs::msg::CompressedVideo>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosCompressedVideo;\n  using ros_message_type = foxglove_msgs::msg::CompressedVideo;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosCompressedVideo,\n  foxglove_msgs::msg::CompressedVideo);\n\n#endif  // ISAAC_ROS_NITROS_COMPRESSED_VIDEO_TYPE__NITROS_COMPRESSED_VIDEO_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_video_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_compressed_video_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS NITROS Compressed Video Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n  <author>Ayusman Saha</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>foxglove_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_video_type/src/nitros_compressed_video.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include <cuda_runtime.h>\n\n#include <sstream>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/timestamp.hpp\"\n#include \"gxf/std/tensor.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n#include \"isaac_ros_nitros_compressed_video_type/nitros_compressed_video.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\n\nnamespace\n{\nconstexpr char kEntityName[] = \"memory_pool\";\nconstexpr char kComponentName[] = \"unbounded_allocator\";\nconstexpr char kComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\n}  // namespace\n\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCompressedVideo,\n  foxglove_msgs::msg::CompressedVideo>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosCompressedVideo::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCompressedVideo\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto maybe_gxf_tensor = msg_entity->get<nvidia::gxf::Tensor>();\n  if (!maybe_gxf_tensor) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get tensor component from message entity: \" <<\n      GxfResultStr(maybe_gxf_tensor.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedVideo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  auto gxf_tensor = maybe_gxf_tensor.value();\n\n  destination.format = \"h264\";\n  destination.data.resize(gxf_tensor->size());\n\n  // compressed results are reside on CPU\n  const cudaError_t cuda_error = cudaMemcpyAsync(\n    destination.data.data(),\n    gxf_tensor->data<uint8_t>().value(), gxf_tensor->size(),\n    cudaMemcpyHostToHost, source.cuda_stream);\n\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] cudaMemcpyAsync failed for conversion from \"\n      \"NitrosCompressedVideo to foxglove_msgs::msg::CompressedVideo: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedVideo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  cudaError_t cuda_result = cudaStreamSynchronize(source.cuda_stream);\n  CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!input_timestamp) {    // Fallback to any 'timestamp'\n    input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>();\n  }\n  if (input_timestamp) {\n    destination.timestamp.sec = static_cast<int32_t>(\n      input_timestamp.value()->acqtime / static_cast<uint64_t>(1e9));\n    destination.timestamp.nanosec = static_cast<uint32_t>(\n      input_timestamp.value()->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Set frame ID\n  destination.frame_id = source.frame_id;\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCompressedVideo\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCompressedVideo,\n  foxglove_msgs::msg::CompressedVideo>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosCompressedVideo::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCompressedVideo\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  auto message = nvidia::gxf::Entity::New(context);\n  if (!message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error initializing new message entity: \" <<\n      GxfResultStr(message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedVideo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  auto gxf_tensor = message->add<nvidia::gxf::Tensor>(\"compressed_video\");\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedVideo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  // Allocate memory of gxf tensor.\n  auto gxf_tensor_result = gxf_tensor.value()->reshape<uint8_t>(\n    nvidia::gxf::Shape{static_cast<int>(source.data.size())}, nvidia::gxf::MemoryStorageType::kHost,\n    allocator_handle);\n  if (!gxf_tensor_result) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error Creating tensors: \" <<\n      GxfResultStr(gxf_tensor_result.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedVideo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto nitros_cuda_stream =\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext()\n    .getCudaStreamFromNitrosGraph();\n  // Copy data from compressed video msg to gxf tensor.\n  const cudaError_t cuda_error = cudaMemcpyAsync(\n    gxf_tensor.value()->data<uint8_t>().value(),\n    source.data.data(), source.data.size(), cudaMemcpyHostToHost, nitros_cuda_stream);\n\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] cudaMemcpy failed for copying data from \"\n      \"foxglove_msgs::msg::CompressedVideo to NitrosCompressedVideo: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedVideo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  cudaError_t cuda_result = cudaStreamSynchronize(nitros_cuda_stream);\n  CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.timestamp.sec * static_cast<uint64_t>(1e9) +\n    source.timestamp.nanosec;\n  auto output_timestamp = message->add<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!output_timestamp) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Failed to add a timestamp component to message: \" <<\n      GxfResultStr(output_timestamp.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCompressedVideo\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  output_timestamp.value()->acqtime = input_timestamp;\n\n  // Set frame ID\n  destination.frame_id = source.frame_id;\n\n  // Set Entity Id\n  destination.handle = message->eid();\n  GxfEntityRefCountInc(context, message->eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCompressedVideo\"),\n    \"[convert_to_custom] Conversion completed (resulting handle=%ld)\", message->eid());\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_video_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_video_type/test/isaac_ros_nitros_compressed_video_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosCompressedVideo type adapter.\"\"\"\n\nimport random\nimport time\n\nfrom foxglove_msgs.msg import CompressedVideo\nfrom isaac_ros_test import IsaacROSBaseTest\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosCompressedVideoTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='compressed_video_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_compressed_video_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosCompressedVideoForwardNode',\n                name='NitrosCompressedVideoForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_compressed_video'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosCompressedVideoTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosCompressedVideoTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosCompressedVideo type adapter.\"\"\"\n\n    def test_nitros_compressed_video_type_conversions(self) -> None:\n        \"\"\"Expect data received to be identical to source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_compressed_video_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', CompressedVideo)],\n            received_messages=received_messages\n        )\n\n        compressed_video_pub = self.node.create_publisher(\n            CompressedVideo, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            compressed_video_msg = CompressedVideo()\n            compressed_video_msg.format = 'h264'\n            compressed_video_msg.data = [random.randint(0, 255) for _ in range(100)]\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 2\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                compressed_video_msg.timestamp = timestamp\n\n                compressed_video_pub.publish(compressed_video_msg)\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, 'Did not receive message on output topic!')\n\n            received_compressed_video = received_messages['output']\n\n            self.assertEqual(compressed_video_msg.format, received_compressed_video.format,\n                             'Source and received video format do not match: ' +\n                             f'{compressed_video_msg.format} \\\n                             != {received_compressed_video.format}')\n\n            self.assertEqual(compressed_video_msg.data, received_compressed_video.data,\n                             'Source and received video data do not match')\n\n            print('Source and received videos are identical.')\n        finally:\n            self.node.destroy_subscription(received_compressed_video_sub)\n            self.node.destroy_publisher(compressed_video_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_compressed_video_type/test/src/nitros_compressed_video_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <string>\n\n#include \"isaac_ros_nitros_compressed_video_type/nitros_compressed_video.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_compressed_video_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_compressed_video\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosCompressedVideoForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosCompressedVideoForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosCompressedVideo>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosCompressedVideoForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_correlated_timestamp_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_correlated_timestamp_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(yaml-cpp)\n\n# NitrosCorrelatedTimestamp\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_correlated_timestamp.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosCorrelatedTimestampForwardNode\n  ament_auto_add_library(isaac_ros_nitros_correlated_timestamp_forward_node SHARED test/src/nitros_correlated_timestamp_forward_node.cpp)\n  target_link_libraries(isaac_ros_nitros_correlated_timestamp_forward_node ${PROJECT_NAME})\n  set_target_properties(isaac_ros_nitros_correlated_timestamp_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(isaac_ros_nitros_correlated_timestamp_forward_node\n    \"nvidia::isaac_ros::nitros::NitrosCorrelatedTimestampForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosCorrelatedTimestampForwardNode;\\\n    $<TARGET_FILE:isaac_ros_nitros_correlated_timestamp_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_correlated_timestamp_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_correlated_timestamp_type/include/isaac_ros_nitros_correlated_timestamp_type/nitros_correlated_timestamp.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_CORRELATED_TIMESTAMP_TYPE__NITROS_CORRELATED_TIMESTAMP_HPP_\n#define ISAAC_ROS_NITROS_CORRELATED_TIMESTAMP_TYPE__NITROS_CORRELATED_TIMESTAMP_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosCorrelatedTimestamp\n *   ROS type:    isaac_ros_nova_interfaces::msg::CorrelatedTimestamp\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"isaac_ros_nova_interfaces/msg/correlated_timestamp.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosCorrelatedTimestamp;\n\n// Formats\nstruct nitros_correlated_timestamp_t\n{\n  using MsgT = NitrosCorrelatedTimestamp;\n  static const inline std::string supported_type_name = \"nitros_correlated_timestamp\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosCorrelatedTimestamp)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_correlated_timestamp_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\n// for nvidia::isaac::StringProvider\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_gxf_helpers\", \"gxf/lib/libgxf_isaac_gxf_helpers.so\")\n// for nvidia::isaac::SightReceiver\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_sight\", \"gxf/lib/libgxf_isaac_sight.so\")\n// for nvidia::isaac::PoseTree\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_atlas\", \"gxf/lib/libgxf_isaac_atlas.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_messages\", \"gxf/lib/libgxf_isaac_messages.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCorrelatedTimestamp,\n  isaac_ros_nova_interfaces::msg::CorrelatedTimestamp>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosCorrelatedTimestamp;\n  using ros_message_type = isaac_ros_nova_interfaces::msg::CorrelatedTimestamp;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosCorrelatedTimestamp,\n  isaac_ros_nova_interfaces::msg::CorrelatedTimestamp);\n\n#endif  // ISAAC_ROS_NITROS_CORRELATED_TIMESTAMP_TYPE__NITROS_CORRELATED_TIMESTAMP_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_correlated_timestamp_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2023-2024, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_correlated_timestamp_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS NITROS Correlated Timestamp Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Ashwin Varghese Kuruttukulam</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>isaac_ros_nova_interfaces</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>gxf_isaac_messages</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <exec_depend>gxf_isaac_atlas</exec_depend>\n  <exec_depend>gxf_isaac_gxf_helpers</exec_depend>\n  <exec_depend>gxf_isaac_sight</exec_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_correlated_timestamp_type/src/nitros_correlated_timestamp.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"extensions/messages/correlated_timestamp_message.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_correlated_timestamp_type/nitros_correlated_timestamp.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCorrelatedTimestamp,\n  isaac_ros_nova_interfaces::msg::CorrelatedTimestamp>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosCorrelatedTimestamp::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCorrelatedTimestamp\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto maybe_correlated_timestamp_parts = nvidia::isaac::GetCorrelatedTimestampMessage(\n    msg_entity.value());\n  if (!maybe_correlated_timestamp_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get correlated_timestamp gxf message\"\n      \" from message entity: \" <<\n      GxfResultStr(maybe_correlated_timestamp_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCorrelatedTimestamp\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto correlated_timestamp_parts = maybe_correlated_timestamp_parts.value();\n  destination.phc_val = correlated_timestamp_parts.correlated_timestamps->phc_val;\n  destination.tsc_val = correlated_timestamp_parts.correlated_timestamps->tsc_val;\n  destination.phc2_val = correlated_timestamp_parts.correlated_timestamps->phc2_val;\n  destination.sys_val = correlated_timestamp_parts.correlated_timestamps->sys_val;\n  destination.phc_latency = correlated_timestamp_parts.correlated_timestamps->phc_latency;\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = correlated_timestamp_parts.timestamp;\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCorrelatedTimestamp\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosCorrelatedTimestamp,\n  isaac_ros_nova_interfaces::msg::CorrelatedTimestamp>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosCorrelatedTimestamp::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCorrelatedTimestamp\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // // Get pointer to allocator component\n  // gxf_uid_t cid;\n  // nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n  //   kEntityName, kComponentName, kComponentTypeName, cid);\n  // auto maybe_allocator_handle =\n  //   nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  // if (!maybe_allocator_handle) {\n  //   std::stringstream error_msg;\n  //   error_msg <<\n  //     \"[convert_to_custom] Failed to get allocator's handle: \" <<\n  //     GxfResultStr(maybe_allocator_handle.error());\n  //   RCLCPP_ERROR(\n  //     rclcpp::get_logger(\"NitrosCorrelatedTimestamp\"), error_msg.str().c_str());\n  //   throw std::runtime_error(error_msg.str().c_str());\n  // }\n  // auto allocator_handle = maybe_allocator_handle.value();\n\n  // create correlated_timestamp gxf msg\n  auto maybe_correlated_timestamp_parts = nvidia::isaac::CreateCorrelatedTimestampMessage(context);\n  if (!maybe_correlated_timestamp_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_gxf_message] Failed to create correlated_timestamp gxf msg \" << GxfResultStr(\n      maybe_correlated_timestamp_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosCorrelatedTimestamp\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto correlated_timestamp_parts = maybe_correlated_timestamp_parts.value();\n\n  // Add correlated_timestamp data\n  correlated_timestamp_parts.correlated_timestamps->phc_val = source.phc_val;\n  correlated_timestamp_parts.correlated_timestamps->tsc_val = source.tsc_val;\n  correlated_timestamp_parts.correlated_timestamps->phc2_val = source.phc2_val;\n  correlated_timestamp_parts.correlated_timestamps->sys_val = source.sys_val;\n  correlated_timestamp_parts.correlated_timestamps->phc_latency = source.phc_latency;\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  correlated_timestamp_parts.timestamp->acqtime = input_timestamp;\n\n  // Set Entity Id\n  destination.handle = correlated_timestamp_parts.entity.eid();\n  GxfEntityRefCountInc(context, correlated_timestamp_parts.entity.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosCorrelatedTimestamp\"),\n    \"[convert_to_custom] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_correlated_timestamp_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_correlated_timestamp_type/test/isaac_ros_nitros_correlated_timestamp_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosCorrelatedTimestamp type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_nova_interfaces.msg import CorrelatedTimestamp\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosCorrelatedTimestampTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_correlated_timestamp_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosCorrelatedTimestampForwardNode',\n                name='NitrosCorrelatedTimestampForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_correlated_timestamp'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosCorrelatedTimestampTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosCorrelatedTimestampTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosCorrelatedTimestamp type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_correlated_timestamp')\n    def test_nitros_correlated_timestamp_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the tag from NitrosCorrelatedTimestamp type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', CorrelatedTimestamp)],\n            received_messages=received_messages\n        )\n\n        correlated_timestamp_pub = self.node.create_publisher(\n            CorrelatedTimestamp, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            correlated_timestamp = self.load_correlated_timestamp_from_json(\n                test_folder / 'correlated_timestamp.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 10\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                correlated_timestamp.header.stamp = timestamp\n\n                correlated_timestamp_pub.publish(correlated_timestamp)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_correlated_timestamp = received_messages['output']\n\n            # Only test for data passed through gxf\n            self.assertEqual(correlated_timestamp.header.stamp.sec,\n                             received_correlated_timestamp.header.stamp.sec)\n            self.assertEqual(correlated_timestamp.header.stamp.nanosec,\n                             received_correlated_timestamp.header.stamp.nanosec)\n            self.assertEqual(correlated_timestamp.phc_val,\n                             received_correlated_timestamp.phc_val)\n            self.assertEqual(correlated_timestamp.tsc_val,\n                             received_correlated_timestamp.tsc_val)\n            self.assertEqual(correlated_timestamp.phc2_val,\n                             received_correlated_timestamp.phc2_val)\n            self.assertEqual(correlated_timestamp.sys_val,\n                             received_correlated_timestamp.sys_val)\n            self.assertEqual(correlated_timestamp.phc_latency,\n                             received_correlated_timestamp.phc_latency)\n            print('The received correlated_timestamp message has been verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(correlated_timestamp_pub)\n\n    @staticmethod\n    def load_correlated_timestamp_from_json(\n            json_filepath: pathlib.Path) -> CorrelatedTimestamp:\n        \"\"\"\n        Load a CorrelatedTimestamp message from a JSON filepath.\n\n        Parameters\n        ----------\n        json_filepath : Path\n            The path to a JSON file containing the CorrelatedTimestamp fields\n\n        Returns\n        -------\n        CorrelatedTimestamp\n            Generated CorrelatedTimestamp message\n\n        \"\"\"\n        correlated_timestamp_json = JSONConversion.load_from_json(\n            json_filepath)\n\n        correlated_timestamp = CorrelatedTimestamp()\n        correlated_timestamp.header.frame_id = correlated_timestamp_json[\n            'header']['frame_id']\n        correlated_timestamp.header.stamp.sec = correlated_timestamp_json[\n            'header']['stamp']['sec']\n        correlated_timestamp.header.stamp.nanosec = correlated_timestamp_json[\n            'header']['stamp']['nanosec']\n        correlated_timestamp.phc_val = correlated_timestamp_json['phc_val']\n        correlated_timestamp.tsc_val = correlated_timestamp_json['tsc_val']\n        correlated_timestamp.phc2_val = correlated_timestamp_json['phc2_val']\n        correlated_timestamp.sys_val = correlated_timestamp_json['sys_val']\n        correlated_timestamp.phc_latency = correlated_timestamp_json['phc_latency']\n        return correlated_timestamp\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_correlated_timestamp_type/test/src/nitros_correlated_timestamp_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_correlated_timestamp_type/nitros_correlated_timestamp.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_correlated_timestamp_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_correlated_timestamp\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosCorrelatedTimestampForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosCorrelatedTimestampForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosCorrelatedTimestamp>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosCorrelatedTimestampForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_correlated_timestamp_type/test/test_cases/nitros_correlated_timestamp/profile/correlated_timestamp.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"\",\n    \"stamp\": {\n      \"sec\": 22125,\n      \"nanosec\": 45172\n    }\n  },\n  \"phc_val\": 541,\n  \"tsc_val\": 12351,\n  \"phc2_val\": 6235,\n  \"sys_val\": 256,\n  \"phc_latency\": 2345\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_detection2_d_array_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(yaml-cpp)\n\n# NitrosDetection2DArray\nament_auto_add_library(${PROJECT_NAME} SHARED\n  src/nitros_detection2_d_array.cpp\n  src/detection2_d_array_message.cpp\n)\ntarget_include_directories(${PROJECT_NAME} PUBLIC include/detectnet)\ntarget_link_libraries(${PROJECT_NAME}\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\n# Make library available for sharing in Nitros\ninstall(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION share/${PROJECT_NAME}/gxf/lib/detection2_d)\nament_export_include_directories(include/detectnet)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosDetection2DArrayForwardNode\n  ament_auto_add_library(isaac_ros_nitros_detection2_d_array_forward_node SHARED\n      test/src/nitros_detection2_d_array_forward_node.cpp\n  )\n  target_link_libraries(isaac_ros_nitros_detection2_d_array_forward_node ${PROJECT_NAME})\n  set_target_properties(isaac_ros_nitros_detection2_d_array_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(isaac_ros_nitros_detection2_d_array_forward_node\n      \"nvidia::isaac_ros::nitros::NitrosDetection2DArrayForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosDetection2DArrayForwardNode;\\\n      $<TARGET_FILE:isaac_ros_nitros_detection2_d_array_forward_node>\\n\")\n\n  # Test registration extension\n  ament_auto_add_library(isaac_ros_nitros_detection2_d_array_test_ext SHARED\n      test/src/detection2_d_test_ext.cpp\n  )\n  install(TARGETS isaac_ros_nitros_detection2_d_array_test_ext LIBRARY DESTINATION share/${PROJECT_NAME}/gxf/lib/detection2_d/test)\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n\n  # Ignore lint warnings from GXF source\n  set(AMENT_LINT_AUTO_FILE_EXCLUDE\n    include/detectnet/detection2_d.hpp\n    include/detectnet/detection2_d_array_message.hpp\n  )\n\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_detection2_d_array_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/include/detectnet/detection2_d.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef DETECTNET__DETECTION2_D_HPP_\n#define DETECTNET__DETECTION2_D_HPP_\n\n#include <string>\n#include <vector>\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\n\nstruct Hypothesis\n{\n  // The unique ID of the object class\n  std::string class_id;\n  // The probability or confidence value of the detected object\n  double score;\n};\n\n// Data structure holding meta information about detections\n// based on ros2 msg vision_msgs::Detection2DArray\n// https://github.com/ros-perception/vision_msgs/blob/ros2/msg/Detection2DArray.msg\nstruct Detection2D\n{\n  // The origin is the top left corner of the image\n  // The postive x axis is towards the right from the origin\n  // The postive y axis is towards the bottom from the origin\n  // The 2D position (in pixels) and orientation of the bounding box center\n  double center_x;\n  double center_y;\n  // The total size (in pixels) of the bounding box\n  double size_x;\n  double size_y;\n  // A vector of object classes and corresponding confidence values\n  // for the bounding box specified in this struct\n  std::vector<Hypothesis> results;\n};\n\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // DETECTNET__DETECTION2_D_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/include/detectnet/detection2_d_array_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef DETECTNET__DETECTION2_D_ARRAY_MESSAGE_HPP_\n#define DETECTNET__DETECTION2_D_ARRAY_MESSAGE_HPP_\n\n#include <vector>\n\n#include \"detection2_d.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\n\n// This struct helps parse the data coming from a gxf message. The message entity consists of a\n// `Detection2D` component and a `Timestamp` component.\n// Note that we do not add the raw type but rather a `Handle` to the type.\nstruct Detection2DParts\n{\n  gxf::Entity message;\n  gxf::Handle<std::vector<Detection2D>> detection2_d_array;\n  gxf::Handle<gxf::Timestamp> timestamp;\n};\n\n// This function creates a new entity and adds the (default-initialized) components from the parts\n// struct. It returns the created parts struct s.t. we can then use this to modify the data\n// contained in the entity.\ngxf::Expected<Detection2DParts> CreateDetection2DList(gxf_context_t context);\n\n// This function allows to parse an entity and returns the parts struct such that we have easy\n// access to the components.\ngxf::Expected<Detection2DParts> GetDetection2DList(gxf::Entity message);\n\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // DETECTNET__DETECTION2_D_ARRAY_MESSAGE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/include/isaac_ros_nitros_detection2_d_array_type/nitros_detection2_d_array.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_DETECTION2_D_ARRAY_TYPE__NITROS_DETECTION2_D_ARRAY_HPP_\n#define ISAAC_ROS_NITROS_DETECTION2_D_ARRAY_TYPE__NITROS_DETECTION2_D_ARRAY_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosDetection2DArray\n *   ROS type:    vision_msgs::msg::Detection2DArray\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"vision_msgs/msg/detection2_d_array.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosDetection2DArray;\n\n// Formats\nstruct nitros_detection2_d_array_t\n{\n  using MsgT = NitrosDetection2DArray;\n  static const inline std::string supported_type_name = \"nitros_detection2_d_array\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosDetection2DArray)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_detection2_d_array_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/std/libgxf_std.so\")\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosDetection2DArray,\n  vision_msgs::msg::Detection2DArray>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosDetection2DArray;\n  using ros_message_type = vision_msgs::msg::Detection2DArray;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosDetection2DArray,\n  vision_msgs::msg::Detection2DArray);\n\n#endif  // ISAAC_ROS_NITROS_DETECTION2_D_ARRAY_TYPE__NITROS_DETECTION2_D_ARRAY_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022-2025, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_detection2_d_array_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS Nitros Detection 2D Array Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Ashwin Varghese Kuruttukulam</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>negotiated</depend>\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>vision_msgs</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>ament_index_cpp</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/src/detection2_d_array_message.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"detection2_d_array_message.hpp\"\n\n#include <utility>\n#include <vector>\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\n\nnamespace\n{\nconstexpr char const * kDetection2DArrayIdentifier = \"detection2_d_array\";\nconstexpr char const * kTimestampIdentifier = \"timestamp\";\n}\n\ngxf::Expected<Detection2DParts> CreateDetection2DList(gxf_context_t context)\n{\n  Detection2DParts parts;\n  return gxf::Entity::New(context)\n         .assign_to(parts.message)\n         .and_then(\n    [&]() {\n      return parts.message.add<std::vector<nvidia::isaac_ros::Detection2D>>(\n        kDetection2DArrayIdentifier);\n    })\n         .assign_to(parts.detection2_d_array)\n         .and_then([&]() {return parts.message.add<gxf::Timestamp>(kTimestampIdentifier);})\n         .assign_to(parts.timestamp)\n         .substitute(parts);\n}\n\ngxf::Expected<Detection2DParts> GetDetection2DList(gxf::Entity message)\n{\n  Detection2DParts parts;\n  parts.message = message;\n  return parts.message.get<std::vector<nvidia::isaac_ros::Detection2D>>(kDetection2DArrayIdentifier)\n         .log_error(\n    \"Entity does not contain component ExampleData %s.\",\n    kDetection2DArrayIdentifier)\n         .assign_to(parts.detection2_d_array)\n         .and_then([&]() {return parts.message.get<gxf::Timestamp>(kTimestampIdentifier);})\n         .log_error(\"Entity does not contain component Timestamp %s.\", kTimestampIdentifier)\n         .assign_to(parts.timestamp)\n         .substitute(parts);\n}\n\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/src/nitros_detection2_d_array.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"detectnet/detection2_d_array_message.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_detection2_d_array_type/nitros_detection2_d_array.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n// This functions extracts data from a nvidia::isaac_ros::Detection2D object\n// and returns a vision_msgs::msg::Detection2D object\nvision_msgs::msg::Detection2D GetNewDetection2DMsg(\n  nvidia::isaac_ros::Detection2D detection2_d,\n  nvidia::gxf::Timestamp detection2_d_timestamp)\n{\n  vision_msgs::msg::Detection2D detection_msg;\n  detection_msg.header.stamp.sec = static_cast<int32_t>(\n    detection2_d_timestamp.acqtime / static_cast<uint64_t>(1e9));\n  detection_msg.header.stamp.nanosec = static_cast<uint32_t>(\n    detection2_d_timestamp.acqtime % static_cast<uint64_t>(1e9));\n\n  detection_msg.bbox.center.position.x = detection2_d.center_x;\n  detection_msg.bbox.center.position.y = detection2_d.center_y;\n  detection_msg.bbox.center.theta = 0;\n  detection_msg.bbox.size_x = detection2_d.size_x;\n  detection_msg.bbox.size_y = detection2_d.size_y;\n  std::vector<vision_msgs::msg::ObjectHypothesisWithPose> hypothesis_list;\n  vision_msgs::msg::ObjectHypothesisWithPose hypothesis;\n  for (size_t i = 0; i < detection2_d.results.size(); i++) {\n    hypothesis.hypothesis.class_id = detection2_d.results[i].class_id;\n    hypothesis.hypothesis.score = detection2_d.results[i].score;\n    hypothesis_list.push_back(hypothesis);\n  }\n  detection_msg.results = hypothesis_list;\n  return detection_msg;\n}\n\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosDetection2DArray,\n  vision_msgs::msg::Detection2DArray>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosDetection2DArray::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDetection2DArray\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  // Extract gxf message data to a struct type defined in detection2_d_array_message.hpp\n  auto detection2_d_parts_expected = nvidia::isaac_ros::GetDetection2DList(\n    msg_entity.value());\n  if (!detection2_d_parts_expected) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get detection2_d_array data from message entity: \" <<\n      GxfResultStr(detection2_d_parts_expected.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDetection2DArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto detection2_d_parts = detection2_d_parts_expected.value();\n\n  // Extract detection2_d array to a struct type defined in detection2_d.hpp\n  std::vector<nvidia::isaac_ros::Detection2D> detection2_d_array =\n    *(detection2_d_parts.detection2_d_array);\n\n  // Set timestamp for ros message from gxf message\n  nvidia::gxf::Timestamp detection2_d_timestamp = *(detection2_d_parts.timestamp);\n  destination.header.stamp.sec = static_cast<int32_t>(\n    detection2_d_timestamp.acqtime / static_cast<uint64_t>(1e9));\n  destination.header.stamp.nanosec = static_cast<uint32_t>(\n    detection2_d_timestamp.acqtime % static_cast<uint64_t>(1e9));\n\n  // Extract number of detections\n  size_t num_bboxes = detection2_d_array.size();\n  // resize output array to the number of tags detected before populating\n  destination.detections.resize(num_bboxes);\n\n  // Finally populate the ros message with detections\n  std::vector<vision_msgs::msg::Detection2D> detections_list;\n  for (size_t i = 0; i < num_bboxes; i++) {\n    vision_msgs::msg::Detection2D msg_detection = GetNewDetection2DMsg(\n      detection2_d_array[i],\n      detection2_d_timestamp);\n\n    detections_list.push_back(msg_detection);\n  }\n  destination.detections = detections_list;\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDetection2DArray\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosDetection2DArray,\n  vision_msgs::msg::Detection2DArray>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosDetection2DArray::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // Get pointer to allocator component\n  const std::string kEntityName = \"memory_pool\";\n  const std::string kComponentName = \"unbounded_allocator\";\n  const std::string kComponentTypeName = \"nvidia::gxf::UnboundedAllocator\";\n\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDetection2DArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  auto message = nvidia::gxf::Entity::New(context);\n  if (!message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error initializing new message entity: \" <<\n      GxfResultStr(message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDetection2DArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Extract timestamp from ros message and convert to gxf timestamp format\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n\n  // count number of tags first to know how many times to run the below for loop\n  size_t num_bboxes = source.detections.size();\n  // Extract detections from the ros message into std::vector<nvidia::isaac_ros::Detection2D>\n  std::vector<nvidia::isaac_ros::Detection2D> detection_info_vector;\n  for (size_t i = 0; i < num_bboxes; i++) {\n    nvidia::isaac_ros::Detection2D temp_detection_message;\n    vision_msgs::msg::Detection2D detection2_d = source.detections[i];\n    temp_detection_message.center_x = detection2_d.bbox.center.position.x;\n    temp_detection_message.center_y = detection2_d.bbox.center.position.y;\n    temp_detection_message.size_x = detection2_d.bbox.size_x;\n    temp_detection_message.size_y = detection2_d.bbox.size_y;\n    for (size_t j = 0; j < detection2_d.results.size(); j++) {\n      nvidia::isaac_ros::Hypothesis result;\n      result.class_id = detection2_d.results[j].hypothesis.class_id;\n      result.score = detection2_d.results[j].hypothesis.score;\n      temp_detection_message.results.push_back(result);\n    }\n    detection_info_vector.push_back(temp_detection_message);\n  }\n\n  // Finally populate the gxf message with detections and the timestamp\n  auto create_detection2_d_list_message_result =\n    nvidia::isaac_ros::CreateDetection2DList(context)\n    .map(\n    [&](nvidia::isaac_ros::Detection2DParts message_parts) -> nvidia::gxf::Expected<void> {\n      nvidia::gxf::Expected<void> result;\n      for (uint32_t i = 0; i < num_bboxes; i++) {\n        (message_parts.detection2_d_array)->push_back(detection_info_vector[i]);\n      }\n      message_parts.timestamp->acqtime = input_timestamp;\n      message = message_parts.message;\n      return result;\n    });\n  if (!create_detection2_d_list_message_result) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to create the detection2_d list message: \" <<\n      GxfResultStr(create_detection2_d_list_message_result.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDetection2DArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Set Entity Id\n  destination.handle = message->eid();\n  GxfEntityRefCountInc(context, message->eid());\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/test/isaac_ros_nitros_detection2_d_array_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosDetection2DArray type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\nfrom vision_msgs.msg import Detection2D, Detection2DArray, \\\n    ObjectHypothesis, ObjectHypothesisWithPose\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosDetection2DArrayTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_detection2_d_array_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosDetection2DArrayForwardNode',\n                name='NitrosDetection2DArrayForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_detection2_d_array'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosDetection2DArrayTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosDetection2DArrayTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosDetection2DArray type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_detection2_d_array')\n    def test_nitros_detection2_d_array_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the tag from NitrosDetection2DArray type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', Detection2DArray)],\n            received_messages=received_messages\n        )\n\n        nitros_detection2_d_array_pub = self.node.create_publisher(\n            Detection2DArray, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            print(test_folder)\n            nitros_detection2_d_array = self.load_nitros_detection2_d_array_from_json(\n                test_folder / 'nitros_detection2_d_array.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 10\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                nitros_detection2_d_array.header.stamp = timestamp\n\n                nitros_detection2_d_array_pub.publish(\n                    nitros_detection2_d_array)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_nitros_detection2_d_array = received_messages['output']\n\n            self.assertEqual(len(nitros_detection2_d_array.detections),\n                             len(received_nitros_detection2_d_array.detections))\n\n            if (len(nitros_detection2_d_array.detections) ==\n                    len(received_nitros_detection2_d_array.detections)):\n                for detection_count, detection2_d in \\\n                        enumerate(nitros_detection2_d_array.detections):\n                    received_detection2_d = received_nitros_detection2_d_array.detections[\n                        detection_count]\n                    self.assertEqual(len(detection2_d.results),\n                                     len(received_detection2_d.results))\n                    if (len(detection2_d.results) ==\n                            len(received_detection2_d.results)):\n                        for result_count, result in enumerate(detection2_d.results):\n                            received_result = received_detection2_d.results[result_count]\n                            self.assertEqual(result.hypothesis.class_id,\n                                             received_result.hypothesis.class_id)\n                            self.assertAlmostEqual(result.hypothesis.score,\n                                                   received_result.hypothesis.score, None,\n                                                   'hypothesis score is not accurate', 0.05)\n                    self.assertEqual(detection2_d.bbox.center.position.x,\n                                     received_detection2_d.bbox.center.position.x)\n                    self.assertEqual(detection2_d.bbox.center.position.y,\n                                     received_detection2_d.bbox.center.position.y)\n                    self.assertEqual(detection2_d.bbox.size_x,\n                                     received_detection2_d.bbox.size_x)\n                    self.assertEqual(detection2_d.bbox.size_y,\n                                     received_detection2_d.bbox.size_y)\n            print('The received detection 2D array has been verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(nitros_detection2_d_array_pub)\n\n    @staticmethod\n    def load_nitros_detection2_d_array_from_json(\n            json_filepath: pathlib.Path) -> Detection2DArray:\n        \"\"\"\n        Load a Detection2DArray message from a JSON filepath.\n\n        Parameters\n        ----------\n        json_filepath : Path\n            The path to a JSON file containing the Detection2DArray fields\n\n        Returns\n        -------\n        Detection2DArray\n            Generated Detection2DArray message\n\n        \"\"\"\n        detection2_d_array_json = JSONConversion.load_from_json(\n            json_filepath)\n\n        detection2_d_array = Detection2DArray()\n        detection2_d_array.header.frame_id = detection2_d_array_json[\n            'header']['frame_id']\n        for detection in detection2_d_array_json['detections']:\n            detection2_d = Detection2D()\n            for result in detection['results']:\n                object_hypothesis_with_pose = ObjectHypothesisWithPose()\n                object_hypothesis = ObjectHypothesis()\n                object_hypothesis.class_id = result['hypothesis']['class_id']\n                object_hypothesis.score = result['hypothesis']['score']\n                object_hypothesis_with_pose.hypothesis = object_hypothesis\n                detection2_d.results.append(object_hypothesis_with_pose)\n            detection2_d.bbox.center.position.x = detection['bbox']['center']['position']['x']\n            detection2_d.bbox.center.position.y = detection['bbox']['center']['position']['y']\n            detection2_d.bbox.size_x = detection['bbox']['size_x']\n            detection2_d.bbox.size_y = detection['bbox']['size_y']\n            detection2_d_array.detections.append(detection2_d)\n        return detection2_d_array\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/test/src/detection2_d_test_ext.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include <string>\n\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/extension_factory_helper.hpp\"\n#include \"detectnet/detection2_d.hpp\"\n\nextern \"C\" {\n\nGXF_EXT_FACTORY_BEGIN()\n\nGXF_EXT_FACTORY_SET_INFO(\n  0x94485739160245e4, 0x8ef19134f30ad931, \"Detection2DTestMessageExtension\",\n  \"Detection2D Message GXF extension\",\n  \"NVIDIA\", \"1.0.0\", \"LICENSE\");\n\nGXF_EXT_FACTORY_ADD_0(\n  0xa4c9101525594104, 0xaf12d9f22a134906,\n  std::vector<nvidia::isaac_ros::Detection2D>,\n  \"Array of decoded 2D object detections in an image\");\n\nGXF_EXT_FACTORY_END()\n\n}  // extern \"C\"\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/test/src/nitros_detection2_d_array_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_detection2_d_array_type/nitros_detection2_d_array.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_detection2_d_array_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_detection2_d_array\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosDetection2DArrayForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosDetection2DArrayForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(\n                1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"},\n          {\"isaac_ros_nitros_detection2_d_array_type\",\n            \"gxf/lib/detection2_d/test/libisaac_ros_nitros_detection2_d_array_test_ext.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosDetection2DArray>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosDetection2DArrayForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection2_d_array_type/test/test_cases/nitros_detection2_d_array/profile/nitros_detection2_d_array.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"tf_camera\"\n  },\n  \"detections\": [\n    {\n      \"results\": [\n        {\n          \"hypothesis\": {\n            \"class_id\": \"0\",\n            \"score\": 0.25\n          }\n        },\n        {\n          \"hypothesis\": {\n            \"class_id\": \"1\",\n            \"score\": 0.45\n          }\n        }\n      ],\n      \"bbox\": {\n        \"center\": {\n          \"position\": {\n            \"x\": 4.0,\n            \"y\": 5.0\n          }\n        },\n        \"size_x\": 6.0,\n        \"size_y\": 7.0\n      }\n    },\n    {\n      \"results\": [\n        {\n          \"hypothesis\": {\n            \"class_id\": \"2\",\n            \"score\": 0.35\n          },\n          \"hypothesis\": {\n            \"class_id\": \"3\",\n            \"score\": 0.65\n          }\n        }\n      ],\n      \"bbox\": {\n        \"center\": {\n          \"position\": {\n            \"x\": 7.0,\n            \"y\": 8.0\n          }\n        },\n        \"size_x\": 9.0,\n        \"size_y\": 10.0\n      }\n    }\n  ]\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection3_d_array_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_detection3_d_array_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(yaml-cpp)\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\n\n# NitrosDetection3DArray\nament_auto_add_library(${PROJECT_NAME} SHARED\n  src/nitros_detection3_d_array.cpp\n  src/detection3_d_array_message.cpp\n)\ntarget_include_directories(${PROJECT_NAME} PUBLIC include/detection3_d_array_message)\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\n# Make library available for sharing in Nitros\ninstall(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION share/${PROJECT_NAME}/gxf/lib/detection3_d)\nament_export_include_directories(include/detection3_d_array_message)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosDetection3DArrayForwardNode\n  ament_auto_add_library(isaac_ros_nitros_detection3_d_array_forward_node SHARED\n    test/src/nitros_detection3_d_array_forward_node.cpp\n  )\n  target_link_libraries(isaac_ros_nitros_detection3_d_array_forward_node ${PROJECT_NAME})\n  set_target_properties(isaac_ros_nitros_detection3_d_array_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(isaac_ros_nitros_detection3_d_array_forward_node\n      \"nvidia::isaac_ros::nitros::NitrosDetection3DArrayForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosDetection3DArrayForwardNode;\\\n      $<TARGET_FILE:isaac_ros_nitros_detection3_d_array_forward_node>\\n\")\n\n  # Test registration extension\n  ament_auto_add_library(isaac_ros_nitros_detection3_d_array_test_ext SHARED\n      test/src/detection3_d_test_ext.cpp\n  )\n  install(TARGETS isaac_ros_nitros_detection3_d_array_test_ext LIBRARY DESTINATION share/${PROJECT_NAME}/gxf/lib/detection3_d/test)\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n\n  ament_lint_auto_find_test_dependencies()\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_detection3_d_array_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection3_d_array_type/include/detection3_d_array_message/detection3_d_array_message.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#ifndef DETECTION3_D_ARRAY_MESSAGE__DETECTION3_D_ARRAY_MESSAGE_HPP_\n#define DETECTION3_D_ARRAY_MESSAGE__DETECTION3_D_ARRAY_MESSAGE_HPP_\n\n#include <string>\n#include <vector>\n\n#include \"gems/core/math/pose3.hpp\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac\n{\n\nstruct ObjectHypothesis\n{\n  std::vector<float> scores;\n  std::vector<std::string> class_ids;\n};\n\nstruct Detection3DListMessageParts\n{\n  gxf::Entity entity;\n  FixedVector<gxf::Handle<::nvidia::isaac::Pose3d>, kMaxComponents> poses;\n  FixedVector<gxf::Handle<::nvidia::isaac::Vector3f>, kMaxComponents> bbox_sizes;\n  FixedVector<gxf::Handle<ObjectHypothesis>, kMaxComponents> hypothesis;\n  gxf::Handle<gxf::Timestamp> timestamp;\n  size_t count;\n};\n\ngxf::Expected<Detection3DListMessageParts> CreateDetection3DListMessage(\n  gxf_context_t context, size_t detections);\ngxf::Expected<Detection3DListMessageParts> GetDetection3DListMessage(gxf::Entity entity);\n\n}  // namespace isaac\n}  // namespace nvidia\n\n#endif  // DETECTION3_D_ARRAY_MESSAGE__DETECTION3_D_ARRAY_MESSAGE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection3_d_array_type/include/isaac_ros_nitros_detection3_d_array_type/nitros_detection3_d_array.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_DETECTION3_D_ARRAY_TYPE__NITROS_DETECTION3_D_ARRAY_HPP_\n#define ISAAC_ROS_NITROS_DETECTION3_D_ARRAY_TYPE__NITROS_DETECTION3_D_ARRAY_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosDetection3DArray\n *   ROS type:    vision_msgs::msg::Detection3DArray\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"vision_msgs/msg/detection3_d_array.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosDetection3DArray;\n\n// Formats\nstruct nitros_detection3_d_array_t\n{\n  using MsgT = NitrosDetection3DArray;\n  static const inline std::string supported_type_name = \"nitros_detection3_d_array\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosDetection3DArray)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_detection3_d_array_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/std/libgxf_std.so\")\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_messages\", \"gxf/lib/libgxf_isaac_messages.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosDetection3DArray,\n  vision_msgs::msg::Detection3DArray>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosDetection3DArray;\n  using ros_message_type = vision_msgs::msg::Detection3DArray;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosDetection3DArray,\n  vision_msgs::msg::Detection3DArray);\n\n#endif  // ISAAC_ROS_NITROS_DETECTION3_D_ARRAY_TYPE__NITROS_DETECTION3_D_ARRAY_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection3_d_array_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2023-2025, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_detection3_d_array_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS Nitros Detection 3D Array Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Kajanan Chinniah</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>negotiated</depend>\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>vision_msgs</depend>\n  <depend>gxf_isaac_messages</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>gxf_isaac_gems</build_depend>\n  <build_depend>ament_index_cpp</build_depend>  \n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection3_d_array_type/src/detection3_d_array_message.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"detection3_d_array_message/detection3_d_array_message.hpp\"\n\n#include \"gems/gxf_helpers/expected_macro_gxf.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac\n{\n\nnamespace\n{\n\nconstexpr char kPoseName[] = \"pose\";\nconstexpr char kBBoxSizeName[] = \"bbox_size\";\nconstexpr char kObjectHypothesisName[] = \"object_hypothesis\";\nconstexpr char kTimestampName[] = \"timestamp\";\n\n}  // namespace\n\ngxf::Expected<Detection3DListMessageParts> CreateDetection3DListMessage(\n  gxf_context_t context, size_t detections)\n{\n  gxf::Entity entity;\n  return gxf::Entity::New(context)\n         .assign_to(entity)\n         .and_then(\n    [&]() {\n      gxf::Expected<void> result;\n      for (size_t i = 0; i < detections; ++i) {\n        result = result & entity.add<::nvidia::isaac::Pose3d>(kPoseName) &\n        entity.add<::nvidia::isaac::Vector3f>(kBBoxSizeName) &\n        entity.add<::nvidia::isaac::ObjectHypothesis>(kObjectHypothesisName);\n      }\n      result = result & entity.add<gxf::Timestamp>(kTimestampName);\n      return result;\n    })\n         .and_then([&]() {return GetDetection3DListMessage(entity);});\n}\n\ngxf::Expected<Detection3DListMessageParts> GetDetection3DListMessage(gxf::Entity entity)\n{\n  Detection3DListMessageParts parts;\n  parts.entity = entity;\n\n  parts.poses = UNWRAP_OR_RETURN(parts.entity.findAll<::nvidia::isaac::Pose3d>());\n  parts.bbox_sizes = UNWRAP_OR_RETURN(parts.entity.findAll<::nvidia::isaac::Vector3f>());\n  parts.hypothesis = UNWRAP_OR_RETURN(parts.entity.findAll<::nvidia::isaac::ObjectHypothesis>());\n  parts.timestamp = UNWRAP_OR_RETURN(parts.entity.get<gxf::Timestamp>(kTimestampName));\n\n  if (parts.poses.size() != parts.bbox_sizes.size() ||\n    parts.poses.size() != parts.hypothesis.size())\n  {\n    return gxf::Unexpected{GXF_FAILURE};\n  }\n  parts.count = parts.poses.size();\n  return parts;\n}\n\n}  // namespace isaac\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection3_d_array_type/src/nitros_detection3_d_array.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"detection3_d_array_message/detection3_d_array_message.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_detection3_d_array_type/nitros_detection3_d_array.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\nconstexpr char kEntityName[] = \"memory_pool\";\nconstexpr char kComponentName[] = \"unbounded_allocator\";\nconstexpr char kComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\n\nnamespace\n{\n\n::nvidia::isaac::Pose3d DetectionToPose3d(const float translation[3], const float rotation_wxyz[4])\n{\n  return ::nvidia::isaac::Pose3d{\n    ::nvidia::isaac::SO3d::FromQuaternion(\n      ::nvidia::isaac::Quaterniond{rotation_wxyz[0], rotation_wxyz[1], rotation_wxyz[2],\n        rotation_wxyz[3]}),\n    ::nvidia::isaac::Vector3d(translation[0], translation[1], translation[2])\n  };\n}\n}  // namespace\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosDetection3DArray,\n  vision_msgs::msg::Detection3DArray>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosDetection3DArray::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDetection3DArray\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto maybe_msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n  if (!maybe_msg_entity) {\n    throw std::runtime_error(\"Unable to get entity!\");\n  }\n\n  auto maybe_detection3d_list = nvidia::isaac::GetDetection3DListMessage(maybe_msg_entity.value());\n  if (!maybe_detection3d_list) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] failed to get detection3d data from message entity: \" <<\n      GxfResultStr(maybe_detection3d_list.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDetection3DArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto detection3_d_parts = maybe_detection3d_list.value();\n\n  // Set timestamp for ros message from gxf message\n  nvidia::gxf::Timestamp detection3_d_timestamp = *(detection3_d_parts.timestamp);\n  destination.header.stamp.sec = static_cast<int32_t>(\n    detection3_d_timestamp.acqtime / static_cast<uint64_t>(1e9));\n  destination.header.stamp.nanosec = static_cast<uint32_t>(\n    detection3_d_timestamp.acqtime % static_cast<uint64_t>(1e9));\n  destination.header.frame_id = source.frame_id;\n\n  size_t n_detections = detection3_d_parts.poses.size();\n\n  for (size_t i = 0; i < n_detections; ++i) {\n    vision_msgs::msg::Detection3D detection;\n    detection.header.frame_id = source.frame_id;\n    detection.header.stamp = destination.header.stamp;\n    vision_msgs::msg::BoundingBox3D bbox;\n\n    bbox.center.position.x = detection3_d_parts.poses.at(i).value()->translation.x();\n    bbox.center.position.y = detection3_d_parts.poses.at(i).value()->translation.y();\n    bbox.center.position.z = detection3_d_parts.poses.at(i).value()->translation.z();\n\n    bbox.center.orientation.x = detection3_d_parts.poses.at(i).value()->rotation.quaternion().x();\n    bbox.center.orientation.y = detection3_d_parts.poses.at(i).value()->rotation.quaternion().y();\n    bbox.center.orientation.z = detection3_d_parts.poses.at(i).value()->rotation.quaternion().z();\n    bbox.center.orientation.w = detection3_d_parts.poses.at(i).value()->rotation.quaternion().w();\n\n    bbox.size.x = detection3_d_parts.bbox_sizes.at(i).value()->x();\n    bbox.size.y = detection3_d_parts.bbox_sizes.at(i).value()->y();\n    bbox.size.z = detection3_d_parts.bbox_sizes.at(i).value()->z();\n    detection.bbox = bbox;\n\n    for (size_t j = 0; j < detection3_d_parts.hypothesis.at(i).value()->scores.size(); ++j) {\n      vision_msgs::msg::ObjectHypothesisWithPose object_hypothesis;\n      object_hypothesis.hypothesis.class_id =\n        detection3_d_parts.hypothesis.at(i).value()->class_ids[j];\n      object_hypothesis.hypothesis.score = detection3_d_parts.hypothesis.at(i).value()->scores[j];\n      object_hypothesis.pose.pose = bbox.center;\n      detection.results.push_back(object_hypothesis);\n    }\n    // If no hypothesis is found, populate results array with the pose of the bounding box\n    // This is requirement for vision_msgs_rviz_plugins\n    // If results is empty, vision_msgs_rviz_plugins will give a Segmentation fault\n    if (detection3_d_parts.hypothesis.at(i).value()->scores.size() == 0) {\n      vision_msgs::msg::ObjectHypothesisWithPose object_hypothesis;\n      object_hypothesis.pose.pose = bbox.center;\n      detection.results.push_back(object_hypothesis);\n    }\n\n    destination.detections.push_back(detection);\n  }\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDetection3DArray\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosDetection3DArray,\n  vision_msgs::msg::Detection3DArray>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosDetection3DArray::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDetection3DArray\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDetection3DArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  // Extract timestamp from ros message and convert to gxf timestamp format\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n\n  // count number of tags first to know how many times to run the below for loop\n  size_t num_detections = source.detections.size();\n\n  nvidia::gxf::Entity message;\n  auto detection3_d_result = nvidia::isaac::CreateDetection3DListMessage(context, num_detections)\n    .map(\n    [&](nvidia::isaac::Detection3DListMessageParts message_parts) {\n      for (size_t i = 0; i < num_detections; ++i) {\n        vision_msgs::msg::Detection3D detection = source.detections[i];\n        message_parts.bbox_sizes[i].value()->x() = detection.bbox.size.x;\n        message_parts.bbox_sizes[i].value()->y() = detection.bbox.size.y;\n        message_parts.bbox_sizes[i].value()->z() = detection.bbox.size.z;\n\n        const float translation[] = {\n          static_cast<float>(detection.bbox.center.position.x),\n          static_cast<float>(detection.bbox.center.position.y),\n          static_cast<float>(detection.bbox.center.position.z)\n        };\n\n        // Follow Eigen convention\n        const float rotation[] = {\n          static_cast<float>(detection.bbox.center.orientation.w),\n          static_cast<float>(detection.bbox.center.orientation.x),\n          static_cast<float>(detection.bbox.center.orientation.y),\n          static_cast<float>(detection.bbox.center.orientation.z)\n        };\n\n        *message_parts.poses[i].value() = DetectionToPose3d(\n          translation,\n          rotation);\n\n        for (const auto & object_hypothesis : detection.results) {\n          message_parts.hypothesis[i].value()->class_ids.push_back(\n            object_hypothesis.hypothesis.\n            class_id);\n          message_parts.hypothesis[i].value()->scores.push_back(\n            object_hypothesis.hypothesis.score\n          );\n        }\n      }\n      message_parts.timestamp->acqtime = input_timestamp;\n      message = message_parts.entity;\n    });\n\n  if (!detection3_d_result) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Failed to transfer message to custom: \" <<\n      GxfResultStr(detection3_d_result.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDetection3DArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Set frame ID\n  destination.frame_id = source.header.frame_id;\n\n  // Set Entity Id\n  destination.handle = message.eid();\n  GxfEntityRefCountInc(context, message.eid());\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDetection3DArray\"),\n    \"[convert_to_custom] Conversion completed\");\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection3_d_array_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection3_d_array_type/test/isaac_ros_nitros_detection3_d_array_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosDetection3DArray type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\nfrom vision_msgs.msg import Detection3D, Detection3DArray, \\\n    ObjectHypothesis, ObjectHypothesisWithPose\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosDetection3DArrayTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_detection3_d_array_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosDetection3DArrayForwardNode',\n                name='NitrosDetection3DArrayForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_detection3_d_array'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosDetection3DArrayTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosDetection3DArrayTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosDetection3DArray type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_detection3_d_array')\n    def test_nitros_detection3_d_array_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the tag from NitrosDetection3DArray type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', Detection3DArray)],\n            received_messages=received_messages\n        )\n\n        nitros_detection3_d_array_pub = self.node.create_publisher(\n            Detection3DArray, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            print(test_folder)\n            nitros_detection3_d_array = self.load_nitros_detection3_d_array_from_json(\n                test_folder / 'nitros_detection3_d_array.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 10\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                nitros_detection3_d_array.header.stamp = timestamp\n\n                nitros_detection3_d_array_pub.publish(\n                    nitros_detection3_d_array)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_nitros_detection3_d_array = received_messages['output']\n            # Header\n            # Frame ID is to be passed from NitrosPoseArray to ROS message\n            # self.assertEqual(nitros_detection3_d_array.header.frame_id,\n            #                 received_nitros_detection3_d_array.header.frame_id)\n\n            self.assertEqual(len(nitros_detection3_d_array.detections),\n                             len(received_nitros_detection3_d_array.detections))\n\n            if (len(nitros_detection3_d_array.detections) ==\n                    len(received_nitros_detection3_d_array.detections)):\n                for detection_count, detection3_d in \\\n                        enumerate(nitros_detection3_d_array.detections):\n                    received_detection3_d = received_nitros_detection3_d_array.detections[\n                        detection_count]\n                    self.assertEqual(len(detection3_d.results),\n                                     len(received_detection3_d.results))\n                    if (len(detection3_d.results) ==\n                            len(received_detection3_d.results)):\n                        for result_count, result in enumerate(detection3_d.results):\n                            received_result = received_detection3_d.results[result_count]\n                            self.assertEqual(result.hypothesis.class_id,\n                                             received_result.hypothesis.class_id)\n                            self.assertAlmostEqual(result.hypothesis.score,\n                                                   received_result.hypothesis.score, None,\n                                                   'hypothesis score is not accurate', 0.05)\n                    self.assertEqual(detection3_d.bbox.center.position.x,\n                                     received_detection3_d.bbox.center.position.x)\n                    self.assertEqual(detection3_d.bbox.center.position.y,\n                                     received_detection3_d.bbox.center.position.y)\n                    self.assertEqual(detection3_d.bbox.center.position.z,\n                                     received_detection3_d.bbox.center.position.z)\n                    self.assertEqual(detection3_d.bbox.center.orientation.x,\n                                     received_detection3_d.bbox.center.orientation.x)\n                    self.assertEqual(detection3_d.bbox.center.orientation.y,\n                                     received_detection3_d.bbox.center.orientation.y)\n                    self.assertEqual(detection3_d.bbox.center.orientation.z,\n                                     received_detection3_d.bbox.center.orientation.z)\n                    self.assertEqual(detection3_d.bbox.center.orientation.w,\n                                     received_detection3_d.bbox.center.orientation.w)\n                    self.assertEqual(detection3_d.bbox.size.x,\n                                     received_detection3_d.bbox.size.x)\n                    self.assertEqual(detection3_d.bbox.size.y,\n                                     received_detection3_d.bbox.size.y)\n                    self.assertEqual(detection3_d.bbox.size.z,\n                                     received_detection3_d.bbox.size.z)\n            print('The received detection 3D array has been verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(nitros_detection3_d_array_pub)\n\n    @staticmethod\n    def load_nitros_detection3_d_array_from_json(\n            json_filepath: pathlib.Path) -> Detection3DArray:\n        \"\"\"\n        Load a Detection3DArray message from a JSON filepath.\n\n        Parameters\n        ----------\n        json_filepath : Path\n            The path to a JSON file containing the Detection3DArray fields\n\n        Returns\n        -------\n        Detection3DArray\n            Generated Detection3DArray message\n\n        \"\"\"\n        detection3_d_array_json = JSONConversion.load_from_json(\n            json_filepath)\n\n        detection3_d_array = Detection3DArray()\n        detection3_d_array.header.frame_id = detection3_d_array_json[\n            'header']['frame_id']\n        for detection in detection3_d_array_json['detections']:\n            detection3_d = Detection3D()\n            for result in detection['results']:\n                object_hypothesis_with_pose = ObjectHypothesisWithPose()\n                object_hypothesis = ObjectHypothesis()\n                object_hypothesis.class_id = result['hypothesis']['class_id']\n                object_hypothesis.score = result['hypothesis']['score']\n                object_hypothesis_with_pose.hypothesis = object_hypothesis\n                detection3_d.results.append(object_hypothesis_with_pose)\n            detection3_d.bbox.center.position.x = detection['bbox']['center']['position']['x']\n            detection3_d.bbox.center.position.y = detection['bbox']['center']['position']['y']\n            detection3_d.bbox.center.position.z = detection['bbox']['center']['position']['z']\n\n            orientation_dict = detection['bbox']['center']['orientation']\n            detection3_d.bbox.center.orientation.x = orientation_dict['x']\n            detection3_d.bbox.center.orientation.y = orientation_dict['y']\n            detection3_d.bbox.center.orientation.z = orientation_dict['z']\n            detection3_d.bbox.center.orientation.w = orientation_dict['w']\n\n            detection3_d.bbox.size.x = detection['bbox']['size_x']\n            detection3_d.bbox.size.y = detection['bbox']['size_y']\n            detection3_d.bbox.size.z = detection['bbox']['size_z']\n\n            detection3_d_array.detections.append(detection3_d)\n        return detection3_d_array\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection3_d_array_type/test/src/detection3_d_test_ext.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include <string>\n\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/extension_factory_helper.hpp\"\n#include \"detection3_d_array_message/detection3_d_array_message.hpp\"\n\nextern \"C\" {\n\nGXF_EXT_FACTORY_BEGIN()\n\nGXF_EXT_FACTORY_SET_INFO(\n  0x5560bbc051a511ee, 0x9be8f7b68805842c, \"Detection3DTestMessageExtension\",\n  \"Detection3D Message GXF extension\",\n  \"NVIDIA\", \"1.0.0\", \"LICENSE\");\n\nGXF_EXT_FACTORY_ADD_0(\n  0x65d4476051a511ee, 0xb3f727d2ed955144,\n  nvidia::isaac::ObjectHypothesis,\n  \"List of scores and class ids\");\n\nGXF_EXT_FACTORY_ADD_0(\n  0x782823c851dc11ee, 0xa5dc87b46496e7b8,\n  nvidia::isaac::Vector3f,\n  \"3 Dimensional Vector\");\n\n\nGXF_EXT_FACTORY_END()\n\n}  // extern \"C\"\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection3_d_array_type/test/src/nitros_detection3_d_array_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_detection3_d_array_type/nitros_detection3_d_array.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_detection3_d_array_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_detection3_d_array\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosDetection3DArrayForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosDetection3DArrayForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(\n                1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"},\n          {\"isaac_ros_nitros_detection3_d_array_type\",\n            \"gxf/lib/detection3_d/test/libisaac_ros_nitros_detection3_d_array_test_ext.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosDetection3DArray>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosDetection3DArrayForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_detection3_d_array_type/test/test_cases/nitros_detection3_d_array/profile/nitros_detection3_d_array.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"tf_camera\"\n  },\n  \"detections\": [\n    {\n      \"results\": [\n        {\n          \"hypothesis\": {\n            \"class_id\": \"0\",\n            \"score\": 0.25\n          }\n        },\n        {\n          \"hypothesis\": {\n            \"class_id\": \"1\",\n            \"score\": 0.45\n          }\n        }\n      ],\n      \"bbox\": {\n        \"center\": {\n          \"position\": {\n            \"x\": 4.0,\n            \"y\": 5.0,\n            \"z\": 6.0\n          },\n          \"orientation\": {\n            \"x\": 0.20056211618247968,\n            \"y\": 0.5319756777932487,\n            \"z\": 0.022260030365134766,\n            \"w\": 0.8223631842665151\n          }\n        },\n        \"size_x\": 11.0,\n        \"size_y\": 12.0,\n        \"size_z\": 13.0\n      }\n    },\n    {\n      \"results\": [\n        {\n          \"hypothesis\": {\n            \"class_id\": \"2\",\n            \"score\": 0.35\n          }\n        },\n        {\n          \"hypothesis\": {\n            \"class_id\": \"3\",\n            \"score\": 0.65\n          }\n        }\n      ],\n      \"bbox\": {\n        \"center\": {\n          \"position\": {\n            \"x\": 14.0,\n            \"y\": 15.0,\n            \"z\": 16.0\n          },\n          \"orientation\": {\n            \"x\": -0.015134439760121994,\n            \"y\": 0.08583164935454375,\n            \"z\": 0.17298738608664135,\n            \"w\": 0.9810602636724909\n          }\n        },\n        \"size_x\": 21.0,\n        \"size_y\": 22.0,\n        \"size_z\": 23.0\n      }\n    }\n  ]\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_disparity_image_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_disparity_image_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(yaml-cpp)\n\n# NitrosDisparityImage\nament_auto_add_library(${PROJECT_NAME} SHARED\nsrc/nitros_disparity_image.cpp\nsrc/nitros_disparity_image_builder.cpp\n)\ntarget_link_libraries(${PROJECT_NAME}\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosDisparityImageForwardNode\n  ament_auto_add_library(nitros_disparity_image_forward_node SHARED test/src/nitros_disparity_image_forward_node.cpp)\n  target_link_libraries(nitros_disparity_image_forward_node ${PROJECT_NAME})\n  set_target_properties(nitros_disparity_image_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(nitros_disparity_image_forward_node \"nvidia::isaac_ros::nitros::NitrosDisparityImageForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosDisparityImageForwardNode;\\\n    $<TARGET_FILE:nitros_disparity_image_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_disparity_image_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_disparity_image_type/include/isaac_ros_nitros_disparity_image_type/nitros_disparity_image.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_DISPARITY_IMAGE_TYPE__NITROS_DISPARITY_IMAGE_HPP_\n#define ISAAC_ROS_NITROS_DISPARITY_IMAGE_TYPE__NITROS_DISPARITY_IMAGE_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosDisparityImage\n *   ROS type:    stereo_msgs::msg::DisparityImage\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"stereo_msgs/msg/disparity_image.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosDisparityImage;\n\n// Formats\nstruct nitros_disparity_image_32FC1_t\n{\n  using MsgT = NitrosDisparityImage;\n  static const inline std::string supported_type_name = \"nitros_disparity_image_32FC1\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosDisparityImage)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_disparity_image_32FC1_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/multimedia/libgxf_multimedia.so\")\n// for nvidia::gxf::ComponentSerializer\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\n// for nvidia::isaac::StringProvider\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_gxf_helpers\", \"gxf/lib/libgxf_isaac_gxf_helpers.so\")\n// for nvidia::isaac::SightReceiver\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_sight\", \"gxf/lib/libgxf_isaac_sight.so\")\n// for nvidia::isaac::PoseTree\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_atlas\", \"gxf/lib/libgxf_isaac_atlas.so\")\n// for nvidia::isaac::PoseFrameUid\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_messages\", \"gxf/lib/libgxf_isaac_messages.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<nvidia::isaac_ros::nitros::NitrosDisparityImage,\n  stereo_msgs::msg::DisparityImage>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosDisparityImage;\n  using ros_message_type = stereo_msgs::msg::DisparityImage;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosDisparityImage,\n  stereo_msgs::msg::DisparityImage);\n\n#endif  // ISAAC_ROS_NITROS_DISPARITY_IMAGE_TYPE__NITROS_DISPARITY_IMAGE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_disparity_image_type/include/isaac_ros_nitros_disparity_image_type/nitros_disparity_image_builder.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_DISPARITY_IMAGE_TYPE__NITROS_DISPARITY_IMAGE_BUILDER_HPP_\n#define ISAAC_ROS_NITROS_DISPARITY_IMAGE_TYPE__NITROS_DISPARITY_IMAGE_BUILDER_HPP_\n\n#include <functional>\n#include <string>\n#include <cstdint>\n\n#include \"isaac_ros_nitros_disparity_image_type/nitros_disparity_image.hpp\"\n#include \"std_msgs/msg/header.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nclass NitrosDisparityImageBuilder\n{\npublic:\n  NitrosDisparityImageBuilder();\n  NitrosDisparityImageBuilder(const NitrosDisparityImageBuilder &) = delete;\n  NitrosDisparityImageBuilder & operator=(const NitrosDisparityImageBuilder &) = delete;\n  NitrosDisparityImageBuilder(NitrosDisparityImageBuilder && other);\n  NitrosDisparityImageBuilder & operator=(NitrosDisparityImageBuilder && other);\n\n  NitrosDisparityImageBuilder & WithHeader(std_msgs::msg::Header header);\n  NitrosDisparityImageBuilder & WithDimensions(uint32_t height, uint32_t width);\n  NitrosDisparityImageBuilder & WithGpuData(void * data);\n  NitrosDisparityImageBuilder & WithReleaseCallback(std::function<void()> release_callback);\n  NitrosDisparityImageBuilder & WithDisparityParameters(\n    float f, float t, float min_disparity, float max_disparity);\n\n  NitrosDisparityImage Build();\n\nprivate:\n  void Validate();\n\n  NitrosDisparityImage nitros_disparity_image_;\n  uint32_t height_{0};\n  uint32_t width_{0};\n  void * data_{nullptr};\n  std::function<void()> release_callback_{nullptr};\n  float f_{0.0f};\n  float t_{0.0f};\n  float min_disparity_{0.0f};\n  float max_disparity_{0.0f};\n  std_msgs::msg::Header header_;\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_DISPARITY_IMAGE_TYPE__NITROS_DISPARITY_IMAGE_BUILDER_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_disparity_image_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022-2025, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_disparity_image_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS Nitros Disparity Image Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n  <author>Yuankun Zhu</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>stereo_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>gxf_isaac_messages</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>ament_index_cpp</build_depend>\n\n  <exec_depend>gxf_isaac_atlas</exec_depend>\n  <exec_depend>gxf_isaac_gxf_helpers</exec_depend>\n  <exec_depend>gxf_isaac_sight</exec_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_disparity_image_type/src/nitros_disparity_image.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <cuda_runtime.h>\n\n#include <string>\n#include <unordered_map>\n#include <vector>\n#include <typeinfo>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/multimedia/video.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#include \"extensions/messages/camera_message.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_disparity_image_type/nitros_disparity_image.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n\nnamespace\n{\nconst char kEntityName[] = \"memory_pool\";\nconst char kComponentName[] = \"unbounded_allocator\";\nconst char kComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\n\nusing VideoFormat = nvidia::gxf::VideoFormat;\n// Map to store the ROS format encoding to Nitros format encoding\nconst std::unordered_map<std::string, VideoFormat> g_ros_to_gxf_video_format({\n    {\"32FC1\", VideoFormat::GXF_VIDEO_FORMAT_D32F}\n  });\n\n// Map to store the Nitros format encoding to ROS format encoding\nconst std::unordered_map<VideoFormat, std::string> g_gxf_to_ros_video_format({\n    {VideoFormat::GXF_VIDEO_FORMAT_D32F, \"32FC1\"},\n    {VideoFormat::GXF_VIDEO_FORMAT_GRAY32, \"32FC1\"}\n  });\n\n// Get step size for ROS Image\nuint32_t GetStepSize(const nvidia::gxf::VideoBufferInfo & video_buff_info)\n{\n  return video_buff_info.width * video_buff_info.color_planes[0].bytes_per_pixel;\n}\n\n// Allocate buffer for Nitros Image\nnvidia::gxf::Expected<nvidia::isaac::CameraMessageParts> CreateCameraMessage(\n  const stereo_msgs::msg::DisparityImage & source,\n  const nvidia::gxf::Handle<nvidia::gxf::Allocator> & allocator_handle,\n  gxf_context_t context)\n{\n  auto color_fmt = g_ros_to_gxf_video_format.find(source.image.encoding);\n  if (color_fmt == std::end(g_ros_to_gxf_video_format)) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Unsupported encoding from ROS: \" <<\n      source.image.encoding.c_str();\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosDisparityImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  constexpr auto surface_layout = nvidia::gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR;\n  constexpr auto storage_type = nvidia::gxf::MemoryStorageType::kDevice;\n  switch (color_fmt->second) {\n    case VideoFormat::GXF_VIDEO_FORMAT_D32F:\n      return nvidia::isaac::CreateCameraMessage<VideoFormat::GXF_VIDEO_FORMAT_D32F>(\n        context, static_cast<uint32_t>(source.image.width),\n        static_cast<uint32_t>(source.image.height), surface_layout,\n        storage_type, allocator_handle, false);\n      break;\n    default:\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosDisparityImage\"),\n        \"[convert_to_custom] Unsupported encoding from ROS [%s].\", source.image.encoding.c_str());\n      throw std::runtime_error(\"[convert_to_custom] Unsupported encoding from ROS.\");\n      break;\n  }\n}\n}  // namespace\n\n\nvoid rclcpp::TypeAdapter<nvidia::isaac_ros::nitros::NitrosDisparityImage,\n  stereo_msgs::msg::DisparityImage>::convert_to_ros_message(\n  const custom_type & source,\n  ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosDisparityImage::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDisparityImage\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto maybe_disparity_image = nvidia::isaac::GetCameraMessage(msg_entity.value());\n  if (!maybe_disparity_image) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get disparity image from message entity: \" <<\n      GxfResultStr(maybe_disparity_image.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDisparityImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto disparity_image = maybe_disparity_image.value();\n\n  // Setting Image data from gxf VideoBuffer\n  auto video_buffer_info = disparity_image.frame->video_frame_info();\n  destination.image.height = video_buffer_info.height;\n  destination.image.width = video_buffer_info.width;\n  const auto encoding = g_gxf_to_ros_video_format.find(video_buffer_info.color_format);\n  if (encoding == std::end(g_gxf_to_ros_video_format)) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Unsupported encoding from GXF: \" <<\n      static_cast<int>(video_buffer_info.color_format);\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosDisparityImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  } else {\n    destination.image.encoding = encoding->second;\n  }\n\n  // Bigendian or not\n  destination.image.is_bigendian = 0;\n\n  // Full row length in bytes\n  destination.image.step = GetStepSize(video_buffer_info);\n\n  // Resize the ROS image buffer to the right size\n  destination.image.data.resize(destination.image.step * destination.image.height);\n\n  // Copy data from Device to Host\n  const cudaError_t cuda_error = cudaMemcpy2D(\n    destination.image.data.data(),\n    destination.image.step,\n    disparity_image.frame->pointer(),\n    video_buffer_info.color_planes[0].stride,\n    destination.image.step,\n    destination.image.height,\n    cudaMemcpyDeviceToHost);\n\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] cudaMemcpy2D failed for conversion from \"\n      \"NitrosDisparityImage to stereo_msgs::msg::Image: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDisparityImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // extract float parameters if available\n  auto gxf_disparity_parameters = disparity_image.entity.findAll<float>();\n  if (!gxf_disparity_parameters) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] failed to get all floats: \" <<\n      GxfResultStr(gxf_disparity_parameters.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDisparityImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  for (const auto & parameter_handle : gxf_disparity_parameters.value()) {\n    auto parameter = parameter_handle.value();\n    if (!std::strcmp(parameter.name(), \"f\")) {\n      destination.f = *parameter.get();\n    } else if (!std::strcmp(parameter.name(), \"t\")) {\n      destination.t = *parameter.get();\n    } else if (!std::strcmp(parameter.name(), \"min_disparity\")) {\n      destination.min_disparity = *parameter.get();\n    } else if (!std::strcmp(parameter.name(), \"max_disparity\")) {\n      destination.max_disparity = *parameter.get();\n    } else {\n      continue;\n    }\n  }\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDisparityImage\"),\n    \"[convert_to_ros_message] Enter the timestamp section\");\n  destination.header.stamp.sec = static_cast<int32_t>(\n    disparity_image.timestamp->acqtime / static_cast<uint64_t>(1e9));\n  destination.header.stamp.nanosec = static_cast<uint32_t>(\n    disparity_image.timestamp->acqtime % static_cast<uint64_t>(1e9));\n  destination.image.header = destination.header;\n\n  // Set frame ID\n  destination.header.frame_id = source.frame_id;\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDisparityImage\"),\n    \"[convert_to_ros_message] Conversion completed for handle=%ld\", source.handle);\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\n\nvoid rclcpp::TypeAdapter<nvidia::isaac_ros::nitros::NitrosDisparityImage,\n  stereo_msgs::msg::DisparityImage>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosDisparityImage::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDisparityImage\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosDisparityImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  if (source.image.width % 2 != 0 || source.image.height % 2 != 0) {\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDisparityImage\"),\n      \"[convert_to_custom] Image width/height must be even for creation of gxf::VideoBuffer\");\n    throw std::runtime_error(\"[convert_to_custom] Odd Image width or height.\");\n  }\n\n  auto maybe_camera_message = CreateCameraMessage(source, allocator_handle, context);\n  if (!maybe_camera_message) {\n    std::string error_msg =\n      \"[convert_to_custom] Failed to create CameraMessage object\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDisparityImage\"), error_msg.c_str());\n    throw std::runtime_error(error_msg.c_str());\n  }\n\n  auto disparity_image = maybe_camera_message.value();\n  auto video_buffer_info = disparity_image.frame->video_frame_info();\n\n  // Copy data from Host to Device\n  const cudaError_t cuda_error = cudaMemcpy2D(\n    disparity_image.frame->pointer(),\n    video_buffer_info.color_planes[0].stride,\n    source.image.data.data(),\n    source.image.step,\n    video_buffer_info.width * video_buffer_info.color_planes[0].bytes_per_pixel,\n    video_buffer_info.height,\n    cudaMemcpyHostToDevice);\n\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] cudaMemcpy2D failed for conversion from \"\n      \"stereo_msgs::msg::DisparityImage to NitrosDisparityImage: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDisparityImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Passthrough baseline, focal length,\n  // min_disparity and max_disparity as an additional component to the entity\n  // passthrough both as extrinsics and standalone float entities\n  disparity_image.intrinsics->focal_length.x = source.f;\n  disparity_image.extrinsics->translation[0] = source.t;\n  *(disparity_image.entity.add<float>(\"t\")->get()) = source.t;\n  *(disparity_image.entity.add<float>(\"f\")->get()) = source.f;\n  *(disparity_image.entity.add<float>(\"min_disparity\")->get()) = source.min_disparity;\n  *(disparity_image.entity.add<float>(\"max_disparity\")->get()) = source.max_disparity;\n\n  // Add timestamp to the disparity_image\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  disparity_image.timestamp->acqtime = input_timestamp;\n\n  // Set frame ID\n  destination.frame_id = source.header.frame_id;\n\n  // Set Entity Id\n  destination.handle = disparity_image.entity.eid();\n  GxfEntityRefCountInc(context, disparity_image.entity.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDisparityImage\"),\n    \"[convert_to_custom] Conversion completed (resulting handle=%ld)\",\n    disparity_image.entity.eid());\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_disparity_image_type/src/nitros_disparity_image_builder.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <cuda_runtime.h>\n\n#include <string>\n#include <vector>\n#include <array>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/multimedia/video.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#include \"extensions/messages/camera_message.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_disparity_image_type/nitros_disparity_image_builder.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\nnamespace\n{\nconstexpr uint64_t kNanosecondsInSeconds = 1e9;\n\n// NoPaddingColorPlanes specialization for D32F format\ntemplate<nvidia::gxf::VideoFormat T>\nstruct NoPaddingColorPlanes {};\n\ntemplate<>\nstruct NoPaddingColorPlanes<nvidia::gxf::VideoFormat::GXF_VIDEO_FORMAT_D32F>\n{\n  explicit NoPaddingColorPlanes(uint32_t width)\n  : planes({nvidia::gxf::ColorPlane(\"D\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\nnvidia::gxf::Expected<void> ReleaseImageCallback(\n  std::function<void()> release_callback,\n  void * ptr)\n{\n  if (release_callback) {\n    release_callback();\n  } else {\n    cudaFree(ptr);\n  }\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDisparityImageBuilder\"),\n    \"[ReleaseImageCallback] Released the cuda memory [%p]\", ptr);\n  return nvidia::gxf::Success;\n}\n\nnvidia::gxf::Expected<nvidia::isaac::CameraMessageParts> CreateDisparityImage(\n  const uint32_t width, const uint32_t height,\n  gxf_context_t context,\n  void * data,\n  std::function<void()> release_callback)\n{\n  if (width % 2 != 0 || height % 2 != 0) {\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDisparityImageBuilder\"),\n      \"[CreateDisparityImage] Image width/height must be even for creation of gxf::VideoBuffer\");\n    throw std::runtime_error(\"[CreateDisparityImage] Odd Image width or height.\");\n  }\n\n  // Create proper CameraMessage structure\n  auto maybe_camera_message = nvidia::isaac::InitializeCameraMessage(context);\n  if (!maybe_camera_message) {\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDisparityImageBuilder\"),\n      \"[CreateDisparityImage] Failed to initialize CameraMessage\");\n    return nvidia::gxf::ForwardError(maybe_camera_message);\n  }\n\n  auto camera_message = maybe_camera_message.value();\n\n  // Setup VideoBuffer with the provided data\n  NoPaddingColorPlanes<nvidia::gxf::VideoFormat::GXF_VIDEO_FORMAT_D32F> nopadding_planes(width);\n  nvidia::gxf::VideoFormatSize<nvidia::gxf::VideoFormat::GXF_VIDEO_FORMAT_D32F> format_size;\n  uint64_t size = format_size.size(width, height, nopadding_planes.planes);\n\n  std::vector<nvidia::gxf::ColorPlane> color_planes{nopadding_planes.planes.begin(),\n    nopadding_planes.planes.end()};\n\n  constexpr auto surface_layout = nvidia::gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR;\n  constexpr auto storage_type = nvidia::gxf::MemoryStorageType::kDevice;\n\n  nvidia::gxf::VideoBufferInfo buffer_info{\n    width, height,\n    nvidia::gxf::VideoFormat::GXF_VIDEO_FORMAT_D32F,\n    color_planes,\n    surface_layout\n  };\n\n  camera_message.frame->wrapMemory(\n    buffer_info, size, storage_type, data,\n    std::bind(&ReleaseImageCallback, release_callback, std::placeholders::_1));\n\n  return camera_message;\n}\n}  // namespace\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nNitrosDisparityImageBuilder::NitrosDisparityImageBuilder()\n: nitros_disparity_image_{}\n{\n  // Entity will be created during Build()\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDisparityImageBuilder\"),\n    \"[constructor] NitrosDisparityImageBuilder initialized\");\n}\n\nNitrosDisparityImageBuilder::NitrosDisparityImageBuilder(NitrosDisparityImageBuilder && other)\n{\n  nitros_disparity_image_ = other.nitros_disparity_image_;\n  height_ = other.height_;\n  width_ = other.width_;\n  data_ = other.data_;\n  release_callback_ = other.release_callback_;\n  f_ = other.f_;\n  t_ = other.t_;\n  min_disparity_ = other.min_disparity_;\n  max_disparity_ = other.max_disparity_;\n  header_ = other.header_;\n\n  // Reset other\n  other.height_ = 0;\n  other.width_ = 0;\n  other.data_ = nullptr;\n  other.release_callback_ = nullptr;\n  other.f_ = 0.0f;\n  other.t_ = 0.0f;\n  other.min_disparity_ = 0.0f;\n  other.max_disparity_ = 0.0f;\n  other.header_ = std_msgs::msg::Header{};\n}\n\nNitrosDisparityImageBuilder &\nNitrosDisparityImageBuilder::operator=(NitrosDisparityImageBuilder && other)\n{\n  // In case other is this, then nothing should be done.\n  if (&other == this) {\n    return *this;\n  }\n  nitros_disparity_image_ = other.nitros_disparity_image_;\n  height_ = other.height_;\n  width_ = other.width_;\n  data_ = other.data_;\n  release_callback_ = other.release_callback_;\n  f_ = other.f_;\n  t_ = other.t_;\n  min_disparity_ = other.min_disparity_;\n  max_disparity_ = other.max_disparity_;\n  header_ = other.header_;\n\n  // Reset other\n  other.height_ = 0;\n  other.width_ = 0;\n  other.data_ = nullptr;\n  other.release_callback_ = nullptr;\n  other.f_ = 0.0f;\n  other.t_ = 0.0f;\n  other.min_disparity_ = 0.0f;\n  other.max_disparity_ = 0.0f;\n  other.header_ = std_msgs::msg::Header{};\n\n  return *this;\n}\n\nNitrosDisparityImageBuilder & NitrosDisparityImageBuilder::WithHeader(std_msgs::msg::Header header)\n{\n  // Store header information to be applied during Build()\n  header_ = header;\n  nitros_disparity_image_.frame_id = header.frame_id;\n\n  return *this;\n}\n\nNitrosDisparityImageBuilder &\nNitrosDisparityImageBuilder::WithDimensions(uint32_t height, uint32_t width)\n{\n  height_ = height;\n  width_ = width;\n  return *this;\n}\n\nNitrosDisparityImageBuilder & NitrosDisparityImageBuilder::WithGpuData(void * data)\n{\n  data_ = data;\n  return *this;\n}\n\nNitrosDisparityImageBuilder & NitrosDisparityImageBuilder::WithReleaseCallback(\n  std::function<void()> release_callback)\n{\n  release_callback_ = release_callback;\n  return *this;\n}\n\nNitrosDisparityImageBuilder &\nNitrosDisparityImageBuilder::WithDisparityParameters(\n  float f, float t, float min_disparity, float max_disparity)\n{\n  f_ = f;\n  t_ = t;\n  min_disparity_ = min_disparity;\n  max_disparity_ = max_disparity;\n  return *this;\n}\n\nvoid NitrosDisparityImageBuilder::Validate()\n{\n  bool failure = false;\n  std::stringstream error_msg;\n  if (height_ == 0 || width_ == 0) {\n    error_msg << \"Dimensions are not set! Call WithDimension method before Build. \\n\";\n    failure = true;\n  }\n  if (data_ == nullptr) {\n    error_msg << \"Data buffer is not set! Call WithGpuData method before Build. \\n\";\n    failure = true;\n  }\n\n  if (failure) {\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosDisparityImageBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n}\n\nNitrosDisparityImage NitrosDisparityImageBuilder::Build()\n{\n  // Validate all data is present before building the NitrosDisparityImage\n  Validate();\n\n  auto context = GetTypeAdapterNitrosContext().getContext();\n\n  // Create the CameraMessage structure with proper components\n  auto maybe_camera_message = CreateDisparityImage(\n    width_, height_, context, data_, release_callback_);\n\n  if (!maybe_camera_message) {\n    std::stringstream error_msg;\n    error_msg << \"[Build] Failed to create CameraMessage: \" <<\n      GxfResultStr(maybe_camera_message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosDisparityImageBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  auto camera_message = maybe_camera_message.value();\n\n  // Set disparity parameters in intrinsics and extrinsics for compatibility with disparity_to_depth\n  camera_message.intrinsics->focal_length.x = f_;\n  camera_message.extrinsics->translation[0] = t_;\n\n  // Add disparity parameters as float components for backward compatibility\n  *(camera_message.entity.add<float>(\"f\")->get()) = f_;\n  *(camera_message.entity.add<float>(\"t\")->get()) = t_;\n  *(camera_message.entity.add<float>(\"min_disparity\")->get()) = min_disparity_;\n  *(camera_message.entity.add<float>(\"max_disparity\")->get()) = max_disparity_;\n\n  // Set timestamp from header if provided\n  if (header_.stamp.sec != 0 || header_.stamp.nanosec != 0) {\n    uint64_t input_timestamp = header_.stamp.sec * kNanosecondsInSeconds + header_.stamp.nanosec;\n    camera_message.timestamp->acqtime = input_timestamp;\n  }\n\n  // Set the handle and frame_id\n  nitros_disparity_image_.handle = camera_message.entity.eid();\n  GxfEntityRefCountInc(context, camera_message.entity.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosDisparityImageBuilder\"),\n    \"[Build] Disparity image built\");\n\n  // Resetting data after it is done building\n  data_ = nullptr;\n  return nitros_disparity_image_;\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_disparity_image_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_disparity_image_type/test/isaac_ros_nitros_disparity_image_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosDisparityImage type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nimport cv2\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\nimport numpy as np\n\nimport pytest\nimport rclpy\n\nfrom stereo_msgs.msg import DisparityImage\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSDisparityImageTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='image_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_disparity_image_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosDisparityImageForwardNode',\n                name='NitrosDisparityImageForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_disparity_image_32FC1'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'error'],\n    )\n\n    return IsaacROSDisparityImageTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSDisparityImageTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosDisparityImage type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_disparity_image')\n    def test_nitros_image_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the image received from NitrosImage type conversion to be identical to source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_image_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', DisparityImage)],\n            received_messages=received_messages\n        )\n\n        image_pub = self.node.create_publisher(\n            DisparityImage, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            disparity_image = DisparityImage()\n            image_json = JSONConversion.load_from_json(test_folder / 'image.json')\n            # Load the main image data from a JSON-specified image file\n            disparity_img_cv2 = cv2.imread(\n                str(test_folder / image_json['image']), cv2.IMREAD_GRAYSCALE)\n            disparity_img_cv2 = disparity_img_cv2.astype(np.float32)\n            disparity_image.image = self.bridge.cv2_to_imgmsg(disparity_img_cv2,\n                                                              encoding=image_json['encoding'])\n            disparity_image.image.encoding = image_json['encoding']\n            disparity_image.f = 1.0\n            disparity_image.t = 2.0\n            disparity_image.min_disparity = 0.0\n            disparity_image.max_disparity = 64.0\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 10\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                disparity_image.header.stamp = timestamp\n\n                image_pub.publish(disparity_image)\n                rclpy.spin_once(self.node, timeout_sec=2)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on output_image topic!\")\n\n            received_image = received_messages['output']\n\n            print(f'Source image data size: {len(disparity_image.image.data)}')\n            print(f'Received image data size: {len(received_image.image.data)}')\n\n            self.assertEqual(\n                len(disparity_image.image.data), len(received_image.image.data),\n                'Source and received image sizes do not match: ' +\n                f'{len(disparity_image.image.data)} != {len(received_image.image.data)}')\n\n            self.assertEqual(disparity_image.f, received_image.f,\n                             'Source and received f value do not match: ' +\n                             f'{disparity_image.f} != {received_image.f}')\n            self.assertEqual(disparity_image.t, received_image.t,\n                             'Source and received t value do not match: ' +\n                             f'{disparity_image.t} != {received_image.t}')\n\n            self.assertEqual(disparity_image.min_disparity, received_image.min_disparity,\n                             'Source and received min_disparity do not match: ' +\n                             f'{disparity_image.min_disparity} != {received_image.min_disparity}')\n            self.assertEqual(disparity_image.max_disparity, received_image.max_disparity,\n                             'Source and received max_disparity do not match: ' +\n                             f'{disparity_image.max_disparity} != {received_image.max_disparity}')\n\n            # Make sure that the source and received images are the same\n            self.assertEqual(received_image.image.height, disparity_image.image.height,\n                             'Source and received image heights do not match: ' +\n                             f'{received_image.image.height} != {disparity_image.image.height}')\n            self.assertEqual(received_image.image.width, disparity_image.image.width,\n                             'Source and received image widths do not match: ' +\n                             f'{received_image.image.width} != {disparity_image.image.width}')\n\n            for i in range(len(disparity_image.image.data)):\n                self.assertEqual(disparity_image.image.data[i], received_image.image.data[i],\n                                 'Source and received image pixels do not match')\n            print('Source and received images are identical.')\n        finally:\n            self.node.destroy_subscription(received_image_sub)\n            self.node.destroy_publisher(image_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_disparity_image_type/test/src/nitros_disparity_image_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_disparity_image_type/nitros_disparity_image.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_disparity_image_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_disparity_image_32FC1\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosDisparityImageForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosDisparityImageForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosDisparityImage>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosDisparityImageForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_disparity_image_type/test/test_cases/nitros_disparity_image/profile/image.json",
    "content": "{\n    \"image\": \"NVIDIAprofile.png\",\n    \"encoding\": \"32FC1\"\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_encoder_ticks_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_encoder_ticks_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(yaml-cpp)\n\n# NitrosEncoderTicks\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_encoder_ticks.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosEncoderTicksForwardNode\n  ament_auto_add_library(isaac_ros_nitros_encoder_ticks_forward_node SHARED test/src/nitros_encoder_ticks_forward_node.cpp)\n  target_link_libraries(isaac_ros_nitros_encoder_ticks_forward_node ${PROJECT_NAME})\n  set_target_properties(isaac_ros_nitros_encoder_ticks_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(isaac_ros_nitros_encoder_ticks_forward_node\n    \"nvidia::isaac_ros::nitros::NitrosEncoderTicksForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosEncoderTicksForwardNode;\\\n    $<TARGET_FILE:isaac_ros_nitros_encoder_ticks_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_encoder_ticks_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_encoder_ticks_type/include/isaac_ros_nitros_encoder_ticks_type/nitros_encoder_ticks.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_ENCODER_TICKS_TYPE__NITROS_ENCODER_TICKS_HPP_\n#define ISAAC_ROS_NITROS_ENCODER_TICKS_TYPE__NITROS_ENCODER_TICKS_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosEncoderTicks\n *   ROS type:    isaac_ros_nova_interfaces::msg::EncoderTicks\n */\n\n#include <string>\n\n#include \"isaac_ros_nova_interfaces/msg/encoder_ticks.hpp\"\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosEncoderTicks;\n\n// Formats\nstruct nitros_encoder_ticks_t\n{\n  using MsgT = NitrosEncoderTicks;\n  static const inline std::string supported_type_name = \"nitros_encoder_ticks\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosEncoderTicks)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_encoder_ticks_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_messages\", \"gxf/lib/libgxf_isaac_messages.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosEncoderTicks,\n  isaac_ros_nova_interfaces::msg::EncoderTicks>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosEncoderTicks;\n  using ros_message_type = isaac_ros_nova_interfaces::msg::EncoderTicks;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosEncoderTicks,\n  isaac_ros_nova_interfaces::msg::EncoderTicks);\n\n#endif  // ISAAC_ROS_NITROS_ENCODER_TICKS_TYPE__NITROS_ENCODER_TICKS_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_encoder_ticks_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_encoder_ticks_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS NITROS Encoder Ticks Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>sensor_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>isaac_ros_nova_interfaces</depend>\n  <depend>gxf_isaac_messages</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_encoder_ticks_type/src/nitros_encoder_ticks.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"extensions/messages/encoder_tick_message.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_encoder_ticks_type/nitros_encoder_ticks.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosEncoderTicks,\n  isaac_ros_nova_interfaces::msg::EncoderTicks>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosEncoderTicks::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosEncoderTicks\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto maybe_encoder_ticks_parts = nvidia::isaac::GetEncoderTickMessage(\n    msg_entity.value());\n  if (!maybe_encoder_ticks_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get battery state gxf message\"\n      \" from message entity: \" <<\n      GxfResultStr(maybe_encoder_ticks_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosEncoderTicks\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto encoder_ticks_parts = maybe_encoder_ticks_parts.value();\n\n  destination.left_ticks = *encoder_ticks_parts.left_ticks.get();\n  destination.right_ticks = *encoder_ticks_parts.right_ticks.get();\n  destination.encoder_timestamp = *encoder_ticks_parts.encoder_timestamp.get();\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = encoder_ticks_parts.timestamp;\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Set frame ID\n  destination.header.frame_id = source.frame_id;\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosEncoderTicks\"),\n    \"[convert_to_ros_message] frame_id = %s\", destination.header.frame_id.c_str());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosEncoderTicks\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosEncoderTicks,\n  isaac_ros_nova_interfaces::msg::EncoderTicks>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosEncoderTicks::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosEncoderTicks\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // create encoder_ticks gxf msg\n  auto maybe_encoder_ticks_parts = nvidia::isaac::CreateEncoderTickMessage(context);\n  if (!maybe_encoder_ticks_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to create encoder ticks gxf msg \" << GxfResultStr(\n      maybe_encoder_ticks_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosEncoderTicks\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto encoder_ticks_parts = maybe_encoder_ticks_parts.value();\n\n  *encoder_ticks_parts.left_ticks.get() = source.left_ticks;\n  *encoder_ticks_parts.right_ticks.get() = source.right_ticks;\n  *encoder_ticks_parts.encoder_timestamp.get() = source.encoder_timestamp;\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  encoder_ticks_parts.timestamp->acqtime = input_timestamp;\n\n  // Set frame ID\n  destination.frame_id = source.header.frame_id;\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosEncoderTicks\"),\n    \"[convert_to_custom] frame_id = %s\", destination.frame_id.c_str());\n\n  // Set Entity Id\n  destination.handle = encoder_ticks_parts.message.eid();\n  GxfEntityRefCountInc(context, encoder_ticks_parts.message.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosEncoderTicks\"),\n    \"[convert_to_custom] Conversion completed (handle=%ld)\", destination.handle);\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_encoder_ticks_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_encoder_ticks_type/test/isaac_ros_nitros_encoder_ticks_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosEncoderTicks type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_nova_interfaces.msg import EncoderTicks\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosEncoderTicksTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_encoder_ticks_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosEncoderTicksForwardNode',\n                name='NitrosEncoderTicksForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_encoder_ticks'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'NitrosEncoderTicks:=debug'],\n    )\n\n    return IsaacROSNitrosEncoderTicksTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosEncoderTicksTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosEncoderTicks type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_encoder_ticks')\n    def test_nitros_encoder_ticks_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the message from NitrosEncoderTicks type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', EncoderTicks)],\n            received_messages=received_messages\n        )\n\n        encoder_ticks_pub = self.node.create_publisher(\n            EncoderTicks, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            encoder_ticks = self.load_encoder_ticks_from_json(test_folder / 'encoder_ticks.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 10\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                encoder_ticks.header.stamp = timestamp\n\n                encoder_ticks_pub.publish(encoder_ticks)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_encoder_ticks = received_messages['output']\n\n            # Only test for data passed through gxf\n            # Header\n            self.assertEqual(encoder_ticks.header.stamp.sec,\n                             received_encoder_ticks.header.stamp.sec)\n            self.assertEqual(encoder_ticks.header.stamp.nanosec,\n                             received_encoder_ticks.header.stamp.nanosec)\n            self.assertEqual(encoder_ticks.header.frame_id,\n                             received_encoder_ticks.header.frame_id)\n            # Values\n            self.assertEqual(encoder_ticks.left_ticks,\n                             received_encoder_ticks.left_ticks)\n            self.assertEqual(encoder_ticks.right_ticks,\n                             received_encoder_ticks.right_ticks)\n            self.assertEqual(encoder_ticks.encoder_timestamp,\n                             received_encoder_ticks.encoder_timestamp)\n            print('The received encoder_ticks message has been verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(encoder_ticks_pub)\n\n    @staticmethod\n    def load_encoder_ticks_from_json(\n            json_filepath: pathlib.Path) -> EncoderTicks:\n        \"\"\"\n        Load a EncoderTicks message from a JSON filepath.\n\n        Parameters\n        ----------\n        json_filepath : Path\n            The path to a JSON file containing the EncoderTicks fields\n\n        Returns\n        -------\n        EncoderTicks\n            Generated EncoderTicks message\n\n        \"\"\"\n        encoder_ticks_json = JSONConversion.load_from_json(\n            json_filepath)\n\n        encoder_ticks = EncoderTicks()\n        encoder_ticks.header.frame_id = encoder_ticks_json[\n            'header']['frame_id']\n        encoder_ticks.header.stamp.sec = encoder_ticks_json[\n            'header']['stamp']['sec']\n        encoder_ticks.header.stamp.nanosec = encoder_ticks_json[\n            'header']['stamp']['nanosec']\n        encoder_ticks.left_ticks = encoder_ticks_json['left_ticks']\n        encoder_ticks.right_ticks = encoder_ticks_json['right_ticks']\n        encoder_ticks.encoder_timestamp = encoder_ticks_json['encoder_timestamp']\n        return encoder_ticks\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_encoder_ticks_type/test/src/nitros_encoder_ticks_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_encoder_ticks_type/nitros_encoder_ticks.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_encoder_ticks_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_encoder_ticks\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosEncoderTicksForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosEncoderTicksForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n              .frame_id_source_key = \"forward/input\",\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosEncoderTicks>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosEncoderTicksForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_encoder_ticks_type/test/test_cases/nitros_encoder_ticks/profile/encoder_ticks.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"encoder_ticks_header\",\n    \"stamp\": {\n      \"sec\": 22125,\n      \"nanosec\": 45172\n    }\n  },\n  \"left_ticks\": 10,\n  \"right_ticks\": 20,\n  \"encoder_timestamp\": 100\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_flat_scan_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_flat_scan_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(yaml-cpp)\n\n# NitrosFlatScan\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_flat_scan.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosFlatScanForwardNode\n  ament_auto_add_library(\n    isaac_ros_nitros_flat_scan_forward_node SHARED test/src/nitros_flat_scan_forward_node.cpp)\n  target_link_libraries(isaac_ros_nitros_flat_scan_forward_node ${PROJECT_NAME})\n  set_target_properties(isaac_ros_nitros_flat_scan_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(isaac_ros_nitros_flat_scan_forward_node \"nvidia::isaac_ros::nitros::NitrosFlatScanForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosFlatScanForwardNode;\\\n      $<TARGET_FILE:isaac_ros_nitros_flat_scan_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_flat_scan_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_flat_scan_type/include/isaac_ros_nitros_flat_scan_type/nitros_flat_scan.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_FLAT_SCAN_TYPE__NITROS_FLAT_SCAN_HPP_\n#define ISAAC_ROS_NITROS_FLAT_SCAN_TYPE__NITROS_FLAT_SCAN_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosFlatScan\n *   ROS type:    isaac_ros_pointcloud_interfaces::msg::FlatScan\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"isaac_ros_pointcloud_interfaces/msg/flat_scan.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosFlatScan;\n\n// Formats\nstruct nitros_flat_scan_t\n{\n  using MsgT = NitrosFlatScan;\n  static const inline std::string supported_type_name = \"nitros_flat_scan\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosFlatScan)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_flat_scan_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\n// for nvidia::isaac::StringProvider\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_gxf_helpers\", \"gxf/lib/libgxf_isaac_gxf_helpers.so\")\n// for nvidia::isaac::SightReceiver\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_sight\", \"gxf/lib/libgxf_isaac_sight.so\")\n// for nvidia::isaac::PoseTree\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_atlas\", \"gxf/lib/libgxf_isaac_atlas.so\")\n// for nvidia::isaac::PoseFrameUid\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_messages\", \"gxf/lib/libgxf_isaac_messages.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosFlatScan,\n  isaac_ros_pointcloud_interfaces::msg::FlatScan>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosFlatScan;\n  using ros_message_type = isaac_ros_pointcloud_interfaces::msg::FlatScan;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosFlatScan,\n  isaac_ros_pointcloud_interfaces::msg::FlatScan);\n\n#endif  // ISAAC_ROS_NITROS_FLAT_SCAN_TYPE__NITROS_FLAT_SCAN_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_flat_scan_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2023-2024, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_flat_scan_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS NITROS Flat Scan Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Ashwin Varghese Kuruttukulam</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>isaac_ros_pointcloud_interfaces</depend>\n  <depend>gxf_isaac_messages</depend>\n  <depend>gxf_isaac_ros_messages</depend>\n  <depend>gxf_isaac_gxf_helpers</depend>\n  <depend>gxf_isaac_atlas</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>gxf_isaac_gems</build_depend>\n\n  <exec_depend>gxf_isaac_sight</exec_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_flat_scan_type/src/nitros_flat_scan.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include <cuda_runtime.h>\n\n#include <cstdint>\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"messages/flat_scan_message.hpp\"\n#include \"gems/pose_tree/pose_tree.hpp\"\n#include \"extensions/atlas/pose_tree_frame.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n#include \"isaac_ros_nitros_flat_scan_type/nitros_flat_scan.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nnamespace\n{\nconstexpr char kMemoryEntityName[] = \"memory_pool\";\nconstexpr char kMemoryComponentName[] = \"unbounded_allocator\";\nconstexpr char kMemoryComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\nconstexpr char kPoseTreeEntityName[] = \"global_pose_tree\";\nconstexpr char kPoseTreeComponentName[] = \"pose_tree\";\nconstexpr char kPoseTreeComponentTypeName[] = \"nvidia::isaac::PoseTree\";\nconstexpr char kNameBeamsDevice[] = \"beams\";\nconstexpr int kFlatscanAngleIndx = 0;\nconstexpr int kFlatscanRangeIndx = 2;\nconstexpr int kNFieldsFlatscanMsg = 5;\n}  // namespace\n\ntemplate<typename Deleter>\nusing unique_p = std::unique_ptr<double[], Deleter>;\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosFlatScan,\n  isaac_ros_pointcloud_interfaces::msg::FlatScan>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosFlatScan::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosFlatScan\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto maybe_flatscan_parts = nvidia::isaac_ros::messages::GetFlatscanMessage(\n    msg_entity.value());\n  if (!maybe_flatscan_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get flatscan message from message entity: \" <<\n      GxfResultStr(maybe_flatscan_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto flatscan_parts = maybe_flatscan_parts.value();\n\n  // Extract GXF Tensor\n  auto beams_tensor = flatscan_parts.beams;\n  const auto & beams_tensor_shape = beams_tensor->shape();\n\n  // Tensor checks\n  if (beams_tensor->rank() != 2) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Flatscan tensor expected to be of rank 2, but got rank \" <<\n      beams_tensor->rank();\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  if (beams_tensor_shape.dimension(1) != kNFieldsFlatscanMsg) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Flatscan tensor shape dimension(1) expected to be\" <<\n      kNFieldsFlatscanMsg <<\n      \"but got\" << beams_tensor_shape.dimension(1);\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Tensor is assumed to be of shape (num_points,kNFieldsFlatscanMsg)\n  size_t num_points = beams_tensor_shape.dimension(0);\n\n  // allocate CPU memory in case tensor for use\n  // in case tensor is GPU based\n  // using char to represnt 1 byte pointer\n  auto deleter = [](double * ptr) {cudaFreeHost(ptr);};\n  auto pointerCudaMalloc = [](size_t mySize) {\n      void * ptr; cudaMallocHost(reinterpret_cast<void **>(&ptr), mySize); return ptr;\n    };\n  unique_p<decltype(deleter)> beams_cpu_pointer(\n    reinterpret_cast<double *>(pointerCudaMalloc(beams_tensor->size())), deleter);\n\n  ::nvidia::isaac::CpuTensorView2d beams_tensor_view;\n  switch (beams_tensor->storage_type()) {\n    case nvidia::gxf::MemoryStorageType::kHost:\n      {\n        // CPU based tensor\n        beams_tensor_view = ::nvidia::isaac::CreateCpuTensorViewFromData<double,\n            2>(\n          beams_tensor->data<double>().value(), beams_tensor_shape.size(),\n          ::nvidia::isaac::Vector2i(\n            beams_tensor_shape.dimension(0), beams_tensor_shape.dimension(1)));\n      }\n      break;\n    case nvidia::gxf::MemoryStorageType::kDevice:\n      {\n        // GPU based tensor\n        // Copy beams tensor off device to CPU memory\n        const cudaError_t cuda_error = cudaMemcpyAsync(\n          beams_cpu_pointer.get(), beams_tensor->pointer(),\n          beams_tensor->size(), cudaMemcpyDeviceToHost, source.cuda_stream);\n        if (cuda_error != cudaSuccess) {\n          std::stringstream error_msg;\n          error_msg <<\n            \"[convert_to_ros_message] cudaMemcpyAsync failed for conversion from \"\n            \"gxf::Tensor to ROS FlatScan: \" <<\n            cudaGetErrorName(cuda_error) <<\n            \" (\" << cudaGetErrorString(cuda_error) << \")\";\n          RCLCPP_ERROR(\n            rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.str().c_str());\n          throw std::runtime_error(error_msg.str().c_str());\n        }\n        cudaError_t cuda_result = cudaStreamSynchronize(source.cuda_stream);\n        CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n        beams_tensor_view =\n          ::nvidia::isaac::CreateCpuTensorViewFromData<double, 2>(\n          beams_cpu_pointer.get(), beams_tensor_shape.size(),\n          ::nvidia::isaac::Vector2i(\n            beams_tensor_shape.dimension(0), beams_tensor_shape.dimension(1)));\n      }\n      break;\n    default:\n      std::string error_msg =\n        \"[convert_to_ros_message] MemoryStorageType not supported: conversion from \"\n        \"gxf::Tensor to ROS FlatScan!\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.c_str());\n      throw std::runtime_error(error_msg.c_str());\n  }\n\n  // iterate through each point and convert float64 beam range and intensity to float32\n  // This is required since the data is stored as a double in the isaac message but as a float32\n  // in the ros message\n  for (size_t i = 0; i < num_points; i += 1) {\n    // index 0 corresponds to angle refer flatscan_types.hpp\n    destination.ranges.push_back(static_cast<float>(beams_tensor_view(i, kFlatscanRangeIndx)));\n    // index 2 corresponds to range refer flatscan_types.hpp\n    destination.angles.push_back(static_cast<float>(beams_tensor_view(i, kFlatscanAngleIndx)));\n  }\n\n  destination.range_max = static_cast<float>(flatscan_parts.info->out_of_range);\n  destination.range_min = 0.0;  // range_min not present in gxf flatscan message\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = flatscan_parts.timestamp;\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Get pointer to posetree component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kPoseTreeEntityName, kPoseTreeComponentName, kPoseTreeComponentTypeName, cid);\n  auto maybe_pose_tree_handle =\n    nvidia::gxf::Handle<nvidia::isaac::PoseTree>::Create(context, cid);\n  if (!maybe_pose_tree_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get pose tree's handle: \" <<\n      GxfResultStr(maybe_pose_tree_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto pose_tree_handle = maybe_pose_tree_handle.value();\n  auto frame_name = pose_tree_handle->getFrameName(flatscan_parts.pose_frame_uid->uid);\n  if (frame_name) {\n    destination.header.frame_id = frame_name.value();\n  } else {\n    RCLCPP_WARN(\n      rclcpp::get_logger(\"NitrosFlatScan\"), \"Setting frame if from NITROS msg\");\n    // Set NITROS frame id as fallback method of populating frame_id\n    // Set frame ID\n    destination.header.frame_id = source.frame_id;\n  }\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosFlatScan\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosFlatScan,\n  isaac_ros_pointcloud_interfaces::msg::FlatScan>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosFlatScan::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosFlatScan\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kMemoryEntityName, kMemoryComponentName, kMemoryComponentTypeName, cid);\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  if (source.ranges.size() != source.angles.size()) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Ranges and Angles array do not have the same size\" <<\n      \"Ranges array size \" << source.ranges.size() <<\n      \"Angles array size \" << source.angles.size();\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  const int n_points = source.ranges.size();\n\n  auto maybe_flatscan_parts = nvidia::isaac_ros::messages::CreateFlatscanMessage(\n    context, allocator_handle, n_points, false);\n  if (!maybe_flatscan_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to create CreatePointCloudMessage \" << GxfResultStr(\n      maybe_flatscan_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto flatscan_parts = maybe_flatscan_parts.value();\n\n  auto beams_tensor = flatscan_parts.beams;\n\n  // allocate CPU memory in case tensor for use\n  // in case tensor is GPU based\n  // using char to represnt 1 byte pointer\n  size_t beams_size_bytes = n_points * sizeof(double) * kNFieldsFlatscanMsg;\n  auto deleter = [](double * ptr) {cudaFreeHost(ptr);};\n  unique_p<decltype(deleter)> beams_cpu_pointer(new double[n_points], deleter);\n  cudaMallocHost(reinterpret_cast<void **>(&beams_cpu_pointer), beams_size_bytes);\n  ::nvidia::isaac::CpuTensorView2d beams_tensor_view =\n    ::nvidia::isaac::CreateCpuTensorViewFromData<double, 2>(\n    beams_cpu_pointer.get(), beams_size_bytes,\n    ::nvidia::isaac::Vector2i(n_points, kNFieldsFlatscanMsg));\n  for (int point_index = 0; point_index < n_points; point_index++) {\n    beams_tensor_view(point_index, kFlatscanAngleIndx) = source.angles[point_index];\n    beams_tensor_view(point_index, kFlatscanRangeIndx) = source.ranges[point_index];\n  }\n  auto nitros_cuda_stream =\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext()\n    .getCudaStreamFromNitrosGraph();\n  switch (beams_tensor->storage_type()) {\n    case nvidia::gxf::MemoryStorageType::kHost:\n      {\n        // CPU based tensor\n        // Copy data from CPU to CPU backed gxf tensor.\n        const cudaError_t cuda_error = cudaMemcpyAsync(\n          beams_tensor->data<double>().value(),\n          beams_cpu_pointer.get(),\n          beams_size_bytes, cudaMemcpyHostToHost, nitros_cuda_stream);\n        if (cuda_error != cudaSuccess) {\n          std::stringstream error_msg;\n          error_msg <<\n            \"[convert_to_custom] cudaMemcpyAsync failed for copying data from \"\n            \"CPU to CPU: \" <<\n            cudaGetErrorName(cuda_error) <<\n            \" (\" << cudaGetErrorString(cuda_error) << \")\";\n          cudaError_t cuda_result = cudaStreamSynchronize(nitros_cuda_stream);\n          CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n          RCLCPP_ERROR(\n            rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.str().c_str());\n          throw std::runtime_error(error_msg.str().c_str());\n        }\n      }\n      break;\n    case nvidia::gxf::MemoryStorageType::kDevice:\n      {\n        // GPU based tensor\n        // Copy data from CPU to GPU backed gxf tensor.\n        const cudaError_t cuda_error = cudaMemcpyAsync(\n          beams_tensor->data<double>().value(),\n          beams_cpu_pointer.get(),\n          beams_size_bytes, cudaMemcpyHostToDevice, nitros_cuda_stream);\n        if (cuda_error != cudaSuccess) {\n          std::stringstream error_msg;\n          error_msg <<\n            \"[convert_to_custom] cudaMemcpyAsync failed for copying data from \"\n            \"CPU to GPU: \" <<\n            cudaGetErrorName(cuda_error) <<\n            \" (\" << cudaGetErrorString(cuda_error) << \")\";\n          RCLCPP_ERROR(\n            rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.str().c_str());\n          throw std::runtime_error(error_msg.str().c_str());\n        }\n      }\n      break;\n    default:\n      std::string error_msg =\n        \"[convert_to_ros_message] MemoryStorageType not supported: conversion from \"\n        \"ROS FlatScan to gxf::Tensor failed!\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.c_str());\n      throw std::runtime_error(error_msg.c_str());\n  }\n\n  flatscan_parts.info->out_of_range = source.range_max;\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  flatscan_parts.timestamp->acqtime = input_timestamp;\n\n  // Get pointer to posetree component\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kPoseTreeEntityName, kPoseTreeComponentName, kPoseTreeComponentTypeName, cid);\n  auto maybe_pose_tree_handle =\n    nvidia::gxf::Handle<nvidia::isaac::PoseTree>::Create(context, cid);\n  if (!maybe_pose_tree_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get pose tree's handle: \" <<\n      GxfResultStr(maybe_pose_tree_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosFlatScan\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto pose_tree_handle = maybe_pose_tree_handle.value();\n  auto maybe_flat_scan_frame_uid = pose_tree_handle->findOrCreateFrame(\n    source.header.frame_id.c_str());\n  if (maybe_flat_scan_frame_uid) {\n    flatscan_parts.pose_frame_uid->uid = maybe_flat_scan_frame_uid.value();\n  } else {\n    RCLCPP_WARN(\n      rclcpp::get_logger(\"NitrosFlatScan\"), \"Could not create Pose Tree Frame\");\n  }\n\n  // Set NITROS frame id as fallback method of populating frame_id\n  destination.frame_id = source.header.frame_id;\n\n  // Set Entity Id\n  destination.handle = flatscan_parts.entity.eid();\n  GxfEntityRefCountInc(context, flatscan_parts.entity.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosFlatScan\"),\n    \"[convert_to_custom] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_flat_scan_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_flat_scan_type/test/isaac_ros_nitros_flat_scan_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosFlatScan type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_pointcloud_interfaces.msg import FlatScan\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\n\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosFlatScanTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_flat_scan_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosFlatScanForwardNode',\n                name='NitrosFlatScanForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_flat_scan'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosFlatScanTest.generate_test_description(\n        [container],\n        node_startup_delay=2.5\n    )\n\n\nclass IsaacROSNitrosFlatScanTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosFlatScan type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case()\n    def test_nitros_flat_scan_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the flatscan from NitrosFlatScan type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', FlatScan)],\n            received_messages=received_messages\n        )\n\n        flat_scan_pub = self.node.create_publisher(\n            FlatScan, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            flat_scan = self.load_flat_scan_from_json(\n                test_folder / 'flat_scan.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 2\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                flat_scan.header.stamp = timestamp\n\n                flat_scan_pub.publish(flat_scan)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_flat_scan = received_messages['output']\n\n            # Header\n            # Note: Since intensities and min range is not present in the\n            # gxf message, passthrough of these values are not tested\n            # Frame ID is to be passed from NitrosFlatScan to ROS message\n            self.assertEqual(flat_scan.header.frame_id,\n                             received_flat_scan.header.frame_id)\n            # Max range to be passed through\n            self.assertEqual(flat_scan.range_max,\n                             received_flat_scan.range_max)\n\n            # Array size check\n            self.assertEqual(len(flat_scan.ranges), len(\n                received_flat_scan.ranges), 'Number of ranges does not match')\n            self.assertEqual(len(flat_scan.angles), len(\n                received_flat_scan.angles), 'Number of angles does not match')\n\n            # Array values check\n            for beam_range, received_range in zip(flat_scan.ranges, received_flat_scan.ranges):\n                self.assertEqual(\n                    beam_range, received_range,\n                    'Range array does not match')\n            for angle, received_angle in zip(flat_scan.angles, received_flat_scan.angles):\n                self.assertEqual(\n                    angle, received_angle,\n                    'Angle array does not match')\n\n            print('The received flatscan is verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(flat_scan_pub)\n\n    @staticmethod\n    def load_flat_scan_from_json(\n            json_filepath: pathlib.Path) -> FlatScan:\n        \"\"\"\n        Load a FlatScan message from a JSON filepath.\n\n        Parameters\n        ----------\n        json_filepath : Path\n            The path to a JSON file containing the FlatScan fields\n\n        Returns\n        -------\n        FlatScan\n            Generated FlatScan message\n\n        \"\"\"\n        flatscan_json = JSONConversion.load_from_json(\n            json_filepath)\n\n        flat_scan_msg = FlatScan()\n        flat_scan_msg.header.frame_id = flatscan_json[\n            'header']['frame_id']\n        flat_scan_msg.header.stamp.sec = flatscan_json[\n            'header']['stamp']['sec']\n        flat_scan_msg.header.stamp.nanosec = flatscan_json[\n            'header']['stamp']['nanosec']\n        flat_scan_msg.range_max = flatscan_json[\n            'range_max']\n        flat_scan_msg.range_min = flatscan_json[\n            'range_min']\n        for beam_range in flatscan_json['ranges']:\n            flat_scan_msg.ranges.append(beam_range)\n        for angle in flatscan_json['angles']:\n            flat_scan_msg.angles.append(angle)\n        return flat_scan_msg\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_flat_scan_type/test/src/nitros_flat_scan_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_flat_scan_type/nitros_flat_scan.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_flat_scan_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_flat_scan\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosFlatScanForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosFlatScanForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosFlatScan>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosFlatScanForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_flat_scan_type/test/test_cases/profile/flat_scan.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"flat_scan_header\",\n    \"stamp\": {\n      \"sec\": 20221,\n      \"nanosec\": 458915\n    }\n  },\n  \"range_min\": 0.01,\n  \"range_max\": 20.0,\n  \"ranges\": [\n    1.0,\n    2.0,\n    3.0,\n    4.0,\n    5.0,\n    6.0,\n    7.0,\n    8.0,\n    9.0,\n    10.0\n  ],\n  \"angles\": [\n    0.05,\n    0.1,\n    0.15,\n    0.2,\n    0.25,\n    0.3,\n    0.35,\n    0.4,\n    0.45,\n    0.5\n  ]\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_image_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(vpi REQUIRED)\nfind_package(yaml-cpp)\n\n# NitrosImage\nament_auto_add_library(${PROJECT_NAME} SHARED\n  src/nitros_image.cpp\n  src/nitros_image_builder.cpp\n  src/nitros_image_view.cpp\n)\ntarget_link_libraries(${PROJECT_NAME}\n  vpi\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosImageForwardNode\n  ament_auto_add_library(nitros_image_forward_node SHARED test/src/nitros_image_forward_node.cpp)\n  target_link_libraries(nitros_image_forward_node ${PROJECT_NAME})\n  set_target_properties(nitros_image_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(nitros_image_forward_node \"nvidia::isaac_ros::nitros::NitrosImageForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosImageForwardNode;$<TARGET_FILE:nitros_image_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_image_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/include/isaac_ros_nitros_image_type/nitros_image.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_HPP_\n#define ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosImage\n *   ROS type:    sensor_msgs::msg::Image\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros_image_type/nitros_image_details.hpp\"\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"sensor_msgs/msg/image.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosImage;\n\n// Formats\nstruct nitros_image_rgb8_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_rgb8\";\n};\n\nstruct nitros_image_rgba8_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_rgba8\";\n};\n\nstruct nitros_image_rgb16_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_rgb16\";\n};\n\nstruct nitros_image_bgr8_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_bgr8\";\n};\n\nstruct nitros_image_bgra8_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_bgra8\";\n};\n\nstruct nitros_image_bgr16_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_bgr16\";\n};\n\nstruct nitros_image_mono8_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_mono8\";\n};\n\nstruct nitros_image_mono16_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_mono16\";\n};\n\nstruct nitros_image_nv12_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_nv12\";\n};\n\nstruct nitros_image_nv24_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_nv24\";\n};\n\nstruct nitros_image_32FC1_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_32FC1\";\n};\n\nstruct nitros_image_32FC3_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_32FC3\";\n};\n\nstruct nitros_image_32FC4_t\n{\n  using MsgT = NitrosImage;\n  static const inline std::string supported_type_name = \"nitros_image_32FC4\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosImage)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_image_rgb8_t)\nNITROS_FORMAT_ADD(nitros_image_rgba8_t)\nNITROS_FORMAT_ADD(nitros_image_rgb16_t)\nNITROS_FORMAT_ADD(nitros_image_bgr8_t)\nNITROS_FORMAT_ADD(nitros_image_bgra8_t)\nNITROS_FORMAT_ADD(nitros_image_bgr16_t)\nNITROS_FORMAT_ADD(nitros_image_mono8_t)\nNITROS_FORMAT_ADD(nitros_image_mono16_t)\nNITROS_FORMAT_ADD(nitros_image_nv12_t)\nNITROS_FORMAT_ADD(nitros_image_nv24_t)\nNITROS_FORMAT_ADD(nitros_image_32FC1_t)\nNITROS_FORMAT_ADD(nitros_image_32FC3_t)\nNITROS_FORMAT_ADD(nitros_image_32FC4_t)\nNITROS_FORMAT_FACTORY_END()\n// Default compatible data format\nNITROS_DEFAULT_COMPATIBLE_FORMAT(nitros_image_rgb8_t)\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/multimedia/libgxf_multimedia.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<nvidia::isaac_ros::nitros::NitrosImage, sensor_msgs::msg::Image>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosImage;\n  using ros_message_type = sensor_msgs::msg::Image;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosImage,\n  sensor_msgs::msg::Image);\n\nuint64_t calculate_image_size(const std::string image_type, uint32_t width, uint32_t height);\n#endif  // ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/include/isaac_ros_nitros_image_type/nitros_image_builder.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_BUILDER_HPP_\n#define ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_BUILDER_HPP_\n\n#include <string>\n\n#include \"isaac_ros_nitros_image_type/nitros_image.hpp\"\n\n#include \"std_msgs/msg/header.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nclass NitrosImageBuilder\n{\npublic:\n  NitrosImageBuilder();\n  NitrosImageBuilder(const NitrosImageBuilder &) = delete;\n  NitrosImageBuilder & operator=(const NitrosImageBuilder &) = delete;\n  NitrosImageBuilder(NitrosImageBuilder && other);\n  NitrosImageBuilder & operator=(NitrosImageBuilder && other);\n\n\n  NitrosImageBuilder & WithHeader(std_msgs::msg::Header header);\n\n  // This is of type listed in image_encodings.hpp\n  NitrosImageBuilder & WithEncoding(std::string encoding);\n\n  NitrosImageBuilder & WithDimensions(uint32_t height, uint32_t width);\n\n  // This has to be GPU buffer\n  NitrosImageBuilder & WithGpuData(void * data);\n\n  NitrosImageBuilder & WithReleaseCallback(std::function<void()> release_callback);\n\n  NitrosImage Build();\n\nprivate:\n  void Validate();\n  NitrosImage nitros_image_{};\n  std::string encoding_{\"\"};\n  uint32_t height_{0};\n  uint32_t width_{0};\n  void * data_{nullptr};\n  cudaEvent_t event_{};\n  std::function<void()> release_callback_{nullptr};\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_BUILDER_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/include/isaac_ros_nitros_image_type/nitros_image_details.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_DETAILS_HPP_\n#define ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_DETAILS_HPP_\n\n#include <string>\n#include <unordered_map>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/multimedia/video.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"sensor_msgs/image_encodings.hpp\"\n#include \"vpi/Image.h\"\n\nnamespace\n{\nusing VideoFormat = nvidia::gxf::VideoFormat;\nnamespace img_encodings = sensor_msgs::image_encodings;\n// Map to store the ROS format encoding to Nitros format encoding\nconst std::unordered_map<std::string, VideoFormat> g_ros_to_gxf_video_format({\n    {img_encodings::RGB8, VideoFormat::GXF_VIDEO_FORMAT_RGB},\n    {img_encodings::RGBA8, VideoFormat::GXF_VIDEO_FORMAT_RGBA},\n    {img_encodings::RGB16, VideoFormat::GXF_VIDEO_FORMAT_RGB16},\n    {img_encodings::BGR8, VideoFormat::GXF_VIDEO_FORMAT_BGR},\n    {img_encodings::BGRA8, VideoFormat::GXF_VIDEO_FORMAT_BGRA},\n    {img_encodings::BGR16, VideoFormat::GXF_VIDEO_FORMAT_BGR16},\n    {img_encodings::MONO8, VideoFormat::GXF_VIDEO_FORMAT_GRAY},\n    {img_encodings::MONO16, VideoFormat::GXF_VIDEO_FORMAT_GRAY16},\n    {img_encodings::TYPE_32FC1, VideoFormat::GXF_VIDEO_FORMAT_GRAY32},\n    {img_encodings::NV24, VideoFormat::GXF_VIDEO_FORMAT_NV24_ER},\n    {\"nv12\", VideoFormat::GXF_VIDEO_FORMAT_NV12_ER},\n    {\"16UC1\", VideoFormat::GXF_VIDEO_FORMAT_GRAY16},\n    {img_encodings::TYPE_32FC3, VideoFormat::GXF_VIDEO_FORMAT_RGB32},\n    {img_encodings::TYPE_32FC4, VideoFormat::GXF_VIDEO_FORMAT_RGBD32}});\n\n// Map to store the Nitros format encoding to ROS format encoding\nconst std::unordered_map<VideoFormat, std::string> g_gxf_to_ros_video_format({\n    {VideoFormat::GXF_VIDEO_FORMAT_RGB, img_encodings::RGB8},\n    {VideoFormat::GXF_VIDEO_FORMAT_RGBA, img_encodings::RGBA8},\n    {VideoFormat::GXF_VIDEO_FORMAT_RGB16, img_encodings::RGB16},\n    {VideoFormat::GXF_VIDEO_FORMAT_BGR, img_encodings::BGR8},\n    {VideoFormat::GXF_VIDEO_FORMAT_BGRA, img_encodings::BGRA8},\n    {VideoFormat::GXF_VIDEO_FORMAT_BGR16, img_encodings::BGR16},\n    {VideoFormat::GXF_VIDEO_FORMAT_GRAY, img_encodings::MONO8},\n    {VideoFormat::GXF_VIDEO_FORMAT_GRAY16, img_encodings::MONO16},\n    {VideoFormat::GXF_VIDEO_FORMAT_GRAY32, img_encodings::TYPE_32FC1},\n    {VideoFormat::GXF_VIDEO_FORMAT_D32F, img_encodings::TYPE_32FC1},\n    {VideoFormat::GXF_VIDEO_FORMAT_NV24_ER, img_encodings::NV24},\n    {VideoFormat::GXF_VIDEO_FORMAT_NV12_ER, \"nv12\"},\n    {VideoFormat::GXF_VIDEO_FORMAT_RGB32, img_encodings::TYPE_32FC3},\n    {VideoFormat::GXF_VIDEO_FORMAT_RGBD32, img_encodings::TYPE_32FC4}});\n\ntemplate<VideoFormat T>\nstruct NoPaddingColorPlanes {};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_RGB>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"RGB\", 3, width * 3)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_BGR>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"BGR\", 3, width * 3)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_RGBA>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"RGBA\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_BGRA>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"BGRA\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_RGB16>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"RGB16\", 6, width * 6)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_BGR16>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"BGR16\", 6, width * 6)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_GRAY>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"GRAY\", 1, width)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_GRAY16>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"GRAY\", 2, width * 2)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_GRAY32>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"GRAY\", 4, width * 4)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_NV12>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n        nvidia::gxf::ColorPlane(\"UV\", 2, width * 2)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_NV12_ER>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n        nvidia::gxf::ColorPlane(\"UV\", 2, width * 2)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_NV24>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n        nvidia::gxf::ColorPlane(\"UV\", 2, width * 2)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_NV24_ER>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"Y\", 1, width),\n        nvidia::gxf::ColorPlane(\"UV\", 2, width * 2)}) {}\n  std::array<nvidia::gxf::ColorPlane, 2> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_RGB32>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"RGB32\", 12, width * 12)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\ntemplate<>\nstruct NoPaddingColorPlanes<VideoFormat::GXF_VIDEO_FORMAT_RGBD32>\n{\n  NoPaddingColorPlanes(size_t width)\n  : planes({nvidia::gxf::ColorPlane(\"RGBD32\", 16, width * 16)}) {}\n  std::array<nvidia::gxf::ColorPlane, 1> planes;\n};\n\n}  // namespace\n\n#endif  // ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_DETAILS_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/include/isaac_ros_nitros_image_type/nitros_image_view.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_VIEW_HPP_\n#define ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_VIEW_HPP_\n\n#include <string>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/multimedia/video.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros/types/nitros_type_view_factory.hpp\"\n#include \"isaac_ros_nitros_image_type/nitros_image.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nNITROS_TYPE_VIEW_FACTORY_BEGIN(NitrosImage)\n\nMARK_PUBLIC_SECTION()\n// Public methods\ninline uint64_t GetSizeInBytes() const {return image_->size();}\ninline uint32_t GetWidth() const {return image_->video_frame_info().width;}\ninline uint32_t GetHeight() const {return image_->video_frame_info().height;}\ninline uint32_t GetStride(const size_t plane_idx = 0) const\n{\n  return image_->video_frame_info().color_planes[plane_idx].stride;\n}\ninline const unsigned char * GetGpuData() const {return image_->pointer();}\nconst std::string GetEncoding() const;\n\nMARK_PUBLIC_SECTION()\ngxf::VideoBuffer * image_{nullptr};\n\nNITROS_TYPE_VIEW_FACTORY_END(NitrosImage)\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_IMAGE_TYPE__NITROS_IMAGE_VIEW_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022-2025, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_image_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS Nitros Image Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n  <author>Swapnesh Wani</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>sensor_msgs</depend>\n  <depend>isaac_ros_vpi_utils</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>isaac_ros_common</depend>\n  <depend>libnvvpi4</depend>\n  <depend>vpi4-dev</depend>\n\n  <build_depend>ament_index_cpp</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/src/nitros_image.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <cuda_runtime.h>\n\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/multimedia/video.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_image_type/nitros_image.hpp\"\n#include \"isaac_ros_nitros_image_type/nitros_image_details.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n#include \"sensor_msgs/image_encodings.hpp\"\n#include \"vpi/Image.h\"\n#include \"vpi/algo/ConvertImageFormat.h\"\n\n#include \"isaac_ros_vpi_utils/vpi_utilities.hpp\"\n#include \"isaac_ros_common/cuda_stream.hpp\"\n\nconstexpr char kEntityName[] = \"memory_pool\";\nconstexpr char kComponentName[] = \"unbounded_allocator\";\nconstexpr char kComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\n\nnamespace\n{\nnamespace nitros = nvidia::isaac_ros::nitros;\n\n// Get step size for ROS Image\nuint32_t get_step_size(const nvidia::gxf::VideoBufferInfo & video_buff_info)\n{\n  return video_buff_info.width * video_buff_info.color_planes[0].bytes_per_pixel;\n}\n\nuint32_t get_step_size(const VPIImageData & vpi_img_data)\n{\n  return vpi_img_data.buffer.pitch.planes[0].pitchBytes;\n}\n\ntemplate<VideoFormat T>\nvoid allocate_video_buffer_no_padding(\n  const uint32_t width,\n  const uint32_t height,\n  const nvidia::gxf::Handle<nvidia::gxf::VideoBuffer> & video_buff,\n  const nvidia::gxf::Handle<nvidia::gxf::Allocator> & allocator_handle)\n{\n  constexpr auto surface_layout = nvidia::gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR;\n  constexpr auto storage_type = nvidia::gxf::MemoryStorageType::kDevice;\n  if (width % 2 != 0 || height % 2 != 0) {\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImage\"),\n      \"[convert_to_custom] Image width/height must be even for creation of gxf::VideoBuffer\");\n    throw std::runtime_error(\"[convert_to_custom] Odd Image width or height.\");\n  }\n  NoPaddingColorPlanes<T> nopadding_planes(width);\n  nvidia::gxf::VideoFormatSize<T> format_size;\n  uint64_t size = format_size.size(width, height, nopadding_planes.planes);\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosImage\"),\n    \"[image size]  [%ld].\", size);\n  std::vector<nvidia::gxf::ColorPlane> color_planes{nopadding_planes.planes.begin(),\n    nopadding_planes.planes.end()};\n  nvidia::gxf::VideoBufferInfo buffer_info{static_cast<uint32_t>(width),\n    static_cast<uint32_t>(height),\n    T, color_planes,\n    surface_layout};\n\n  video_buff->resizeCustom(buffer_info, size, storage_type, allocator_handle);\n}\n\nvoid allocate_video_buffer(\n  const sensor_msgs::msg::Image & source,\n  const nvidia::gxf::Handle<nvidia::gxf::VideoBuffer> & video_buff,\n  const nvidia::gxf::Handle<nvidia::gxf::Allocator> & allocator_handle)\n{\n  auto color_fmt = g_ros_to_gxf_video_format.find(source.encoding);\n  if (color_fmt == std::end(g_ros_to_gxf_video_format)) {\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImage\"),\n      \"[convert_to_custom] Unsupported encoding from ROS [%s].\", source.encoding.c_str());\n    throw std::runtime_error(\"[convert_to_custom] Unsupported encoding from ROS.\");\n  }\n\n  switch (color_fmt->second) {\n    case VideoFormat::GXF_VIDEO_FORMAT_RGB:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_RGB>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_RGBA:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_RGBA>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_RGB16:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_RGB16>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_BGR:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_BGR>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_BGRA:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_BGRA>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_BGR16:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_BGR16>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_GRAY:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_GRAY>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_GRAY16:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_GRAY16>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_GRAY32:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_GRAY32>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_NV24:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_NV24>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_NV12:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_NV12>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n    case VideoFormat::GXF_VIDEO_FORMAT_RGB32:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_RGB32>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_RGBD32:\n      allocate_video_buffer_no_padding<VideoFormat::GXF_VIDEO_FORMAT_RGBD32>(\n        source.width, source.height, video_buff, allocator_handle);\n      break;\n\n    default:\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosImage\"),\n        \"[convert_to_custom] Unsupported encoding from ROS [%s].\", source.encoding.c_str());\n      throw std::runtime_error(\"[convert_to_custom] Unsupported encoding from ROS.\");\n      break;\n  }\n}\n\nVPIStatus CreateVPIImageWrapper(\n  VPIImage & vpi_image, VPIImageData & img_data, uint64_t flags,\n  const nvidia::gxf::Handle<nvidia::gxf::VideoBuffer> & video_buff)\n{\n  nvidia::gxf::VideoBufferInfo image_info = video_buff->video_frame_info();\n  nvidia::isaac_ros::vpi_utils::VPIFormat vpi_format =\n    nvidia::isaac_ros::vpi_utils::ToVpiFormat(image_info.color_format);\n  img_data.bufferType = VPI_IMAGE_BUFFER_CUDA_PITCH_LINEAR;\n  img_data.buffer.pitch.format = vpi_format.image_format;\n  img_data.buffer.pitch.numPlanes = image_info.color_planes.size();\n  auto data_ptr_offset = 0;\n  for (size_t i = 0; i < image_info.color_planes.size(); ++i) {\n    img_data.buffer.pitch.planes[i].pBase = video_buff->pointer() + data_ptr_offset;\n    img_data.buffer.pitch.planes[i].height = image_info.color_planes[i].height;\n    img_data.buffer.pitch.planes[i].width = image_info.color_planes[i].width;\n    img_data.buffer.pitch.planes[i].pixelType = vpi_format.pixel_type[i];\n    img_data.buffer.pitch.planes[i].pitchBytes = image_info.color_planes[i].stride;\n\n    data_ptr_offset = image_info.color_planes[i].size;\n  }\n  return vpiImageCreateWrapper(&img_data, nullptr, flags, &vpi_image);\n}\n}  // namespace\n\n\nvoid rclcpp::TypeAdapter<nitros::NitrosImage, sensor_msgs::msg::Image>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nitros::nvtxRangePushWrapper(\"NitrosImage::convert_to_ros_message\", nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosImage\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto gxf_video_buffer = msg_entity->get<nvidia::gxf::VideoBuffer>();\n  if (!gxf_video_buffer) {\n    std::string error_msg =\n      \"[convert_to_ros_message] Failed to get the existing VideoBuffer object\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImage\"), error_msg.c_str());\n    throw std::runtime_error(error_msg.c_str());\n  }\n\n  // Setting Image date from gxf VideoBuffer\n  auto video_buffer_info = gxf_video_buffer.value()->video_frame_info();\n  destination.height = video_buffer_info.height;\n  destination.width = video_buffer_info.width;\n  const auto encoding = g_gxf_to_ros_video_format.find(video_buffer_info.color_format);\n  if (encoding == std::end(g_gxf_to_ros_video_format)) {\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImage\"),\n      \"[convert_to_ros_message] Unsupported encoding from gxf [%d].\",\n      (int)video_buffer_info.color_format);\n    throw std::runtime_error(\"[convert_to_custom] Unsupported encoding from gxf .\");\n  } else {\n    destination.encoding = encoding->second;\n  }\n\n  void * src_ptr;\n  size_t src_pitch{0};\n  uint32_t step_size{0};\n  VPIStream vpi_stream{};\n  uint64_t vpi_flags{VPI_BACKEND_CUDA};\n  VPIImage input{}, output{};\n  VPIImageData input_data{}, output_data{};\n\n  if (destination.encoding == \"nv12\" || destination.encoding == \"nv24\") {\n    // Convert multiplanar to interleaved rgb\n    RCLCPP_DEBUG(\n      rclcpp::get_logger(\"NitrosImage\"),\n      \"[convert_to_ros_message] Multiplanar to interleaved started\");\n\n    // Create VPI stream\n    CHECK_VPI_STATUS(vpiStreamCreate(vpi_flags, &vpi_stream));\n\n    // VPI input image\n    CHECK_VPI_STATUS(CreateVPIImageWrapper(input, input_data, vpi_flags, gxf_video_buffer.value()));\n    // VPI output image\n    CHECK_VPI_STATUS(\n      vpiImageCreate(\n        destination.width, destination.height, VPI_IMAGE_FORMAT_RGB8, vpi_flags, &output));\n\n    // Call for color format conversion\n    CHECK_VPI_STATUS(vpiSubmitConvertImageFormat(vpi_stream, vpi_flags, input, output, nullptr));\n\n    // Wait for operations to complete\n    CHECK_VPI_STATUS(vpiStreamSync(vpi_stream));\n\n    // Copy data\n    CHECK_VPI_STATUS(\n      vpiImageLockData(output, VPI_LOCK_READ, VPI_IMAGE_BUFFER_CUDA_PITCH_LINEAR, &output_data));\n    // Release lock\n    CHECK_VPI_STATUS(vpiImageUnlock(output));\n\n    destination.encoding = img_encodings::RGB8;\n    src_ptr = output_data.buffer.pitch.planes[0].pBase;\n    src_pitch = get_step_size(output_data);\n    step_size = get_step_size(output_data);\n\n    RCLCPP_DEBUG(\n      rclcpp::get_logger(\"NitrosImage\"),\n      \"[convert_to_ros_message] Multiplanar to interleaved finished\");\n  } else {\n    src_ptr = gxf_video_buffer.value()->pointer();\n    src_pitch = video_buffer_info.color_planes[0].stride;\n    step_size = get_step_size(video_buffer_info);\n  }\n\n  destination.is_bigendian = 0;\n\n  // Full row length in bytes\n  destination.step = step_size;\n\n  // Resize the ROS image buffer to the right size\n  destination.data.resize(destination.step * destination.height);\n\n  // Copy data from Device to Host\n  const cudaError_t cuda_error = cudaMemcpy2DAsync(\n    destination.data.data(),\n    destination.step,\n    src_ptr,\n    src_pitch,\n    destination.step,\n    destination.height,\n    cudaMemcpyDeviceToHost,\n    source.cuda_stream\n  );\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] cudaMemcpy2D failed for conversion from \"\n      \"NitrosImage to sensor_msgs::msg::Image: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // since this is meant to be synced right away, we should sync the stream\n  cudaError_t cuda_result = cudaStreamSynchronize(source.cuda_stream);\n  CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n\n  vpiImageDestroy(input);\n  vpiImageDestroy(output);\n  vpiStreamDestroy(vpi_stream);\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>();\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp.value()->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp.value()->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Set frame ID\n  destination.header.frame_id = source.frame_id;\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosImage\"),\n    \"[convert_to_ros_message] Conversion completed for handle=%ld\", source.handle);\n\n  nitros::nvtxRangePopWrapper();\n}\n\n\nvoid rclcpp::TypeAdapter<nitros::NitrosImage, sensor_msgs::msg::Image>::convert_to_custom(\n  const ros_message_type & source, custom_type & destination)\n{\n  nitros::nvtxRangePushWrapper(\"NitrosImage::convert_to_custom\", nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(rclcpp::get_logger(\"NitrosImage\"), \"[convert_to_custom] Conversion started\");\n\n  auto context = nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  auto message = nvidia::gxf::Entity::New(context);\n  if (!message) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Error initializing new message entity: \" <<\n      GxfResultStr(message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  auto gxf_video_buffer = message->add<nvidia::gxf::VideoBuffer>(source.header.frame_id.c_str());\n  if (!gxf_video_buffer) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Failed to create a VideoBuffer object: \" <<\n      GxfResultStr(gxf_video_buffer.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // The uint64_t component which represents as sequential frame number\n  // is required by gxf camera_message\n  auto uint64_t_buffer = message->add<uint64_t>();\n  if (!uint64_t_buffer) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Failed to create a uint64_t object: \" <<\n      GxfResultStr(uint64_t_buffer.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Allocate buffer\n  allocate_video_buffer(source, gxf_video_buffer.value(), allocator_handle);\n\n  auto video_buffer_info = gxf_video_buffer.value()->video_frame_info();\n  auto nitros_cuda_stream =\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext()\n    .getCudaStreamFromNitrosGraph();\n  // Copy data from Host to Device\n  auto width = get_step_size(video_buffer_info);\n  const cudaError_t cuda_error = cudaMemcpy2DAsync(\n    gxf_video_buffer.value()->pointer(),\n    video_buffer_info.color_planes[0].stride,\n    source.data.data(),\n    source.step,\n    width,\n    video_buffer_info.height,\n    cudaMemcpyHostToDevice,\n    nitros_cuda_stream);\n\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] cudaMemcpy2D failed for conversion from \"\n      \"sensor_msgs::msg::Image to NitrosImage: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  cudaError_t cuda_result = cudaStreamSynchronize(nitros_cuda_stream);\n  CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  auto output_timestamp = message->add<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!output_timestamp) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Failed to add a timestamp component to Image message: \" <<\n      GxfResultStr(output_timestamp.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImage\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  output_timestamp.value()->acqtime = input_timestamp;\n\n  // Set frame ID\n  destination.frame_id = source.header.frame_id;\n\n  // Set Entity Id\n  destination.handle = message->eid();\n  GxfEntityRefCountInc(context, message->eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosImage\"),\n    \"[convert_to_custom] Conversion completed (resulting handle=%ld)\", message->eid());\n\n  nitros::nvtxRangePopWrapper();\n}\n\n// Map to store the Nitros image format encoding to Video buffer format\nconst std::unordered_map<std::string, VideoFormat> g_nitros_to_gxf_video_format({\n  {\"nitros_image_rgb8\", VideoFormat::GXF_VIDEO_FORMAT_RGB},\n  {\"nitros_image_rgba8\", VideoFormat::GXF_VIDEO_FORMAT_RGBA},\n  {\"nitros_image_rgb16\", VideoFormat::GXF_VIDEO_FORMAT_RGB16},\n  {\"nitros_image_bgr8\", VideoFormat::GXF_VIDEO_FORMAT_BGR},\n  {\"nitros_image_bgra8\", VideoFormat::GXF_VIDEO_FORMAT_BGRA},\n  {\"nitros_image_bgr16\", VideoFormat::GXF_VIDEO_FORMAT_BGR16},\n  {\"nitros_image_mono8\", VideoFormat::GXF_VIDEO_FORMAT_GRAY},\n  {\"nitros_image_mono16\", VideoFormat::GXF_VIDEO_FORMAT_GRAY16},\n  {\"nitros_image_32FC1\", VideoFormat::GXF_VIDEO_FORMAT_GRAY32},\n  {\"nitros_image_nv12\", VideoFormat::GXF_VIDEO_FORMAT_NV12},\n  {\"nitros_image_nv24\", VideoFormat::GXF_VIDEO_FORMAT_NV24},\n  {\"nitros_image_32FC3\", VideoFormat::GXF_VIDEO_FORMAT_RGB32},\n  {\"nitros_image_32FC4\", VideoFormat::GXF_VIDEO_FORMAT_RGBD32}\n});\n\nuint64_t calculate_image_size(const std::string image_type, uint32_t width, uint32_t height)\n{\n  auto color_fmt = g_nitros_to_gxf_video_format.find(image_type);\n  if (color_fmt == std::end(g_nitros_to_gxf_video_format)) {\n    throw std::runtime_error(\"[calculate_image_size] Unsupported encoding from ROS.\");\n  }\n\n  uint64_t image_size = 0;\n  switch (color_fmt->second) {\n    case VideoFormat::GXF_VIDEO_FORMAT_RGB:\n      nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_RGB> format_size_rgb8;\n      image_size = format_size_rgb8.size(width, height);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_RGBA:\n      nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_RGBA> format_size_rgba;\n      image_size = format_size_rgba.size(width, height);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_RGB16:\n      nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_RGB16> format_size_rgb16;\n      image_size = format_size_rgb16.size(width, height);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_BGR:\n      nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_BGR> format_size_bgr8;\n      image_size = format_size_bgr8.size(width, height);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_BGRA:\n      nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_BGRA> format_size_bgra;\n      image_size = format_size_bgra.size(width, height);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_BGR16:\n      nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_BGR16> format_size_bgr16;\n      image_size = format_size_bgr16.size(width, height);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_GRAY:\n      nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_GRAY> format_size_gray;\n      image_size = format_size_gray.size(width, height);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_GRAY16:\n      nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_GRAY16> format_size_gray16;\n      image_size = format_size_gray16.size(width, height);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_GRAY32:\n      nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_GRAY32> format_size_gray32;\n      image_size = format_size_gray32.size(width, height);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_NV12:\n      {\n        nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_NV12> format_size_nv12;\n        image_size = format_size_nv12.size(width, height);\n        break;\n      }\n\n    case VideoFormat::GXF_VIDEO_FORMAT_NV24:\n      {\n        nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_NV24> format_size_nv24;\n        image_size = format_size_nv24.size(width, height);\n        break;\n      }\n\n    case VideoFormat::GXF_VIDEO_FORMAT_RGB32:\n      {\n        nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_RGB32> format_size_rgb32;\n        image_size = format_size_rgb32.size(width, height);\n        break;\n      }\n    case VideoFormat::GXF_VIDEO_FORMAT_RGBD32:\n      {\n        nvidia::gxf::VideoFormatSize<VideoFormat::GXF_VIDEO_FORMAT_RGBD32> format_size_rgbd32;\n        image_size = format_size_rgbd32.size(width, height);\n        break;\n      }\n\n    default:\n      break;\n  }\n  return image_size;\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/src/nitros_image_builder.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <cuda_runtime.h>\n\n#include <string>\n#include <vector>\n\n#include \"isaac_ros_nitros_image_type/nitros_image_builder.hpp\"\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/multimedia/video.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n\nnamespace\n{\nconstexpr uint64_t kNanosecondsInSeconds = 1e9;\n\nnvidia::gxf::Expected<void> ReleaseImageCallback(\n  std::function<void()> release_callback,\n  void * ptr)\n{\n  if (release_callback) {\n    release_callback();\n  } else {\n    cudaFree(ptr);\n  }\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosImageBuilder\"),\n    \"[ReleaseImageCallback] Released the cuda memory [%p]\", ptr);\n  return nvidia::gxf::Success;\n}\n\ntemplate<VideoFormat T>\nvoid create_image(\n  const uint32_t width, const uint32_t height,\n  nvidia::gxf::Expected<nvidia::gxf::Entity> & msg_entity, void * data, const std::string & name,\n  std::function<void()> release_callback)\n{\n  if (width % 2 != 0 || height % 2 != 0) {\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImageBuilder\"),\n      \"[create_image] Image width/height must be even for creation of gxf::VideoBuffer\");\n    throw std::runtime_error(\"[create_image] Odd Image width or height.\");\n  }\n\n  auto gxf_image = msg_entity->add<nvidia::gxf::VideoBuffer>(name.c_str());\n  NoPaddingColorPlanes<T> nopadding_planes(width);\n  nvidia::gxf::VideoFormatSize<T> format_size;\n  uint64_t size = format_size.size(width, height, nopadding_planes.planes);\n\n  std::vector<nvidia::gxf::ColorPlane> color_planes{nopadding_planes.planes.begin(),\n    nopadding_planes.planes.end()};\n\n  constexpr auto surface_layout = nvidia::gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR;\n  constexpr auto storage_type = nvidia::gxf::MemoryStorageType::kDevice;\n\n  nvidia::gxf::VideoBufferInfo buffer_info{width, height, T, color_planes, surface_layout};\n\n  gxf_image.value()->wrapMemory(\n    buffer_info, size, storage_type, data,\n    std::bind(&ReleaseImageCallback, release_callback, std::placeholders::_1));\n}\n}  // namespace\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nNitrosImageBuilder::NitrosImageBuilder()\n: nitros_image_{}\n{\n  auto message = gxf::Entity::New(GetTypeAdapterNitrosContext().getContext());\n  if (!message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[constructor] Error initializing new message entity: \" <<\n      GxfResultStr(message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImageBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  nitros_image_.handle = message->eid();\n  GxfEntityRefCountInc(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), message->eid());\n\n  RCLCPP_DEBUG(rclcpp::get_logger(\"NitrosImageBuilder\"), \"[constructor] NitrosImage initialized\");\n}\n\nNitrosImageBuilder::NitrosImageBuilder(NitrosImageBuilder && other)\n{\n  nitros_image_ = other.nitros_image_;\n  encoding_ = other.encoding_;\n  height_ = other.height_;\n  width_ = other.width_;\n  data_ = other.data_;\n  event_ = other.event_;\n\n  // Reset other\n  other.encoding_ = \"\";\n  other.height_ = 0;\n  other.width_ = 0;\n  other.data_ = nullptr;\n}\n\nNitrosImageBuilder & NitrosImageBuilder::operator=(NitrosImageBuilder && other)\n{\n  // In case other is this, then nothing should be done.\n  if (&other == this) {\n    return *this;\n  }\n  nitros_image_ = other.nitros_image_;\n  encoding_ = other.encoding_;\n  height_ = other.height_;\n  width_ = other.width_;\n  data_ = other.data_;\n  event_ = other.event_;\n  return *this;\n}\n\nvoid NitrosImageBuilder::Validate()\n{\n  bool failure = false;\n  std::stringstream error_msg;\n  if (height_ == 0 || width_ == 0) {\n    error_msg << \"Dimensions are not set! Call WithDimension method before Build. \\n\";\n    failure = true;\n  }\n  if (encoding_ == \"\") {\n    error_msg << \"Encoding is not set! Call WithEncoding method before Build. \\n\";\n    failure = true;\n  }\n  if (data_ == nullptr) {\n    error_msg << \"Data buffer is not set! Call WithGpuData method before Build. \\n\";\n    failure = true;\n  }\n\n  if (failure) {\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosImageBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n}\n\nNitrosImageBuilder & NitrosImageBuilder::WithHeader(std_msgs::msg::Header header)\n{\n  auto message = gxf::Entity::Shared(\n    GetTypeAdapterNitrosContext().getContext(), nitros_image_.handle);\n\n  // Set timestamp\n  auto output_timestamp = message->add<gxf::Timestamp>(\"timestamp\");\n  if (!output_timestamp) {\n    std::stringstream error_msg;\n    error_msg << \"[WithHeader] Failed to add a timestamp component to message: \" <<\n      GxfResultStr(output_timestamp.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImageBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  output_timestamp.value()->acqtime = header.stamp.sec * kNanosecondsInSeconds +\n    header.stamp.nanosec;\n\n  // Set frame ID\n  nitros_image_.frame_id = header.frame_id;\n\n  return *this;\n}\n\nNitrosImageBuilder & NitrosImageBuilder::WithEncoding(std::string encoding)\n{\n  encoding_ = encoding;\n  return *this;\n}\n\nNitrosImageBuilder & NitrosImageBuilder::WithDimensions(uint32_t height, uint32_t width)\n{\n  height_ = height;\n  width_ = width;\n  return *this;\n}\n\nNitrosImageBuilder & NitrosImageBuilder::WithGpuData(void * data)\n{\n  data_ = data;\n  return *this;\n}\n\nNitrosImageBuilder & NitrosImageBuilder::WithReleaseCallback(std::function<void()> release_callback)\n{\n  release_callback_ = release_callback;\n  return *this;\n}\n\nNitrosImage NitrosImageBuilder::Build()\n{\n  // Validate all data is present before building the NitrosImage\n  Validate();\n\n  auto message = gxf::Entity::Shared(\n    GetTypeAdapterNitrosContext().getContext(), nitros_image_.handle);\n\n  // If CUDA event provided, synchronize on that event before building\n  if (event_) {\n    cudaError_t cuda_error = cudaEventSynchronize(event_);\n\n    if (cuda_error != cudaSuccess) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[Build] cudaEventSynchronize failed: \" <<\n        cudaGetErrorName(cuda_error) <<\n        \" (\" << cudaGetErrorString(cuda_error) << \")\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosImageBuilder\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n\n    cuda_error = cudaEventDestroy(event_);\n    if (cuda_error != cudaSuccess) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[Build] cudaEventDestroy failed: \" <<\n        cudaGetErrorName(cuda_error) <<\n        \" (\" << cudaGetErrorString(cuda_error) << \")\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosImageBuilder\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n\n  auto color_fmt = g_ros_to_gxf_video_format.find(encoding_);\n  if (color_fmt == std::end(g_ros_to_gxf_video_format)) {\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImageBuilder\"), \"Unsupported encoding [%s].\", encoding_.c_str());\n    throw std::runtime_error(\"[NitrosImageBuilder] Unsupported encoding.\");\n  }\n\n  switch (color_fmt->second) {\n    case VideoFormat::GXF_VIDEO_FORMAT_RGB:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_RGB>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_RGBA:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_RGBA>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_RGB16:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_RGB16>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_BGR:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_BGR>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_BGRA:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_BGRA>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_BGR16:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_BGR16>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_GRAY:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_GRAY>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_GRAY16:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_GRAY16>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_GRAY32:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_GRAY32>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_NV24:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_NV24>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_NV24_ER:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_NV24_ER>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_NV12:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_NV12>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_NV12_ER:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_NV12_ER>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_RGB32:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_RGB32>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    case VideoFormat::GXF_VIDEO_FORMAT_RGBD32:\n      create_image<VideoFormat::GXF_VIDEO_FORMAT_RGBD32>(\n        width_, height_, message, data_, nitros_image_.frame_id, release_callback_);\n      break;\n\n    default:\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosImageBuilder\"), \"Unsupported encoding [%s].\", encoding_.c_str());\n      throw std::runtime_error(\"[convert_to_custom] Unsupported encoding from ROS.\");\n      break;\n  }\n\n  RCLCPP_DEBUG(rclcpp::get_logger(\"NitrosImageBuilder\"), \"[Build] Image built\");\n\n  // Reseting data after it is done building\n  data_ = nullptr;\n  return nitros_image_;\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/src/nitros_image_view.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <string>\n#include <vector>\n\n#include \"isaac_ros_nitros_image_type/nitros_image_view.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nvoid NitrosImageView::InitView()\n{\n  auto gxf_video_buffer = msg_entity_->get<gxf::VideoBuffer>();\n  if (!gxf_video_buffer) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[NitrosImageView::InitView] failed to get GXF video buffer\" <<\n      GxfResultStr(gxf_video_buffer.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImageView\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  image_ = &(*gxf_video_buffer.value());\n}\n\nconst std::string NitrosImageView::GetEncoding() const\n{\n  const auto encoding = g_gxf_to_ros_video_format.find(image_->video_frame_info().color_format);\n  if (encoding == std::end(g_gxf_to_ros_video_format)) {\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImageView\"),\n      \"[NitrosImageView::GetEncoding] Unsupported encoding from gxf [%d].\",\n      (int)image_->video_frame_info().color_format);\n    throw std::runtime_error(\"[NitrosImageView::GetEncoding] Unsupported encoding from gxf .\");\n  } else {\n    return encoding->second;\n  }\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/test/isaac_ros_nitros_image_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosImage type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import Image\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosImageTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='image_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_image_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosImageForwardNode',\n                name='NitrosImageForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_image_bgr8'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/image'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output_image'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosImageTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosImageTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosImage type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_image')\n    def test_nitros_image_ros_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the image received from NitrosImage type conversion to be identical to source.\"\"\"\n        self.generate_namespace_lookup(['image', 'output_image'])\n        received_messages = {}\n\n        received_image_sub = self.create_logging_subscribers(\n            subscription_requests=[('output_image', Image)],\n            received_messages=received_messages\n        )\n\n        image_pub = self.node.create_publisher(\n            Image, self.namespaces['image'], self.DEFAULT_QOS)\n\n        try:\n            image = JSONConversion.load_image_from_json(\n                test_folder / 'image.json')\n            timestamp = self.node.get_clock().now().to_msg()\n            image.header.stamp = timestamp\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 2\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                image_pub.publish(image)\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output_image' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on output_image topic!\")\n\n            received_image = received_messages['output_image']\n\n            print(f'Source image data size: {len(image.data)}')\n            print(f'Received image data size: {len(received_image.data)}')\n\n            self.assertEqual(str(timestamp), str(received_image.header.stamp),\n                             'Timestamps do not match.')\n\n            self.assertEqual(len(image.data), len(received_image.data),\n                             'Source and received image sizes do not match: ' +\n                             f'{len(image.data)} != {len(received_image.data)}')\n\n            for i in range(100):\n                self.assertEqual(image.data[i], received_image.data[i],\n                                 'Source and received image pixels do not match')\n\n            # Make sure that the source and received images are the same\n            self.assertEqual(received_image.height, image.height,\n                             'Source and received image heights do not match: ' +\n                             f'{image.height} != {received_image.height}')\n            self.assertEqual(received_image.width, image.width,\n                             'Source and received image widths do not match: ' +\n                             f'{image.width} != {received_image.width}')\n\n            print('Source and received images are identical.')\n        finally:\n            self.node.destroy_subscription(received_image_sub)\n            self.node.destroy_publisher(image_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/test/src/nitros_image_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_image_type/nitros_image.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_image_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_image_bgr8\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosImageForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosImageForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosImage>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosImageForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_image_type/test/test_cases/nitros_image/profile/image.json",
    "content": "{\n    \"image\": \"NVIDIAprofile.png\",\n    \"encoding\": \"bgr8\"\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_imu_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_imu_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(yaml-cpp)\n\n# NitrosImu\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_imu.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosImuForwardNode\n  ament_auto_add_library(isaac_ros_nitros_imu_forward_node SHARED test/src/nitros_imu_forward_node.cpp)\n  target_link_libraries(isaac_ros_nitros_imu_forward_node ${PROJECT_NAME})\n  set_target_properties(isaac_ros_nitros_imu_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(isaac_ros_nitros_imu_forward_node\n    \"nvidia::isaac_ros::nitros::NitrosImuForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosImuForwardNode;\\\n    $<TARGET_FILE:isaac_ros_nitros_imu_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_imu_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_imu_type/include/isaac_ros_nitros_imu_type/nitros_imu.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_IMU_TYPE__NITROS_IMU_HPP_\n#define ISAAC_ROS_NITROS_IMU_TYPE__NITROS_IMU_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosImu\n *   ROS type:    sensor_msgs::msg::Imu\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"sensor_msgs/msg/imu.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosImu;\n\n// Formats\nstruct nitros_imu_t\n{\n  using MsgT = NitrosImu;\n  static const inline std::string supported_type_name = \"nitros_imu\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosImu)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_imu_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\n// for nvidia::isaac::StringProvider\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_gxf_helpers\", \"gxf/lib/libgxf_isaac_gxf_helpers.so\")\n// for nvidia::isaac::SightReceiver\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_sight\", \"gxf/lib/libgxf_isaac_sight.so\")\n// for nvidia::isaac::PoseTree\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_atlas\", \"gxf/lib/libgxf_isaac_atlas.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_messages\", \"gxf/lib/libgxf_isaac_messages.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosImu,\n  sensor_msgs::msg::Imu>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosImu;\n  using ros_message_type = sensor_msgs::msg::Imu;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosImu,\n  sensor_msgs::msg::Imu);\n\n#endif  // ISAAC_ROS_NITROS_IMU_TYPE__NITROS_IMU_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_imu_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_imu_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS NITROS IMU Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Ashwin Varghese Kuruttukulam</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>sensor_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>gxf_isaac_messages</depend>\n  <depend>gxf_isaac_gxf_helpers</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>gxf_isaac_gems</build_depend>\n\n  <exec_depend>gxf_isaac_atlas</exec_depend>\n  <exec_depend>gxf_isaac_sight</exec_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_imu_type/src/nitros_imu.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gems/pose_tree/pose_tree.hpp\"\n#include \"gxf/std/allocator.hpp\"\n#include \"extensions/messages/imu_message.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_imu_type/nitros_imu.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nnamespace\n{\nconstexpr char kEntityName[] = \"memory_pool\";\nconstexpr char kComponentName[] = \"unbounded_allocator\";\nconstexpr char kComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\nconstexpr char kPoseTreeEntityName[] = \"global_pose_tree\";\nconstexpr char kPoseTreeComponentName[] = \"pose_tree\";\nconstexpr char kPoseTreeComponentTypeName[] = \"nvidia::isaac::PoseTree\";\nconstexpr int kCovarianceArrSize = 9;\nconst std::array<double, kCovarianceArrSize> invalidCovMat = {-1, -1, -1, -1, -1, -1, -1, -1, -1};\nconst std::array<double, kCovarianceArrSize> emptyCovMat = {0, 0, 0, 0, 0, 0, 0, 0, 0};\n}  // namespace\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosImu,\n  sensor_msgs::msg::Imu>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosImu::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosImu\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto maybe_imu_parts = nvidia::isaac::GetImuMessage(\n    msg_entity.value());\n  if (!maybe_imu_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get imu gxf message\"\n      \" from message entity: \" <<\n      GxfResultStr(maybe_imu_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImu\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto imu_parts = maybe_imu_parts.value();\n  destination.angular_velocity.x = imu_parts.imu->angular_velocity_x;\n  destination.angular_velocity.y = imu_parts.imu->angular_velocity_y;\n  destination.angular_velocity.z = imu_parts.imu->angular_velocity_z;\n  destination.linear_acceleration.x = imu_parts.imu->linear_acceleration_x;\n  destination.linear_acceleration.y = imu_parts.imu->linear_acceleration_y;\n  destination.linear_acceleration.z = imu_parts.imu->linear_acceleration_z;\n\n  // https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/Imu.msg\n  // Ang Vel and Lin Accel covariances set to 0.0 since data is unavailable in gxf\n  destination.angular_velocity_covariance = emptyCovMat;\n  destination.linear_acceleration_covariance = emptyCovMat;\n  // Orientation covariances set to -1 since orientation data not populated\n  destination.orientation_covariance = invalidCovMat;\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = imu_parts.timestamp;\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Get pointer to posetree component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kPoseTreeEntityName, kPoseTreeComponentName, kPoseTreeComponentTypeName, cid);\n  auto maybe_pose_tree_handle =\n    nvidia::gxf::Handle<nvidia::isaac::PoseTree>::Create(context, cid);\n  if (!maybe_pose_tree_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get pose tree's handle: \" <<\n      GxfResultStr(maybe_pose_tree_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImu\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto pose_tree_handle = maybe_pose_tree_handle.value();\n  auto frame_name = pose_tree_handle->getFrameName(imu_parts.pose_frame_uid->uid);\n  if (frame_name) {\n    destination.header.frame_id = frame_name.value();\n  } else {\n    RCLCPP_DEBUG(\n      rclcpp::get_logger(\"NitrosImu\"),\n      \"Setting frame_id=%s from NITROS msg\",\n      source.frame_id.c_str());\n    // Set NITROS frame id as fallback method of populating frame_id\n    destination.header.frame_id = source.frame_id;\n  }\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosImu\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosImu,\n  sensor_msgs::msg::Imu>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosImu::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosImu\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImu\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  // create imu gxf msg\n  auto maybe_imu_parts = nvidia::isaac::CreateImuMessage(context);\n  if (!maybe_imu_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_gxf_message] Failed to create imu gxf msg \" << GxfResultStr(\n      maybe_imu_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImu\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto imu_parts = maybe_imu_parts.value();\n\n  // Add imu data\n  imu_parts.imu->angular_velocity_x = source.angular_velocity.x;\n  imu_parts.imu->angular_velocity_y = source.angular_velocity.y;\n  imu_parts.imu->angular_velocity_z = source.angular_velocity.z;\n  imu_parts.imu->linear_acceleration_x = source.linear_acceleration.x;\n  imu_parts.imu->linear_acceleration_y = source.linear_acceleration.y;\n  imu_parts.imu->linear_acceleration_z = source.linear_acceleration.z;\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  imu_parts.timestamp->acqtime = input_timestamp;\n\n  // Set frame ID\n  // Get pointer to posetree component\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kPoseTreeEntityName, kPoseTreeComponentName, kPoseTreeComponentTypeName, cid);\n  auto maybe_pose_tree_handle =\n    nvidia::gxf::Handle<nvidia::isaac::PoseTree>::Create(context, cid);\n  if (!maybe_pose_tree_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get pose tree's handle: \" <<\n      GxfResultStr(maybe_pose_tree_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosImu\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto pose_tree_handle = maybe_pose_tree_handle.value();\n  auto maybe_imu_frame_uid = pose_tree_handle->findOrCreateFrame(\n    source.header.frame_id.c_str());\n  if (maybe_imu_frame_uid) {\n    imu_parts.pose_frame_uid->uid = maybe_imu_frame_uid.value();\n  } else {\n    RCLCPP_WARN(\n      rclcpp::get_logger(\"NitrosImu\"), \"Could not create Pose Tree Frame\");\n  }\n\n  // Set NITROS frame id as fallback method of populating frame_id\n  destination.frame_id = source.header.frame_id;\n\n  // Set Entity Id\n  destination.handle = imu_parts.message.eid();\n  GxfEntityRefCountInc(context, imu_parts.message.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosImu\"),\n    \"[convert_to_custom] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_imu_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_imu_type/test/isaac_ros_nitros_imu_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosImu type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\n\nfrom sensor_msgs.msg import Imu\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosImuTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_imu_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosImuForwardNode',\n                name='NitrosImuForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_imu'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosImuTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosImuTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosImu type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_imu')\n    def test_nitros_imu_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the tag from NitrosImu type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', Imu)],\n            received_messages=received_messages\n        )\n\n        imu_pub = self.node.create_publisher(\n            Imu, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            imu = self.load_imu_from_json(test_folder / 'imu.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 10\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                imu.header.stamp = timestamp\n\n                imu_pub.publish(imu)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_imu = received_messages['output']\n\n            # Only test for data passed through gxf\n            self.assertEqual(imu.header.stamp.sec,\n                             received_imu.header.stamp.sec)\n            self.assertEqual(imu.header.stamp.nanosec,\n                             received_imu.header.stamp.nanosec)\n            self.assertEqual(imu.header.frame_id,\n                             received_imu.header.frame_id)\n            self.assertEqual(imu.angular_velocity.x,\n                             received_imu.angular_velocity.x)\n            self.assertEqual(imu.angular_velocity.y,\n                             received_imu.angular_velocity.y)\n            self.assertEqual(imu.angular_velocity.z,\n                             received_imu.angular_velocity.z)\n            self.assertEqual(imu.linear_acceleration.x,\n                             received_imu.linear_acceleration.x)\n            self.assertEqual(imu.linear_acceleration.y,\n                             received_imu.linear_acceleration.y)\n            self.assertEqual(imu.linear_acceleration.z,\n                             received_imu.linear_acceleration.z)\n            print('The received imu message has been verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(imu_pub)\n\n    @staticmethod\n    def load_imu_from_json(\n            json_filepath: pathlib.Path) -> Imu:\n        \"\"\"\n        Load a Imu message from a JSON filepath.\n\n        Parameters\n        ----------\n        json_filepath : Path\n            The path to a JSON file containing the Imu fields\n\n        Returns\n        -------\n        Imu\n            Generated Imu message\n\n        \"\"\"\n        imu_json = JSONConversion.load_from_json(\n            json_filepath)\n\n        imu = Imu()\n        imu.header.frame_id = imu_json[\n            'header']['frame_id']\n        imu.header.stamp.sec = imu_json[\n            'header']['stamp']['sec']\n        imu.header.stamp.nanosec = imu_json[\n            'header']['stamp']['nanosec']\n        imu.orientation.x = imu_json['orientation']['x']\n        imu.orientation.y = imu_json['orientation']['y']\n        imu.orientation.z = imu_json['orientation']['z']\n        imu.orientation.w = imu_json['orientation']['w']\n        imu.angular_velocity.x = imu_json['angular_velocity']['x']\n        imu.angular_velocity.y = imu_json['angular_velocity']['y']\n        imu.angular_velocity.z = imu_json['angular_velocity']['z']\n        imu.linear_acceleration.x = imu_json['linear_acceleration']['x']\n        imu.linear_acceleration.y = imu_json['linear_acceleration']['y']\n        imu.linear_acceleration.z = imu_json['linear_acceleration']['z']\n        for i in range(9):\n            imu.orientation_covariance[i] = imu_json['orientation_covariance'][i]\n            imu.angular_velocity_covariance[i] = imu_json['angular_velocity_covariance'][i]\n            imu.linear_acceleration_covariance[i] = imu_json['linear_acceleration_covariance'][i]\n        return imu\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_imu_type/test/src/nitros_imu_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_imu_type/nitros_imu.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_imu_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_imu\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosImuForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosImuForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosImu>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosImuForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_imu_type/test/test_cases/nitros_imu/profile/imu.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"imu_header\",\n    \"stamp\": {\n      \"sec\": 22125,\n      \"nanosec\": 45172\n    }\n  },\n  \"orientation\": {\n    \"x\": 0.3728217,\n    \"y\": 0.0,\n    \"z\": 0.3728217,\n    \"w\": 0.8497105\n  },\n  \"orientation_covariance\": [\n    1.1,\n    1.2,\n    1.3,\n    1.4,\n    1.5,\n    1.6,\n    1.7,\n    1.8,\n    1.9\n  ],\n  \"angular_velocity\": {\n    \"x\": 2.0,\n    \"y\": 2.1,\n    \"z\": 2.2\n  },\n  \"angular_velocity_covariance\": [\n    2.3,\n    2.4,\n    2.5,\n    2.6,\n    2.7,\n    2.8,\n    2.9,\n    3.0,\n    3.1\n  ],\n  \"linear_acceleration\": {\n    \"x\": 3.2,\n    \"y\": 3.3,\n    \"z\": 3.4\n  },\n  \"linear_acceleration_covariance\": [\n    3.5,\n    3.6,\n    3.7,\n    3.8,\n    3.9,\n    4.0,\n    4.1,\n    4.2,\n    4.3\n  ]\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_occupancy_grid_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_occupancy_grid_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(yaml-cpp)\n\n# NitrosOccupancyGrid\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_occupancy_grid.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosOccupancyGridForwardNode\n  ament_auto_add_library(nitros_occupancy_grid_forward_node SHARED test/src/nitros_occupancy_grid_forward_node.cpp)\n  target_link_libraries(nitros_occupancy_grid_forward_node ${PROJECT_NAME})\n  set_target_properties(nitros_occupancy_grid_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(nitros_occupancy_grid_forward_node \"nvidia::isaac_ros::nitros::NitrosOccupancyGridForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosOccupancyGridForwardNode;\\\n      $<TARGET_FILE:nitros_occupancy_grid_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_occupancy_grid_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_occupancy_grid_type/include/isaac_ros_nitros_occupancy_grid_type/nitros_occupancy_grid.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_OCCUPANCY_GRID_TYPE__NITROS_OCCUPANCY_GRID_HPP_\n#define ISAAC_ROS_NITROS_OCCUPANCY_GRID_TYPE__NITROS_OCCUPANCY_GRID_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosOccupancyGrid\n *   ROS type:    nav_msgs::msg::OccupancyGrid\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"nav_msgs/msg/occupancy_grid.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosOccupancyGrid;\n\n// Formats\nstruct nitros_occupancy_grid_t\n{\n  using MsgT = NitrosOccupancyGrid;\n  static const inline std::string supported_type_name = \"nitros_occupancy_grid\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosOccupancyGrid)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_occupancy_grid_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/multimedia/libgxf_multimedia.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosOccupancyGrid,\n  nav_msgs::msg::OccupancyGrid>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosOccupancyGrid;\n  using ros_message_type = nav_msgs::msg::OccupancyGrid;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosOccupancyGrid,\n  nav_msgs::msg::OccupancyGrid);\n\n#endif  // ISAAC_ROS_NITROS_OCCUPANCY_GRID_TYPE__NITROS_OCCUPANCY_GRID_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_occupancy_grid_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_occupancy_grid_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS Nitros Occupancy Grid Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Jaiveer Singh</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>nav_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_occupancy_grid_type/src/nitros_occupancy_grid.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <cuda_runtime.h>\n\n#include <string>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n#include \"isaac_ros_nitros_occupancy_grid_type/nitros_occupancy_grid.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nconstexpr char kEntityName[] = \"memory_pool\";\nconstexpr char kComponentName[] = \"unbounded_allocator\";\nconstexpr char kComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\n\nconstexpr int kExpectedPoseAsTensorSize = (3 + 4);  // Translation (XYZ) and Rotation (XYZW)\n\nconstexpr char kResolutionName[] = \"resolution\";\nconstexpr char kWidthName[] = \"width\";\nconstexpr char kHeightName[] = \"height\";\nconstexpr char kOriginName[] = \"origin\";\nconstexpr char kDataName[] = \"data\";\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosOccupancyGrid,\n  nav_msgs::msg::OccupancyGrid>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosOccupancyGrid::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosOccupancyGrid\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  // Primitive metadata\n  auto maybe_gxf_resolution = msg_entity->get<float>(kResolutionName);\n  if (!maybe_gxf_resolution) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get the resolution: \" <<\n      GxfResultStr(maybe_gxf_resolution.error());\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  destination.info.resolution = *maybe_gxf_resolution.value().get();\n\n  auto maybe_gxf_width = msg_entity->get<int>(kWidthName);\n  if (!maybe_gxf_width) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get the width: \" <<\n      GxfResultStr(maybe_gxf_width.error());\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  destination.info.width = *maybe_gxf_width.value().get();\n\n  auto maybe_gxf_height = msg_entity->get<int>(kHeightName);\n  if (!maybe_gxf_height) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get the height: \" <<\n      GxfResultStr(maybe_gxf_height.error());\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  destination.info.height = *maybe_gxf_height.value().get();\n\n  auto maybe_gxf_pose_tensor = msg_entity->get<nvidia::gxf::Tensor>(kOriginName);\n  if (!maybe_gxf_pose_tensor) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get the origin: \" <<\n      GxfResultStr(maybe_gxf_pose_tensor.error());\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto gxf_pose_tensor = maybe_gxf_pose_tensor.value();\n\n  if (gxf_pose_tensor->shape() != nvidia::gxf::Shape{kExpectedPoseAsTensorSize}) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Pose Tensor was not the correct shape: \" <<\n      gxf_pose_tensor->size();\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Allocate array for copying GXF Pose Tensor data as contiguous block\n  std::array<double, kExpectedPoseAsTensorSize> ros_pose_tensor{};\n\n  // Copy pose tensor off device to CPU memory\n  switch (gxf_pose_tensor->storage_type()) {\n    case nvidia::gxf::MemoryStorageType::kHost:\n      {\n        std::memcpy(ros_pose_tensor.data(), gxf_pose_tensor->pointer(), gxf_pose_tensor->size());\n      }\n      break;\n    case nvidia::gxf::MemoryStorageType::kDevice:\n      {\n        const cudaError_t cuda_error = cudaMemcpyAsync(\n          ros_pose_tensor.data(), gxf_pose_tensor->pointer(),\n          gxf_pose_tensor->size(), cudaMemcpyDeviceToHost, source.cuda_stream);\n        if (cuda_error != cudaSuccess) {\n          std::stringstream error_msg;\n          error_msg <<\n            \"[convert_to_ros_message] cudaMemcpy failed for conversion from \"\n            \"gxf::Tensor to ROS Pose: \" <<\n            cudaGetErrorName(cuda_error) <<\n            \" (\" << cudaGetErrorString(cuda_error) << \")\";\n          RCLCPP_ERROR(\n            rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n          throw std::runtime_error(error_msg.str().c_str());\n        }\n        auto cuda_result = cudaStreamSynchronize(source.cuda_stream);\n        CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n      }\n      break;\n    default:\n      std::string error_msg =\n        \"[convert_to_ros_message] MemoryStorageType not supported: conversion from \"\n        \"gxf::Tensor to ROS Pose failed!\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.c_str());\n      throw std::runtime_error(error_msg.c_str());\n  }\n\n  // Populate the message object's origin fields\n  destination.info.origin.position.x = ros_pose_tensor.at(0);\n  destination.info.origin.position.y = ros_pose_tensor.at(1);\n  destination.info.origin.position.z = ros_pose_tensor.at(2);\n\n  destination.info.origin.orientation.x = ros_pose_tensor.at(3);\n  destination.info.origin.orientation.y = ros_pose_tensor.at(4);\n  destination.info.origin.orientation.z = ros_pose_tensor.at(5);\n  destination.info.origin.orientation.w = ros_pose_tensor.at(6);\n\n  auto maybe_gxf_data_tensor = msg_entity->get<nvidia::gxf::Tensor>(kDataName);\n  if (!maybe_gxf_data_tensor) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get the data: \" <<\n      GxfResultStr(maybe_gxf_data_tensor.error());\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto gxf_data_tensor = maybe_gxf_data_tensor.value();\n\n  // Resize before copying GXF cell data as contiguous block\n  destination.data.resize(gxf_data_tensor->size());\n\n  // Copy data tensor off device to CPU memory\n  switch (gxf_data_tensor->storage_type()) {\n    case nvidia::gxf::MemoryStorageType::kHost:\n      {\n        std::memcpy(destination.data.data(), gxf_data_tensor->pointer(), gxf_data_tensor->size());\n      }\n      break;\n    case nvidia::gxf::MemoryStorageType::kDevice:\n      {\n        const cudaError_t cuda_error = cudaMemcpyAsync(\n          destination.data.data(), gxf_data_tensor->pointer(),\n          gxf_data_tensor->size(), cudaMemcpyDeviceToHost, source.cuda_stream);\n        if (cuda_error != cudaSuccess) {\n          std::stringstream error_msg;\n          error_msg <<\n            \"[convert_to_ros_message] cudaMemcpy failed for conversion from \"\n            \"gxf::Tensor to ROS int8 array: \" <<\n            cudaGetErrorName(cuda_error) <<\n            \" (\" << cudaGetErrorString(cuda_error) << \")\";\n          RCLCPP_ERROR(\n            rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n          throw std::runtime_error(error_msg.str().c_str());\n        }\n        auto cuda_result = cudaStreamSynchronize(source.cuda_stream);\n        CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n      }\n      break;\n    default:\n      std::string error_msg =\n        \"[convert_to_ros_message] MemoryStorageType not supported: conversion from \"\n        \"gxf::Tensor to ROS int8 array failed!\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.c_str());\n      throw std::runtime_error(error_msg.c_str());\n  }\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!input_timestamp) {    // Fallback to label 'timestamp'\n    input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>();\n  }\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp.value()->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp.value()->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Set frame ID\n  destination.header.frame_id = source.frame_id;\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosOccupancyGrid\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosOccupancyGrid,\n  nav_msgs::msg::OccupancyGrid>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosOccupancyGrid::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosOccupancyGrid\"),\n    \"[convert_to_custom] Conversion started\");\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  auto message = nvidia::gxf::Entity::New(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext());\n  if (!message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error initializing new message entity: \" <<\n      GxfResultStr(message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Primitive metadata\n  auto gxf_resolution = message->add<float>(kResolutionName);\n  *gxf_resolution.value() = source.info.resolution;\n\n  auto gxf_width = message->add<int>(kWidthName);\n  *gxf_width.value() = source.info.width;\n\n  auto gxf_height = message->add<int>(kHeightName);\n  *gxf_height.value() = source.info.height;\n\n  // Initializing GXF tensor\n  auto gxf_pose_tensor = message->add<nvidia::gxf::Tensor>(kOriginName);\n  auto result = gxf_pose_tensor.value()->reshape<double>(\n    nvidia::gxf::Shape{kExpectedPoseAsTensorSize}, nvidia::gxf::MemoryStorageType::kDevice,\n    allocator_handle);\n\n  if (!result) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error initializing GXF pose tensor of shape (\" <<\n      kExpectedPoseAsTensorSize << \",): \" <<\n      GxfResultStr(result.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Get pointer to first element of pose tensor\n  auto maybe_pose_data = gxf_pose_tensor.value()->data<double>();\n  if (!maybe_pose_data) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Could not get data pointer from Pose Tensor: \" <<\n      gxf_pose_tensor.value();\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Allocate array for copying ROS Pose data as contiguous block\n  std::array<double, kExpectedPoseAsTensorSize> ros_pose_tensor{\n    source.info.origin.position.x,\n    source.info.origin.position.y,\n    source.info.origin.position.z,\n\n    source.info.origin.orientation.x,\n    source.info.origin.orientation.y,\n    source.info.origin.orientation.z,\n    source.info.origin.orientation.w\n  };\n  auto nitros_cuda_stream =\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext()\n    .getCudaStreamFromNitrosGraph();\n  // Populate ROS Pose data into GXF Tensor\n  cudaError_t cuda_error = cudaMemcpyAsync(\n    gxf_pose_tensor.value()->pointer(),\n    ros_pose_tensor.data(),\n    gxf_pose_tensor.value()->size(),\n    cudaMemcpyHostToDevice,\n    nitros_cuda_stream\n  );\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] cudaMemcpy failed for copying data from \"\n      \"ROS Pose Tensor to GXF Tensor: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPoseArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  auto cuda_result = cudaStreamSynchronize(nitros_cuda_stream);\n  CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n\n  // Initializing GXF tensor\n  auto gxf_data_tensor = message->add<nvidia::gxf::Tensor>(kDataName);\n  result = gxf_data_tensor.value()->reshape<int>(\n    nvidia::gxf::Shape{static_cast<int>(source.data.size())},\n    nvidia::gxf::MemoryStorageType::kDevice,\n    allocator_handle);\n\n  if (!result) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error initializing GXF data tensor of shape (\" <<\n      source.data.size() << \",): \" <<\n      GxfResultStr(result.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Get pointer to first element of data tensor\n  auto maybe_data = gxf_data_tensor.value()->data<int>();\n  if (!maybe_data) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Could not get data pointer from data tensor: \" <<\n      gxf_data_tensor.value();\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  // Populate ROS data into GXF Tensor\n  cuda_error = cudaMemcpyAsync(\n    gxf_data_tensor.value()->pointer(),\n    source.data.data(),\n    gxf_data_tensor.value()->size(),\n    cudaMemcpyHostToDevice,\n    nitros_cuda_stream\n  );\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] cudaMemcpy failed for copying data from \"\n      \"ROS data tensor to GXF tensor: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  cuda_result = cudaStreamSynchronize(nitros_cuda_stream);\n  CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  auto output_timestamp = message->add<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!output_timestamp) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Failed to add a timestamp component to message: \" <<\n      GxfResultStr(output_timestamp.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOccupancyGrid\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  output_timestamp.value()->acqtime = input_timestamp;\n\n  // Set frame ID\n  destination.frame_id = source.header.frame_id;\n\n  // Set message entity\n  destination.handle = message->eid();\n  GxfEntityRefCountInc(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), message->eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosOccupancyGrid\"),\n    \"[convert_to_custom] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_occupancy_grid_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_occupancy_grid_type/test/isaac_ros_nitros_occupancy_grid_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosOccupancyGrid type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nfrom nav_msgs.msg import OccupancyGrid\n\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosOccupancyGridTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_occupancy_grid_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosOccupancyGridForwardNode',\n                name='NitrosOccupancyGridForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_occupancy_grid'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosOccupancyGridTest.generate_test_description(\n        [container],\n        node_startup_delay=2.5\n    )\n\n\nclass IsaacROSNitrosOccupancyGridTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosOccupancyGrid type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case()\n    def test_nitros_occupancy_grid_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the occupancy grid from NitrosOccupancyGrid type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', OccupancyGrid)],\n            received_messages=received_messages\n        )\n\n        occupancy_grid_pub = self.node.create_publisher(\n            OccupancyGrid, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            occupancy_grid: OccupancyGrid = JSONConversion.load_occupancy_grid_from_json(\n                test_folder / 'occupancy_grid.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 2\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                occupancy_grid.header.stamp = timestamp\n\n                occupancy_grid_pub.publish(occupancy_grid)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_occupancy_grid = received_messages['output']\n\n            # Header\n            # Frame ID is to be passed from NitrosOccupancyGrid to ROS message\n            # self.assertEqual(\n            #   occupancy_grid.header.frame_id, received_occupancy_grid.header.frame_id)\n\n            # MapMetadata\n            self.assertEqual(occupancy_grid.info.resolution,\n                             received_occupancy_grid.info.resolution, 'Resolution does not match')\n            self.assertEqual(occupancy_grid.info.width,\n                             received_occupancy_grid.info.width, 'Width does not match')\n            self.assertEqual(occupancy_grid.info.height,\n                             received_occupancy_grid.info.height, 'Height does not match')\n            self.assertEqual(\n                round(occupancy_grid.info.origin.position.x, 2),\n                round(received_occupancy_grid.info.origin.position.x, 2),\n                'Position x value does not match')\n            self.assertEqual(\n                round(occupancy_grid.info.origin.position.y, 2),\n                round(received_occupancy_grid.info.origin.position.y, 2),\n                'Position y value does not match')\n            self.assertEqual(\n                round(occupancy_grid.info.origin.position.z, 2),\n                round(received_occupancy_grid.info.origin.position.z, 2),\n                'Position z value does not match')\n\n            self.assertEqual(\n                round(occupancy_grid.info.origin.orientation.x, 2),\n                round(received_occupancy_grid.info.origin.orientation.x, 2),\n                'Orientation x value does not match')\n            self.assertEqual(\n                round(occupancy_grid.info.origin.orientation.y, 2),\n                round(received_occupancy_grid.info.origin.orientation.y, 2),\n                'Orientation x value does not match')\n            self.assertEqual(\n                round(occupancy_grid.info.origin.orientation.z, 2),\n                round(received_occupancy_grid.info.origin.orientation.z, 2),\n                'Orientation x value does not match')\n            self.assertEqual(\n                round(occupancy_grid.info.origin.orientation.w, 2),\n                round(received_occupancy_grid.info.origin.orientation.w, 2),\n                'Orientation w value does not match')\n\n            # data\n            for cell, received_cell in zip(occupancy_grid.data, received_occupancy_grid.data):\n                self.assertEqual(cell, received_cell, 'Occupancy data does not match')\n\n            print('The received occupancy grid is verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(occupancy_grid_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_occupancy_grid_type/test/src/nitros_occupancy_grid_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_occupancy_grid_type/nitros_occupancy_grid.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_occupancy_grid_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_occupancy_grid\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosOccupancyGridForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosOccupancyGridForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosOccupancyGrid>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosOccupancyGridForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_occupancy_grid_type/test/test_cases/profile/occupancy_grid.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"tf_camera\"\n  },\n  \"info\": {\n    \"resolution\": 1.0,\n    \"width\": 5,\n    \"height\": 2,\n    \"origin\": {\n      \"position\": {\n        \"x\": 1.0,\n        \"y\": 2.0,\n        \"z\": 3.0\n      },\n      \"orientation\": {\n        \"x\": 0.0,\n        \"y\": 0.0,\n        \"z\": 0.0,\n        \"w\": 1.0\n      }\n    }\n  },\n  \"data\": [\n    1,\n    2,\n    3,\n    4,\n    5,\n    6,\n    7,\n    8,\n    9,\n    10\n  ]\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_odometry_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_odometry_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n  add_definitions(-DUSE_NVTX)\n  link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n  link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(yaml-cpp)\n\n# NitrosOdometry\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_odometry.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosOdometryForwardNode\n  ament_auto_add_library(isaac_ros_nitros_odometry_forward_node SHARED test/src/nitros_odometry_forward_node.cpp)\n  target_link_libraries(isaac_ros_nitros_odometry_forward_node ${PROJECT_NAME})\n  set_target_properties(isaac_ros_nitros_odometry_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(isaac_ros_nitros_odometry_forward_node\n    \"nvidia::isaac_ros::nitros::NitrosOdometryForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosOdometryForwardNode;\\\n    $<TARGET_FILE:isaac_ros_nitros_odometry_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_odometry_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_odometry_type/include/isaac_ros_nitros_odometry_type/nitros_odometry.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_ODOMETRY_TYPE__NITROS_ODOMETRY_HPP_\n#define ISAAC_ROS_NITROS_ODOMETRY_TYPE__NITROS_ODOMETRY_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosOdometry\n *   ROS type:    nav_msgs::msg::Odometry\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"nav_msgs/msg/odometry.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosOdometry;\n\n// Formats\nstruct nitros_odometry_t\n{\n  using MsgT = NitrosOdometry;\n  static const inline std::string supported_type_name = \"nitros_odometry\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosOdometry)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_odometry_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\n// for nvidia::isaac::StringProvider\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_gxf_helpers\", \"gxf/lib/libgxf_isaac_gxf_helpers.so\")\n// for nvidia::isaac::SightReceiver\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_sight\", \"gxf/lib/libgxf_isaac_sight.so\")\n// for nvidia::isaac::PoseTree\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_atlas\", \"gxf/lib/libgxf_isaac_atlas.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_messages\", \"gxf/lib/libgxf_isaac_messages.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosOdometry,\n  nav_msgs::msg::Odometry>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosOdometry;\n  using ros_message_type = nav_msgs::msg::Odometry;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosOdometry,\n  nav_msgs::msg::Odometry);\n\n#endif  // ISAAC_ROS_NITROS_ODOMETRY_TYPE__NITROS_ODOMETRY_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_odometry_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_odometry_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS NITROS Odometry Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Ashwin Varghese Kuruttukulam</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>nav_msgs</depend>\n  <depend>tf2_ros</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>gxf_isaac_messages</depend>\n  <depend>gxf_isaac_atlas</depend>\n  <depend>gxf_isaac_gxf_helpers</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>gxf_isaac_gems</build_depend>\n\n  <exec_depend>gxf_isaac_sight</exec_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_odometry_type/src/nitros_odometry.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"extensions/messages/helpers.hpp\"\n#include \"extensions/messages/composite_message.hpp\"\n#include \"gems/control_types/differential_drive.hpp\"\n#include \"gems/pose_tree/pose_tree.hpp\"\n#include \"extensions/atlas/pose_tree_frame.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_odometry_type/nitros_odometry.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n#include \"tf2/LinearMath/Quaternion.h\"\n#include \"tf2/LinearMath/Transform.h\"\n\nnamespace\n{\nconstexpr int kPositionXIndx = 0;\nconstexpr int kPositionYIndx = 1;\nconstexpr int kHeadingIndx = 2;\nconstexpr int kLinearSpeedIndx = 3;\nconstexpr int kAngularSpeedIndx = 4;\nconstexpr int DifferentialBaseEgoMotionIndices = 7;\nconstexpr char kPoseTreeEntityName[] = \"global_pose_tree\";\nconstexpr char kPoseTreeComponentName[] = \"pose_tree\";\nconstexpr char kPoseTreeComponentTypeName[] = \"nvidia::isaac::PoseTree\";\n}  // namespace\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosOdometry,\n  nav_msgs::msg::Odometry>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosOdometry::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosOdometry\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  // Create composite msg and populate values\n  auto maybe_composite_message = nvidia::isaac::GetCompositeMessage(msg_entity.value());\n  if (!maybe_composite_message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get maybe_composite_message gxf message\"\n      \" from message entity: \" <<\n      GxfResultStr(maybe_composite_message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOdometry\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto composite_message = maybe_composite_message.value();\n\n  // Populate odometry data of the ROS msg from gxf msg\n  destination.pose.pose.position.x = composite_message.view(0, kPositionXIndx);\n  destination.pose.pose.position.y = composite_message.view(0, kPositionYIndx);\n  tf2::Quaternion quaternion;\n  quaternion.setRPY(0, 0, composite_message.view(0, kHeadingIndx));\n  destination.pose.pose.orientation.x = quaternion.getX();\n  destination.pose.pose.orientation.y = quaternion.getY();\n  destination.pose.pose.orientation.z = quaternion.getZ();\n  destination.pose.pose.orientation.w = quaternion.getW();\n  destination.twist.twist.linear.x = composite_message.view(0, kLinearSpeedIndx);\n  destination.twist.twist.angular.z = composite_message.view(0, kAngularSpeedIndx);\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = composite_message.timestamp;\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Get pointer to posetree component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kPoseTreeEntityName, kPoseTreeComponentName, kPoseTreeComponentTypeName, cid);\n  auto maybe_pose_tree_handle =\n    nvidia::gxf::Handle<nvidia::isaac::PoseTree>::Create(context, cid);\n  if (!maybe_pose_tree_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get pose tree's handle: \" <<\n      GxfResultStr(maybe_pose_tree_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOdometry\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto pose_tree_handle = maybe_pose_tree_handle.value();\n  auto frame_name = pose_tree_handle->getFrameName(composite_message.pose_frame_uid->uid);\n  if (frame_name) {\n    destination.header.frame_id = frame_name.value();\n  } else {\n    RCLCPP_DEBUG(\n      rclcpp::get_logger(\"NitrosOdometry\"), \"Setting frame if from NITROS msg\");\n    // Set NITROS frame id as fallback method of populating frame_id\n    // Set frame ID\n    destination.header.frame_id = source.frame_id;\n  }\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosOdometry\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosOdometry,\n  nav_msgs::msg::Odometry>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosOdometry::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosOdometry\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // Get pointer to allocator component\n  const std::string kAllocatorEntityName = \"memory_pool\";\n  const std::string kAllocatorComponentName = \"unbounded_allocator\";\n  const std::string kAllocatorComponentTypeName = \"nvidia::gxf::UnboundedAllocator\";\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kAllocatorEntityName, kAllocatorComponentName, kAllocatorComponentTypeName, cid);\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOdometry\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  // Create composite msg and populate values\n  auto maybe_composite_message = nvidia::isaac::CreateCompositeMessage(\n    context, allocator_handle, 1, DifferentialBaseEgoMotionIndices);\n  if (!maybe_composite_message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get maybe_composite_message gxf message\"\n      \" from message entity: \" <<\n      GxfResultStr(maybe_composite_message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOdometry\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto composite_message = maybe_composite_message.value();\n\n  // Get pointer to composite_schema_server component\n  const std::string kSchemaServerEntityName = \"global_pose_tree\";\n  const std::string kSchemaServerComponentName = \"composite_schema_server\";\n  const std::string kSchemaServerComponentTypeName = \"nvidia::isaac::CompositeSchemaServer\";\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kSchemaServerEntityName, kSchemaServerComponentName,\n    kSchemaServerComponentTypeName, cid);\n  auto maybe_schema_server_handle =\n    nvidia::gxf::Handle<nvidia::isaac::CompositeSchemaServer>::Create(context, cid);\n  if (!maybe_schema_server_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get maybe_schema_server_handle gxf message\"\n      \" from message entity: \" <<\n      GxfResultStr(maybe_schema_server_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOdometry\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto schema_server_handle = maybe_schema_server_handle.value();\n\n  // Add schema to server if it doesn't already exist\n  auto maybe_schema_uid = schema_server_handle->add(\n    nvidia::isaac::DifferentialBaseEgoMotionCompositeSchema());\n  if (!maybe_schema_uid) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Cannot add schema to server\" <<\n      GxfResultStr(maybe_composite_message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOdometry\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  // Set schema uid for the gxf msg\n  composite_message.composite_schema_uid->uid = maybe_schema_uid.value();\n\n  // Populate odometry data of the gxf msg from ROS msg\n  composite_message.view(0, kPositionXIndx) = source.pose.pose.position.x;\n  composite_message.view(0, kPositionYIndx) = source.pose.pose.position.y;\n  composite_message.view(0, kLinearSpeedIndx) = source.twist.twist.linear.x;\n  composite_message.view(0, kAngularSpeedIndx) = source.twist.twist.angular.z;\n  tf2::Quaternion q(\n    source.pose.pose.orientation.x,\n    source.pose.pose.orientation.y,\n    source.pose.pose.orientation.z,\n    source.pose.pose.orientation.w);\n  tf2::Matrix3x3 m(q);\n  double roll, pitch, yaw;\n  m.getRPY(roll, pitch, yaw);\n  composite_message.view(0, kHeadingIndx) = yaw;\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  composite_message.timestamp->acqtime = input_timestamp;\n\n  // Get pointer to posetree component\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kPoseTreeEntityName, kPoseTreeComponentName, kPoseTreeComponentTypeName, cid);\n  auto maybe_pose_tree_handle =\n    nvidia::gxf::Handle<nvidia::isaac::PoseTree>::Create(context, cid);\n  if (!maybe_pose_tree_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get pose tree's handle: \" <<\n      GxfResultStr(maybe_pose_tree_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosOdometry\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto pose_tree_handle = maybe_pose_tree_handle.value();\n  auto maybe_composite_msg_frame_uid = pose_tree_handle->findOrCreateFrame(\n    source.header.frame_id.c_str());\n  if (maybe_composite_msg_frame_uid) {\n    composite_message.pose_frame_uid->uid = maybe_composite_msg_frame_uid.value();\n  } else {\n    RCLCPP_WARN(\n      rclcpp::get_logger(\"NitrosOdometry\"), \"Could not create Pose Tree Frame\");\n  }\n\n  // Set Entity Id\n  destination.handle = composite_message.message.eid();\n  GxfEntityRefCountInc(context, composite_message.message.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosOdometry\"),\n    \"[convert_to_custom] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_odometry_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_odometry_type/test/isaac_ros_nitros_odometry_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosOdometry type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nfrom nav_msgs.msg import Odometry\n\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosOdometryTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_odometry_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosOdometryForwardNode',\n                name='NitrosOdometryForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_odometry'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosOdometryTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosOdometryTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosOdometry type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_odometry')\n    def test_nitros_odometry_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the tag from NitrosOdometry type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', Odometry)],\n            received_messages=received_messages\n        )\n\n        odometry_pub = self.node.create_publisher(\n            Odometry, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            odometry = self.load_odometry_from_json(\n                test_folder / 'odometry.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 10\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n\n                odometry_pub.publish(odometry)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_odometry = received_messages['output']\n\n            # Only test for data passed through gxf\n            self.assertEqual(odometry.pose.pose.position.x,\n                             received_odometry.pose.pose.position.x)\n            self.assertEqual(odometry.pose.pose.position.y,\n                             received_odometry.pose.pose.position.y)\n            self.assertEqual(odometry.pose.pose.orientation.x,\n                             received_odometry.pose.pose.orientation.x)\n            self.assertEqual(odometry.pose.pose.orientation.y,\n                             received_odometry.pose.pose.orientation.y)\n            self.assertEqual(odometry.pose.pose.orientation.z,\n                             received_odometry.pose.pose.orientation.z)\n            self.assertEqual(odometry.pose.pose.orientation.w,\n                             received_odometry.pose.pose.orientation.w)\n            self.assertEqual(odometry.twist.twist.linear.x,\n                             received_odometry.twist.twist.linear.x)\n            self.assertEqual(odometry.twist.twist.angular.z,\n                             received_odometry.twist.twist.angular.z)\n            print('The received odometry message has been verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(odometry_pub)\n\n    @staticmethod\n    def load_odometry_from_json(\n            json_filepath: pathlib.Path) -> Odometry:\n        \"\"\"\n        Load a Odometry message from a JSON filepath.\n\n        Parameters\n        ----------\n        json_filepath : Path\n            The path to a JSON file containing the Odometry fields\n\n        Returns\n        -------\n        Odometry\n            Generated Odometry message\n\n        \"\"\"\n        odometry_json = JSONConversion.load_from_json(\n            json_filepath)\n\n        odometry = Odometry()\n        odometry.header.frame_id = odometry_json[\n            'header']['frame_id']\n        odometry.header.stamp.sec = odometry_json[\n            'header']['stamp']['sec']\n        odometry.header.stamp.nanosec = odometry_json[\n            'header']['stamp']['nanosec']\n        odometry.child_frame_id = odometry_json['child_frame_id']\n        odometry.pose.pose.position.x = odometry_json[\n            'pose']['pose']['position']['x']\n        odometry.pose.pose.position.y = odometry_json[\n            'pose']['pose']['position']['y']\n        odometry.pose.pose.position.z = odometry_json[\n            'pose']['pose']['position']['z']\n        odometry.pose.pose.orientation.x = odometry_json[\n            'pose']['pose']['orientation']['x']\n        odometry.pose.pose.orientation.y = odometry_json[\n            'pose']['pose']['orientation']['y']\n        odometry.pose.pose.orientation.z = odometry_json[\n            'pose']['pose']['orientation']['z']\n        odometry.pose.pose.orientation.w = odometry_json[\n            'pose']['pose']['orientation']['w']\n        odometry.twist.twist.linear.x = odometry_json[\n            'twist']['twist']['linear']['x']\n        odometry.twist.twist.linear.y = odometry_json[\n            'twist']['twist']['linear']['y']\n        odometry.twist.twist.linear.z = odometry_json[\n            'twist']['twist']['linear']['z']\n        odometry.twist.twist.angular.x = odometry_json[\n            'twist']['twist']['angular']['x']\n        odometry.twist.twist.angular.y = odometry_json[\n            'twist']['twist']['angular']['y']\n        odometry.twist.twist.angular.z = odometry_json[\n            'twist']['twist']['angular']['z']\n        return odometry\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_odometry_type/test/src/nitros_odometry_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_odometry_type/nitros_odometry.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_odometry_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_odometry\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosOdometryForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosOdometryForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosOdometry>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosOdometryForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_odometry_type/test/test_cases/nitros_odometry/profile/odometry.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"test_frame_id\",\n    \"stamp\": {\n      \"sec\": 213,\n      \"nanosec\": 1245\n    }\n  },\n  \"child_frame_id\": \"test_child_frame_id\",\n  \"pose\": {\n    \"pose\": {\n      \"position\": {\n        \"x\": 4.0,\n        \"y\": 5.0,\n        \"z\": 6.0\n      },\n      \"orientation\": {\n        \"x\": 0.0,\n        \"y\": 0.0,\n        \"z\": 0.0,\n        \"w\": 1.0\n      }\n    }\n  },\n  \"twist\": {\n    \"twist\": {\n      \"linear\": {\n        \"x\": 6.0,\n        \"y\": 2.0,\n        \"z\": 16.0\n      },\n      \"angular\": {\n        \"x\": 1.0,\n        \"y\": 2.0,\n        \"z\": 3.0\n      }\n    }\n  }\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_point_cloud_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(yaml-cpp)\n\n# NitrosPointCloud\nament_auto_add_library(${PROJECT_NAME} SHARED\n  src/nitros_point_cloud.cpp\n  src/nitros_point_cloud_builder.cpp\n  src/nitros_point_cloud_view.cpp\n)\n\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosPointCloudForwardNode\n  ament_auto_add_library(nitros_point_cloud_forward_node SHARED test/src/nitros_point_cloud_forward_node.cpp)\n  target_link_libraries(nitros_point_cloud_forward_node ${PROJECT_NAME})\n  set_target_properties(nitros_point_cloud_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n\n  rclcpp_components_register_nodes(nitros_point_cloud_forward_node \"nvidia::isaac_ros::nitros::NitrosPointCloudForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosPointCloudForwardNode;$<TARGET_FILE:nitros_point_cloud_forward_node>\\n\")\n\n  # Find isaac_ros_managed_nitros for test node\n  # (ament_auto_find_build_dependencies only finds <depend>, not <test_depend>)\n  find_package(isaac_ros_managed_nitros REQUIRED)\n\n  # ManagedNitrosPointCloudForwardNode\n  ament_auto_add_library(managed_nitros_point_cloud_forward_node SHARED\n    test/managed_nitros_point_cloud_forward_node.cpp)\n  target_link_libraries(managed_nitros_point_cloud_forward_node ${PROJECT_NAME})\n  ament_target_dependencies(managed_nitros_point_cloud_forward_node isaac_ros_managed_nitros)\n  set_target_properties(managed_nitros_point_cloud_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n\n  rclcpp_components_register_nodes(managed_nitros_point_cloud_forward_node\n    \"nvidia::isaac_ros::nitros::ManagedNitrosPointCloudForwardNode\")\n  set(node_plugins\n    \"${node_plugins}nvidia::isaac_ros::nitros::ManagedNitrosPointCloudForwardNode;\\\n$<TARGET_FILE:managed_nitros_point_cloud_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_point_cloud_type_test_pol.py TIMEOUT \"15\")\n  add_launch_test(test/isaac_ros_managed_nitros_point_cloud_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/include/isaac_ros_nitros_point_cloud_type/nitros_point_cloud.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_POINT_CLOUD_TYPE__NITROS_POINT_CLOUD_HPP_\n#define ISAAC_ROS_NITROS_POINT_CLOUD_TYPE__NITROS_POINT_CLOUD_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosPointCloud\n *   ROS type:    sensor_msgs::msg::PointCloud2\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"sensor_msgs/msg/point_cloud2.hpp\"\n#include \"sensor_msgs/point_cloud2_iterator.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosPointCloud;\n\n// Formats\nstruct nitros_point_cloud_t\n{\n  using MsgT = NitrosPointCloud;\n  static const inline std::string supported_type_name = \"nitros_point_cloud\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosPointCloud)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_point_cloud_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_messages\", \"gxf/lib/libgxf_isaac_messages.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_ros_messages\", \"gxf/lib/libgxf_isaac_ros_messages.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_atlas\", \"gxf/lib/libgxf_isaac_atlas.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_sight\", \"gxf/lib/libgxf_isaac_sight.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosPointCloud,\n  sensor_msgs::msg::PointCloud2>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosPointCloud;\n  using ros_message_type = sensor_msgs::msg::PointCloud2;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosPointCloud,\n  sensor_msgs::msg::PointCloud2);\n\n#endif  // ISAAC_ROS_NITROS_POINT_CLOUD_TYPE__NITROS_POINT_CLOUD_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/include/isaac_ros_nitros_point_cloud_type/nitros_point_cloud_builder.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_POINT_CLOUD_TYPE__NITROS_POINT_CLOUD_BUILDER_HPP_\n#define ISAAC_ROS_NITROS_POINT_CLOUD_TYPE__NITROS_POINT_CLOUD_BUILDER_HPP_\n\n#include <cuda_runtime.h>\n#include <functional>\n\n#include \"isaac_ros_nitros_point_cloud_type/nitros_point_cloud.hpp\"\n#include \"std_msgs/msg/header.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nclass NitrosPointCloudBuilder\n{\npublic:\n  NitrosPointCloudBuilder();\n  NitrosPointCloudBuilder(const NitrosPointCloudBuilder &) = delete;\n  NitrosPointCloudBuilder & operator=(const NitrosPointCloudBuilder &) = delete;\n  NitrosPointCloudBuilder(NitrosPointCloudBuilder && other);\n  NitrosPointCloudBuilder & operator=(NitrosPointCloudBuilder && other);\n\n  NitrosPointCloudBuilder & WithHeader(const std_msgs::msg::Header & header);\n\n  NitrosPointCloudBuilder & WithPoints(\n    const void * points_data, int32_t num_points,\n    bool use_color = false);\n\n  NitrosPointCloudBuilder & WithEvent(cudaEvent_t event);\n\n  NitrosPointCloudBuilder & WithReleaseCallback(std::function<void()> release_callback);\n\n  NitrosPointCloud Build();\n\nprivate:\n  void Validate();\n  NitrosPointCloud nitros_point_cloud_{};\n  const void * points_data_{nullptr};\n  int32_t num_points_{0};\n  bool use_color_{false};\n  std_msgs::msg::Header header_{};\n  cudaEvent_t event_{};\n  std::function<void()> release_callback_{nullptr};\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n#endif  // ISAAC_ROS_NITROS_POINT_CLOUD_TYPE__NITROS_POINT_CLOUD_BUILDER_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/include/isaac_ros_nitros_point_cloud_type/nitros_point_cloud_view.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_POINT_CLOUD_TYPE__NITROS_POINT_CLOUD_VIEW_HPP_\n#define ISAAC_ROS_NITROS_POINT_CLOUD_TYPE__NITROS_POINT_CLOUD_VIEW_HPP_\n\n#include <string>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/tensor.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros/types/nitros_type_view_factory.hpp\"\n#include \"isaac_ros_nitros_point_cloud_type/nitros_point_cloud.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nNITROS_TYPE_VIEW_FACTORY_BEGIN(NitrosPointCloud)\n\nMARK_PUBLIC_SECTION()\n// Public methods\ninline uint32_t GetWidth() const {return width_;}\ninline uint32_t GetHeight() const {return height_;}\ninline uint32_t GetPointCount() const {return point_count_;}\ninline bool HasColor() const {return has_color_;}\ninline const float * GetPointsData() const {return points_data_;}\ninline uint32_t GetPointStep() const {return point_step_;}\ninline uint32_t GetRowStep() const {return row_step_;}\ninline bool IsBigEndian() const {return is_bigendian_;}\ninline bool IsDense() const {return is_dense_;}\n\nMARK_PRIVATE_SECTION()\nvoid GetPointCloudData();\nuint32_t width_{0};\nuint32_t height_{0};\nuint32_t point_count_{0};\nbool has_color_{false};\nconst float * points_data_{nullptr};\nuint32_t point_step_{0};\nuint32_t row_step_{0};\nbool is_bigendian_{false};\nbool is_dense_{false};\n\nNITROS_TYPE_VIEW_FACTORY_END(NitrosPointCloud)\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_POINT_CLOUD_TYPE__NITROS_POINT_CLOUD_VIEW_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022-2025, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_point_cloud_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS NITROS Point Cloud Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n  <author>Yuankun Zhu</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>sensor_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>gxf_isaac_messages</depend>\n  <depend>gxf_isaac_ros_messages</depend>\n\n  <build_depend>ament_index_cpp</build_depend>\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>gxf_isaac_gems</build_depend>\n\n  <exec_depend>gxf_isaac_atlas</exec_depend>\n  <exec_depend>gxf_isaac_sight</exec_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n  <test_depend>isaac_ros_managed_nitros</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/src/nitros_point_cloud.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include <cuda_runtime.h>\n\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"messages/point_cloud_message.hpp\"\n#include \"gems/pose_tree/pose_tree.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n#include \"isaac_ros_nitros_point_cloud_type/nitros_point_cloud.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nnamespace\n{\nconstexpr char kEntityName[] = \"memory_pool\";\nconstexpr char kComponentName[] = \"unbounded_allocator\";\nconstexpr char kComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\nconstexpr char kPoseTreeEntityName[] = \"global_pose_tree\";\nconstexpr char kPoseTreeComponentName[] = \"pose_tree\";\nconstexpr char kPoseTreeComponentTypeName[] = \"nvidia::isaac::PoseTree\";\n}  // namespace\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosPointCloud,\n  sensor_msgs::msg::PointCloud2>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosPointCloud::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPointCloud\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto maybe_point_cloud_parts = nvidia::isaac_ros::messages::GetPointCloudMessage(\n    msg_entity.value());\n  if (!maybe_point_cloud_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get pointcloud message from message entity: \" <<\n      GxfResultStr(maybe_point_cloud_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloud\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto point_cloud_parts = maybe_point_cloud_parts.value();\n  const int32_t n_points = point_cloud_parts.points->shape().dimension(0);\n\n  destination.height = 1;\n  destination.width = n_points;\n  destination.is_bigendian = point_cloud_parts.info->is_bigendian;\n  destination.is_dense = false;\n\n  sensor_msgs::PointCloud2Modifier pc2_modifier(destination);\n  if (point_cloud_parts.info->use_color) {\n    // Data format: x,y,z,rgb; 16 bytes per point\n    pc2_modifier.setPointCloud2Fields(\n      4,\n      \"x\", 1, sensor_msgs::msg::PointField::FLOAT32,\n      \"y\", 1, sensor_msgs::msg::PointField::FLOAT32,\n      \"z\", 1, sensor_msgs::msg::PointField::FLOAT32,\n      \"rgb\", 1, sensor_msgs::msg::PointField::FLOAT32);\n  } else {\n    // Data format: x,y,z; 12 bytes per point\n    pc2_modifier.setPointCloud2Fields(\n      3,\n      \"x\", 1, sensor_msgs::msg::PointField::FLOAT32,\n      \"y\", 1, sensor_msgs::msg::PointField::FLOAT32,\n      \"z\", 1, sensor_msgs::msg::PointField::FLOAT32);\n  }\n  const cudaError_t cuda_error = cudaMemcpyAsync(\n    destination.data.data(),\n    point_cloud_parts.points->data<float>().value(), destination.row_step * destination.height,\n    cudaMemcpyDeviceToHost, source.cuda_stream);\n\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] cudaMemcpy failed for conversion from \"\n      \"NitrosPointCloud to sensor_msgs::msg::PointCloud2: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloud\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  cudaError_t cuda_result = cudaStreamSynchronize(source.cuda_stream);\n  CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPointCloud\"),\n    \"[convert_to_ros_message] \"\n    \"row_step: %d, point_step: %d, x_offset: %d, y_offset: %d, y_offset: %d\",\n    destination.row_step,\n    destination.point_step,\n    destination.fields[0].offset,\n    destination.fields[1].offset,\n    destination.fields[2].offset);\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = point_cloud_parts.timestamp;\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Get pointer to posetree component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kPoseTreeEntityName, kPoseTreeComponentName, kPoseTreeComponentTypeName, cid);\n  auto maybe_pose_tree_handle =\n    nvidia::gxf::Handle<nvidia::isaac::PoseTree>::Create(context, cid);\n  if (!maybe_pose_tree_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get pose tree's handle: \" <<\n      GxfResultStr(maybe_pose_tree_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloud\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto pose_tree_handle = maybe_pose_tree_handle.value();\n  auto frame_name = pose_tree_handle->getFrameName(point_cloud_parts.pose_frame_uid->uid);\n  if (frame_name) {\n    destination.header.frame_id = frame_name.value();\n  } else {\n    RCLCPP_DEBUG(\n      rclcpp::get_logger(\"NitrosPointCloud\"),\n      \"Setting frame_id=%s from NITROS msg\",\n      source.frame_id.c_str());\n    // Set NITROS frame id as fallback method of populating frame_id\n    destination.header.frame_id = source.frame_id;\n  }\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPointCloud\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosPointCloud,\n  sensor_msgs::msg::PointCloud2>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosPointCloud::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPointCloud\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloud\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  const int32_t height = source.height;\n  const int32_t width = source.width;\n  const int32_t point_float_count = source.point_step / sizeof(float);\n  const bool use_color = (point_float_count == 4);  // {x, y, z, RGB}\n  const int32_t n_points = source.height * source.width;\n\n  auto maybe_point_cloud_message_parts = nvidia::isaac_ros::messages::CreatePointCloudMessage(\n    context, allocator_handle, n_points, use_color);\n  if (!maybe_point_cloud_message_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to create CreatePointCloudMessage \" << GxfResultStr(\n      maybe_point_cloud_message_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloud\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto point_cloud_message_parts = maybe_point_cloud_message_parts.value();\n\n  point_cloud_message_parts.info->use_color = use_color;\n  point_cloud_message_parts.info->is_bigendian = source.is_bigendian;\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPointCloud\"),\n    \"[convert_to_custom] height: %d, width: %d, point_float_count: %d, use_color: %d\",\n    height, width, point_float_count, use_color);\n\n  // Copy data from point cloud msg to gxf tensor.\n  auto nitros_cuda_stream =\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext()\n    .getCudaStreamFromNitrosGraph();\n  const cudaError_t cuda_error = cudaMemcpyAsync(\n    point_cloud_message_parts.points->data<float>().value(),\n    source.data.data(), source.row_step * source.height, cudaMemcpyHostToDevice,\n    nitros_cuda_stream);\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] cudaMemcpy failed for copying data from \"\n      \"sensor_msgs::msg::PointCloud2 to NitrosPointCloud: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloud\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  cudaError_t cuda_result = cudaStreamSynchronize(nitros_cuda_stream);\n  CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  point_cloud_message_parts.timestamp->acqtime = input_timestamp;\n\n  // Get pointer to posetree component\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kPoseTreeEntityName, kPoseTreeComponentName, kPoseTreeComponentTypeName, cid);\n  auto maybe_pose_tree_handle =\n    nvidia::gxf::Handle<nvidia::isaac::PoseTree>::Create(context, cid);\n  if (!maybe_pose_tree_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get pose tree's handle: \" <<\n      GxfResultStr(maybe_pose_tree_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloud\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto pose_tree_handle = maybe_pose_tree_handle.value();\n  auto maybe_pointcloud_frame_uid = pose_tree_handle->findOrCreateFrame(\n    source.header.frame_id.c_str());\n\n  if (maybe_pointcloud_frame_uid) {\n    point_cloud_message_parts.pose_frame_uid->uid = maybe_pointcloud_frame_uid.value();\n  } else {\n    RCLCPP_WARN(\n      rclcpp::get_logger(\"NitrosPointCloud\"), \"Could not create Pose Tree Frame\");\n  }\n\n  // Set NITROS frame id as fallback method of populating frame_id\n  destination.frame_id = source.header.frame_id;\n\n  // Set Entity Id\n  destination.handle = point_cloud_message_parts.message.eid();\n  GxfEntityRefCountInc(context, point_cloud_message_parts.message.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPointCloud\"),\n    \"[convert_to_custom] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/src/nitros_point_cloud_builder.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_point_cloud_type/nitros_point_cloud_builder.hpp\"\n\n#include <sstream>\n\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/allocator.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#include \"messages/point_cloud_message.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"rclcpp/rclcpp.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nNitrosPointCloudBuilder::NitrosPointCloudBuilder()\n: nitros_point_cloud_{}\n{\n  // NOTE: we defer creation of the msg until the Build function\n  // which is done to use CreatePointCloudMessage\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPointCloudBuilder\"),\n    \"[constructor] NitrosPointCloudBuilder initialized\");\n}\n\nNitrosPointCloudBuilder::NitrosPointCloudBuilder(NitrosPointCloudBuilder && other)\n{\n  nitros_point_cloud_ = other.nitros_point_cloud_;\n  points_data_ = other.points_data_;\n  num_points_ = other.num_points_;\n  use_color_ = other.use_color_;\n  header_ = other.header_;\n  event_ = other.event_;\n  release_callback_ = other.release_callback_;\n\n  // Reset other\n  other.points_data_ = nullptr;\n  other.num_points_ = 0;\n  other.use_color_ = false;\n  other.event_ = {};\n  other.release_callback_ = nullptr;\n}\n\nNitrosPointCloudBuilder & NitrosPointCloudBuilder::operator=(NitrosPointCloudBuilder && other)\n{\n  // In case other is this, then nothing should be done.\n  if (&other == this) {\n    return *this;\n  }\n  nitros_point_cloud_ = other.nitros_point_cloud_;\n  points_data_ = other.points_data_;\n  num_points_ = other.num_points_;\n  use_color_ = other.use_color_;\n  header_ = other.header_;\n  event_ = other.event_;\n  release_callback_ = other.release_callback_;\n\n  // Reset other\n  other.points_data_ = nullptr;\n  other.num_points_ = 0;\n  other.use_color_ = false;\n  other.event_ = {};\n  other.release_callback_ = nullptr;\n\n  return *this;\n}\n\nvoid NitrosPointCloudBuilder::Validate()\n{\n  bool failure = false;\n  std::stringstream error_msg;\n  if (num_points_ <= 0) {\n    error_msg << \"Points count is not set. Call WithPoints method before Build.\";\n    failure = true;\n  }\n  if (points_data_ == nullptr) {\n    if (failure) {\n      error_msg << \" \";\n    }\n    error_msg << \"Points data is not set. Call WithPoints method before Build.\";\n    failure = true;\n  }\n\n  if (failure) {\n    RCLCPP_ERROR(rclcpp::get_logger(\"NitrosPointCloudBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n}\n\nNitrosPointCloudBuilder & NitrosPointCloudBuilder::WithHeader(const std_msgs::msg::Header & header)\n{\n  header_ = header;\n  return *this;\n}\n\nNitrosPointCloudBuilder & NitrosPointCloudBuilder::WithPoints(\n  const void * points_data, int32_t num_points, bool use_color)\n{\n  if (num_points <= 0) {\n    throw std::invalid_argument(\n      \"[WithPoints] num_points must be greater than zero\");\n  }\n  if (points_data == nullptr) {\n    throw std::invalid_argument(\n      \"[WithPoints] points_data must not be null\");\n  }\n  points_data_ = points_data;\n  num_points_ = num_points;\n  use_color_ = use_color;\n\n  return *this;\n}\n\nNitrosPointCloudBuilder & NitrosPointCloudBuilder::WithEvent(cudaEvent_t event)\n{\n  event_ = event;\n  return *this;\n}\n\nNitrosPointCloudBuilder & NitrosPointCloudBuilder::WithReleaseCallback(\n  std::function<void()> release_callback)\n{\n  release_callback_ = release_callback;\n  return *this;\n}\n\nNitrosPointCloud NitrosPointCloudBuilder::Build()\n{\n  // Validate all data is present before building the NitrosPointCloud\n  Validate();\n\n  // If CUDA event provided, synchronize on that event before building\n  if (event_) {\n    cudaError_t cuda_error = cudaEventSynchronize(event_);\n\n    if (cuda_error != cudaSuccess) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[Build] cudaEventSynchronize failed: \" <<\n        cudaGetErrorName(cuda_error) <<\n        \" (\" << cudaGetErrorString(cuda_error) << \")\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosPointCloudBuilder\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n\n    cuda_error = cudaEventDestroy(event_);\n    if (cuda_error != cudaSuccess) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[Build] cudaEventDestroy failed: \" <<\n        cudaGetErrorName(cuda_error) <<\n        \" (\" << cudaGetErrorString(cuda_error) << \")\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosPointCloudBuilder\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n\n  // Get allocator handle\n  gxf_uid_t cid;\n  GetTypeAdapterNitrosContext().getCid(\n    \"memory_pool\", \"unbounded_allocator\", \"nvidia::gxf::UnboundedAllocator\", cid);\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(\n      GetTypeAdapterNitrosContext().getContext(), cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[Build] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloudBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  // Create point cloud message\n  auto maybe_point_cloud_message_parts = nvidia::isaac_ros::messages::CreatePointCloudMessage(\n    GetTypeAdapterNitrosContext().getContext(), allocator_handle, num_points_, use_color_);\n  if (!maybe_point_cloud_message_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[Build] Failed to create point cloud message: \" << GxfResultStr(\n      maybe_point_cloud_message_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloudBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto point_cloud_message_parts = maybe_point_cloud_message_parts.value();\n\n  // Set point cloud info\n  point_cloud_message_parts.info->use_color = use_color_;\n  point_cloud_message_parts.info->is_bigendian = false;\n\n  // Apply header (timestamp and frame_id)\n  point_cloud_message_parts.timestamp->acqtime =\n    header_.stamp.sec * static_cast<uint64_t>(1e9) + header_.stamp.nanosec;\n  nitros_point_cloud_.frame_id = header_.frame_id;\n\n  // Copy data to the tensor\n  auto nitros_cuda_stream =\n    GetTypeAdapterNitrosContext().getCudaStreamFromNitrosGraph();\n\n  const size_t data_size = num_points_ * (use_color_ ? 4 : 3) * sizeof(float);\n\n  auto maybe_dst = point_cloud_message_parts.points->data<float>();\n  if (!maybe_dst) {\n    throw std::runtime_error(\"[Build] Tensor returned no data pointer\");\n  }\n\n  const cudaError_t cuda_error = cudaMemcpyAsync(\n    *maybe_dst,\n    points_data_, data_size, cudaMemcpyDeviceToDevice,\n    nitros_cuda_stream);\n  if (cuda_error != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[Build] cudaMemcpyAsync failed for copying point cloud data: \" <<\n      cudaGetErrorName(cuda_error) <<\n      \" (\" << cudaGetErrorString(cuda_error) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloudBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  cudaError_t cuda_result = cudaStreamSynchronize(nitros_cuda_stream);\n  if (cuda_result != cudaSuccess) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[Build] Stream was not able to be synchronized: \" <<\n      cudaGetErrorName(cuda_result) <<\n      \" (\" << cudaGetErrorString(cuda_result) << \")\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloudBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  nitros_point_cloud_.handle = point_cloud_message_parts.message.eid();\n  GxfEntityRefCountInc(\n    GetTypeAdapterNitrosContext().getContext(),\n    point_cloud_message_parts.message.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPointCloudBuilder\"),\n    \"[Build] Point cloud built with %d points, use_color: %d\", num_points_, use_color_);\n\n  // Free the input buffer since we copied the data\n  if (release_callback_) {\n    release_callback_();\n    RCLCPP_DEBUG(\n      rclcpp::get_logger(\"NitrosPointCloudBuilder\"),\n      \"[Build] Release callback invoked to free input buffer\");\n  } else {\n    // Default: free GPU memory with cudaFree\n    cudaError_t free_error = cudaFree(const_cast<void *>(points_data_));\n    if (free_error != cudaSuccess) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[Build] cudaFree failed: \" <<\n        cudaGetErrorName(free_error) <<\n        \" (\" << cudaGetErrorString(free_error) << \")\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosPointCloudBuilder\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n    RCLCPP_DEBUG(\n      rclcpp::get_logger(\"NitrosPointCloudBuilder\"),\n      \"[Build] Input buffer freed with cudaFree\");\n  }\n\n  // Resetting data after it is done building\n  points_data_ = nullptr;\n  return nitros_point_cloud_;\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/src/nitros_point_cloud_view.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_point_cloud_type/nitros_point_cloud_view.hpp\"\n\n#include <sstream>\n\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n#include \"messages/point_cloud_message.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nvoid NitrosPointCloudView::InitView()\n{\n  GetPointCloudData();\n}\n\nvoid NitrosPointCloudView::GetPointCloudData()\n{\n  auto maybe_point_cloud_parts = nvidia::isaac_ros::messages::GetPointCloudMessage(\n    msg_entity_.value());\n  if (!maybe_point_cloud_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[GetPointCloudData] Failed to get pointcloud message from message entity: \" <<\n      GxfResultStr(maybe_point_cloud_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloudView\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto point_cloud_parts = maybe_point_cloud_parts.value();\n\n  // Extract point cloud data\n  height_ = 1;\n  width_ = static_cast<uint32_t>(point_cloud_parts.points->shape().dimension(0));\n  point_count_ = width_ * height_;\n  has_color_ = point_cloud_parts.info->use_color;\n  is_bigendian_ = point_cloud_parts.info->is_bigendian;\n  is_dense_ = false;\n\n  // Calculate strides\n  point_step_ = has_color_ ? 16 : 12;  // 4 or 3 floats * 4 bytes\n  row_step_ = point_step_ * width_;\n\n  // Get pointer to data\n  auto maybe_points_data = point_cloud_parts.points->data<float>();\n  if (!maybe_points_data) {\n    std::stringstream error_msg;\n    error_msg << \"[GetPointCloudData] Failed to get float data from points tensor\";\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPointCloudView\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  points_data_ = maybe_points_data.value();\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPointCloudView\"),\n    \"[GetPointCloudData] Point cloud data extracted: width=%d, height=%d, \"\n    \"point_count=%d, has_color=%d, point_step=%d, row_step=%d\",\n    width_, height_, point_count_, has_color_, point_step_, row_step_);\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/test/isaac_ros_managed_nitros_point_cloud_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for ManagedNITROS with NitrosPointCloud type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest\nfrom isaac_ros_test.pcd_loader import PCDLoader\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import PointCloud2\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with ManagedNITROS forward node for testing.\"\"\"\n    test_ns = IsaacROSManagedNitrosPointCloudTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='managed_nitros_point_cloud_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_point_cloud_type',\n                plugin='nvidia::isaac_ros::nitros::ManagedNitrosPointCloudForwardNode',\n                name='ManagedNitrosPointCloudForwardNode',\n                namespace=test_ns,\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSManagedNitrosPointCloudTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSManagedNitrosPointCloudTest(IsaacROSBaseTest):\n    \"\"\"Validate ManagedNITROS with NitrosPointCloud type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_point_cloud')\n    def test_managed_nitros_point_cloud_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the point cloud received via ManagedNITROS to be identical to source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_cloud_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', PointCloud2)],\n            received_messages=received_messages\n        )\n\n        cloud_pub = self.node.create_publisher(\n            PointCloud2, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            cloud_msg = PCDLoader.generate_pointcloud2_from_pcd_file(\n                test_folder / 'ketchup.pcd', 'sample_points')\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 2\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                cloud_msg.header.stamp = timestamp\n\n                cloud_pub.publish(cloud_msg)\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(\n                done, \"Didn't receive output on output cloud topic!\")\n\n            received_points = received_messages['output']\n\n            # Validate header fields\n            self.assertEqual(cloud_msg.header.frame_id, received_points.header.frame_id,\n                             'Source and received frame ids dont match')\n\n            self.assertEqual(cloud_msg.header.stamp.sec, received_points.header.stamp.sec,\n                             f'Source and received timestamp seconds do not match: '\n                             f'{cloud_msg.header.stamp.sec} != {received_points.header.stamp.sec}')\n\n            self.assertEqual(cloud_msg.header.stamp.nanosec, received_points.header.stamp.nanosec,\n                             f'Source and received timestamp nanoseconds do not match: '\n                             f'{cloud_msg.header.stamp.nanosec} != '\n                             f'{received_points.header.stamp.nanosec}')\n\n            # Validate point cloud structure\n            self.assertEqual(len(cloud_msg.data), len(received_points.data),\n                             'Source and received point cloud sizes do not match: ' +\n                             f'{len(cloud_msg.data)} != {len(received_points.data)}')\n            self.assertEqual(cloud_msg.is_bigendian, received_points.is_bigendian,\n                             'Source and received point cloud is_bigendian field do not match')\n            self.assertEqual(cloud_msg.row_step, received_points.row_step,\n                             'Source and received point cloud row_step field do not match')\n            self.assertEqual(cloud_msg.point_step, received_points.point_step,\n                             'Source and received point cloud point_step field do not match')\n            self.assertEqual(cloud_msg.height, received_points.height,\n                             'Source and received point cloud height field do not match: '\n                             f'{cloud_msg.height} != {received_points.height}')\n            self.assertEqual(cloud_msg.width, received_points.width,\n                             'Source and received point cloud width field do not match: '\n                             f'{cloud_msg.width} != {received_points.width}')\n            self.assertEqual(cloud_msg.is_dense, received_points.is_dense,\n                             'Source and received point cloud is_dense field do not match')\n\n            # Validate PointFields\n            self.assertEqual(len(cloud_msg.fields), len(received_points.fields),\n                             'Source and received point cloud field counts do not match: '\n                             f'{len(cloud_msg.fields)} != {len(received_points.fields)}')\n            for i, (src_field, recv_field) in enumerate(\n                    zip(cloud_msg.fields, received_points.fields)):\n                self.assertEqual(src_field.name, recv_field.name,\n                                 f'Field {i} name mismatch: '\n                                 f'{src_field.name} != {recv_field.name}')\n                self.assertEqual(src_field.offset, recv_field.offset,\n                                 f'Field {i} ({src_field.name}) offset mismatch: '\n                                 f'{src_field.offset} != {recv_field.offset}')\n                self.assertEqual(src_field.datatype, recv_field.datatype,\n                                 f'Field {i} ({src_field.name}) datatype mismatch: '\n                                 f'{src_field.datatype} != {recv_field.datatype}')\n                self.assertEqual(src_field.count, recv_field.count,\n                                 f'Field {i} ({src_field.name}) count mismatch: '\n                                 f'{src_field.count} != {recv_field.count}')\n\n            # Validate point data\n            for i in range(len(cloud_msg.data)):\n                self.assertEqual(cloud_msg.data[i], received_points.data[i],\n                                 'Source and received point clouds do not match')\n\n            print('Source and received point clouds are identical.')\n        finally:\n            self.node.destroy_subscription(received_cloud_sub)\n            self.node.destroy_publisher(cloud_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/test/isaac_ros_nitros_point_cloud_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosPointCloud type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest\nfrom isaac_ros_test.pcd_loader import PCDLoader\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\nfrom sensor_msgs.msg import PointCloud2\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosPointCloudTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='point_cloud_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_point_cloud_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosPointCloudForwardNode',\n                name='NitrosPointCloudForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_point_cloud'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosPointCloudTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosPointCloudTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosPointCloud type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_point_cloud')\n    def test_nitros_point_cloud_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the image received from NitrosImage type conversion to be identical to source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_cloud_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', PointCloud2)],\n            received_messages=received_messages\n        )\n\n        cloud_pub = self.node.create_publisher(\n            PointCloud2, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            cloud_msg = PCDLoader.generate_pointcloud2_from_pcd_file(\n                test_folder / 'ketchup.pcd', 'sample_points')\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 2\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                cloud_msg.header.stamp = timestamp\n\n                cloud_pub.publish(cloud_msg)\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(\n                done, \"Didn't receive output on output_point_cloud topic!\")\n\n            received_points = received_messages['output']\n\n            self.assertEqual(cloud_msg.header.frame_id, received_points.header.frame_id,\n                             'Source and received frame ids dont match')\n\n            self.assertEqual(len(cloud_msg.data), len(received_points.data),\n                             'Source and received point cloud sizes do not match: ' +\n                             f'{len(cloud_msg.data)} != {len(received_points.data)}')\n            self.assertEqual(cloud_msg.is_bigendian, received_points.is_bigendian,\n                             'Source and received point cloud is_bigendian field do not match')\n            self.assertEqual(cloud_msg.row_step, received_points.row_step,\n                             'Source and received point cloud row_step field do not match')\n            self.assertEqual(cloud_msg.point_step, received_points.point_step,\n                             'Source and received point cloud point_step field do not match')\n            for i in range(len(cloud_msg.data)):\n                # by convention the 15th bit is not used and hence not passed through\n                if (i % 16 != 15):\n                    self.assertEqual(cloud_msg.data[i], received_points.data[i],\n                                     'Source and received point cloud data do not match')\n\n            print('Source and received point clouds are identical.')\n        finally:\n            self.node.destroy_subscription(received_cloud_sub)\n            self.node.destroy_publisher(cloud_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/test/managed_nitros_point_cloud_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <cuda_runtime.h>\n\n#include <memory>\n#include <string>\n\n#include \"rclcpp/rclcpp.hpp\"\n\n#include \"isaac_ros_managed_nitros/managed_nitros_publisher.hpp\"\n#include \"isaac_ros_managed_nitros/managed_nitros_subscriber.hpp\"\n#include \"isaac_ros_nitros_point_cloud_type/nitros_point_cloud.hpp\"\n#include \"isaac_ros_nitros_point_cloud_type/nitros_point_cloud_view.hpp\"\n#include \"isaac_ros_nitros_point_cloud_type/nitros_point_cloud_builder.hpp\"\n#include \"isaac_ros_common/cuda_stream.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nclass ManagedNitrosPointCloudForwardNode : public rclcpp::Node\n{\npublic:\n  explicit ManagedNitrosPointCloudForwardNode(\n    const rclcpp::NodeOptions options = rclcpp::NodeOptions())\n  : rclcpp::Node(\"managed_nitros_point_cloud_forward_node\", options),\n    nitros_sub_{std::make_shared<nvidia::isaac_ros::nitros::ManagedNitrosSubscriber<\n          nvidia::isaac_ros::nitros::NitrosPointCloudView>>(\n        this,\n        \"topic_forward_input\",\n        nvidia::isaac_ros::nitros::nitros_point_cloud_t::supported_type_name,\n        std::bind(&ManagedNitrosPointCloudForwardNode::InputCallback, this,\n        std::placeholders::_1))},\n    nitros_pub_{std::make_shared<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n          nvidia::isaac_ros::nitros::NitrosPointCloud>>(\n        this,\n        \"topic_forward_output\",\n        nvidia::isaac_ros::nitros::nitros_point_cloud_t::supported_type_name)}\n  {\n    CHECK_CUDA_ERROR(\n      ::nvidia::isaac_ros::common::initNamedCudaStream(\n        cuda_stream_, \"managed_nitros_point_cloud_forward_node\"),\n      \"Error initializing CUDA stream\");\n  }\n\n  ~ManagedNitrosPointCloudForwardNode() = default;\n\nprivate:\n  void InputCallback(const nvidia::isaac_ros::nitros::NitrosPointCloudView & view)\n  {\n    const size_t point_count = view.GetPointCount();\n    const bool has_color = view.HasColor();\n    const float * gpu_data_ptr = view.GetPointsData();\n\n    // Calculate buffer size\n    const size_t point_size = has_color ? 4 : 3;  // xyzrgb or xyz\n    const size_t buffer_size = point_count * point_size * sizeof(float);\n\n    // Allocate output buffer\n    float * output_buffer = nullptr;\n    CHECK_CUDA_ERROR(\n      cudaMallocAsync(&output_buffer, buffer_size, cuda_stream_),\n      \"Error allocating CUDA buffer for point cloud\");\n\n    // Copy data\n    CHECK_CUDA_ERROR(\n      cudaMemcpyAsync(\n        output_buffer, gpu_data_ptr, buffer_size,\n        cudaMemcpyDeviceToDevice, cuda_stream_),\n      \"Error copying point cloud data to CUDA buffer\");\n\n    // Wait for copy to complete\n    CHECK_CUDA_ERROR(\n      cudaStreamSynchronize(cuda_stream_),\n      \"Error synchronizing CUDA stream\");\n\n    std_msgs::msg::Header header;\n    header.frame_id = view.GetFrameId();\n    header.stamp.sec = view.GetTimestampSeconds();\n    header.stamp.nanosec = view.GetTimestampNanoseconds();\n\n    nvidia::isaac_ros::nitros::NitrosPointCloudBuilder builder;\n    builder.WithHeader(header);\n    builder.WithPoints(output_buffer, point_count, has_color);\n    builder.WithReleaseCallback([output_buffer, stream = cuda_stream_]() {\n        cudaFreeAsync(output_buffer, stream);\n    });\n    nvidia::isaac_ros::nitros::NitrosPointCloud nitros_pc = builder.Build();\n\n    nitros_pub_->publish(nitros_pc);\n  }\n\n  // Subscription to input NitrosPointCloud messages\n  std::shared_ptr<nvidia::isaac_ros::nitros::ManagedNitrosSubscriber<\n      nvidia::isaac_ros::nitros::NitrosPointCloudView>> nitros_sub_;\n\n  // Publisher for output NitrosPointCloud messages\n  std::shared_ptr<nvidia::isaac_ros::nitros::ManagedNitrosPublisher<\n      nvidia::isaac_ros::nitros::NitrosPointCloud>> nitros_pub_;\n\n  cudaStream_t cuda_stream_;\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n// Register as component\n#include \"rclcpp_components/register_node_macro.hpp\"\nRCLCPP_COMPONENTS_REGISTER_NODE(\n  nvidia::isaac_ros::nitros::ManagedNitrosPointCloudForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/test/src/nitros_point_cloud_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_point_cloud_type/nitros_point_cloud.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_point_cloud_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_point_cloud\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosPointCloudForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosPointCloudForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosPointCloud>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosPointCloudForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_point_cloud_type/test/test_cases/nitros_point_cloud/profile/ketchup.pcd",
    "content": "# .PCD v0.7 - Point Cloud Data file format\nVERSION 0.7\nFIELDS x y z rgb\nSIZE 4 4 4 4\nTYPE F F F F\nCOUNT 1 1 1 1\nWIDTH 500\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0\nPOINTS 20\nDATA ascii\n-1.634617501 -4.290111561 -1.858700299 0.2619938229\n2.853279639 -7.209591076 -0.6885781819 0.4857394092\n-1.031189043 -0.7250457872 -1.98802603 0.5820572901\n-0.8246446857 -7.238877545 -1.052616484 0.4920581732\n-1.268758149 1.713223204 -1.907102878 0.4820194757\n0.7747915949 -7.244047803 -0.8747465489 0.5830105839\n0.1939418026 -7.285154885 -0.5047866621 0.5729205837\n-1.677812876 -2.032508282 -1.813671809 0.5839017563\n-0.5203375641 0.707792248 -2.066546191 0.5739105831\n0.2619938229 3.527571041 -1.784421252 0.2619938229\n3.149173217 -5.719117107 -0.5116390493 0.2619938229\n2.333993901 -7.365552464 -0.398136829 0.2619938229\n-0.4208003406 1.830185188 -2.065341214 0.2619938229\n2.852645176 -0.4909255341 -0.6731062388 0.2619938229\n-1.764735432 -3.413405289 -1.798219496 0.2619938229\n-2.064404219 -4.691953551 -1.700539264 0.2619938229\n-0.4341473384 2.614184337 -2.041596547 0.2619938229\n-1.367775279 -1.257706668 -1.90602497 0.2619938229\n2.51894068 0.5080515703 -1.072134684 0.2619938229\n3.100033603 -1.851751732 -0.3858886334 0.2619938229"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_array_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_pose_array_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(yaml-cpp)\n\n# NitrosPoseArray\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_pose_array.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosPoseArrayForwardNode\n  ament_auto_add_library(nitros_pose_array_forward_node SHARED test/src/nitros_pose_array_forward_node.cpp)\n  target_link_libraries(nitros_pose_array_forward_node ${PROJECT_NAME})\n  set_target_properties(nitros_pose_array_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(nitros_pose_array_forward_node \"nvidia::isaac_ros::nitros::NitrosPoseArrayForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosPoseArrayForwardNode;$<TARGET_FILE:nitros_pose_array_forward_node>\\n\")\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_pose_array_type_test_pol.py TIMEOUT \"15\")\n  endif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_array_type/include/isaac_ros_nitros_pose_array_type/nitros_pose_array.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_POSE_ARRAY_TYPE__NITROS_POSE_ARRAY_HPP_\n#define ISAAC_ROS_NITROS_POSE_ARRAY_TYPE__NITROS_POSE_ARRAY_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosPoseArray\n *   ROS type:    geometry_msgs::msg::PoseArray\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"geometry_msgs/msg/pose_array.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosPoseArray;\n\n// Formats\nstruct nitros_pose_array_t\n{\n  using MsgT = NitrosPoseArray;\n  static const inline std::string supported_type_name = \"nitros_pose_array\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosPoseArray)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_pose_array_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/multimedia/libgxf_multimedia.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosPoseArray,\n  geometry_msgs::msg::PoseArray>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosPoseArray;\n  using ros_message_type = geometry_msgs::msg::PoseArray;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosPoseArray,\n  geometry_msgs::msg::PoseArray);\n\n#endif  // ISAAC_ROS_NITROS_POSE_ARRAY_TYPE__NITROS_POSE_ARRAY_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_array_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_pose_array_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS Nitros Pose Array Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Jaiveer Singh</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>geometry_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_array_type/src/nitros_pose_array.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <cuda_runtime.h>\n\n#include <string>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n#include \"isaac_ros_nitros_pose_array_type/nitros_pose_array.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nconstexpr char kEntityName[] = \"memory_pool\";\nconstexpr char kComponentName[] = \"unbounded_allocator\";\nconstexpr char kComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\n\n// Each pose is stored as a (7,) tensor: position (xyz) and orientation (quaternion, xyzw)\nconstexpr int kExpectedPoseAsTensorSize = (3 + 4);\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosPoseArray,\n  geometry_msgs::msg::PoseArray>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosPoseArray::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPoseArray\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto gxf_tensors = msg_entity->findAll<nvidia::gxf::Tensor>();\n  if (!gxf_tensors) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] failed to get all GXF tensors: \" <<\n      GxfResultStr(gxf_tensors.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPoseArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  for (auto gxf_pose_tensor_handle : gxf_tensors.value()) {\n    auto gxf_pose_tensor = gxf_pose_tensor_handle.value();\n    // Ensure pose tensor has correct shape\n    if (gxf_pose_tensor->shape() != nvidia::gxf::Shape{kExpectedPoseAsTensorSize}) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[convert_to_ros_message] Pose Tensor was not the correct shape: \" <<\n        gxf_pose_tensor->size();\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosPoseArray\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n\n    // Allocate array for copying GXF Pose Tensor data as contiguous block\n    std::array<double, kExpectedPoseAsTensorSize> ros_pose_tensor{};\n\n    // Copy pose tensor off device to CPU memory\n    switch (gxf_pose_tensor->storage_type()) {\n      case nvidia::gxf::MemoryStorageType::kHost:\n        {\n          std::memcpy(ros_pose_tensor.data(), gxf_pose_tensor->pointer(), gxf_pose_tensor->size());\n        }\n        break;\n      case nvidia::gxf::MemoryStorageType::kDevice:\n        {\n          const cudaError_t cuda_error = cudaMemcpyAsync(\n            ros_pose_tensor.data(), gxf_pose_tensor->pointer(),\n            gxf_pose_tensor->size(), cudaMemcpyDeviceToHost, source.cuda_stream);\n          if (cuda_error != cudaSuccess) {\n            std::stringstream error_msg;\n            error_msg <<\n              \"[convert_to_ros_message] cudaMemcpy failed for conversion from \"\n              \"gxf::Tensor to ROS Pose: \" <<\n              cudaGetErrorName(cuda_error) <<\n              \" (\" << cudaGetErrorString(cuda_error) << \")\";\n            RCLCPP_ERROR(\n              rclcpp::get_logger(\"NitrosPoseArray\"), error_msg.str().c_str());\n            throw std::runtime_error(error_msg.str().c_str());\n          }\n          cudaError_t cuda_result = cudaStreamSynchronize(source.cuda_stream);\n          CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n        }\n        break;\n      default:\n        std::string error_msg =\n          \"[convert_to_ros_message] MemoryStorageType not supported: conversion from \"\n          \"gxf::Tensor to ROS Pose failed!\";\n        RCLCPP_ERROR(\n          rclcpp::get_logger(\"NitrosPoseArray\"), error_msg.c_str());\n        throw std::runtime_error(error_msg.c_str());\n    }\n\n    // Create corresponding ROS 2 Pose and populate the message object's fields\n    auto ros_pose = geometry_msgs::msg::Pose{};\n    ros_pose.position.x = ros_pose_tensor.at(0);\n    ros_pose.position.y = ros_pose_tensor.at(1);\n    ros_pose.position.z = ros_pose_tensor.at(2);\n\n    ros_pose.orientation.x = ros_pose_tensor.at(3);\n    ros_pose.orientation.y = ros_pose_tensor.at(4);\n    ros_pose.orientation.z = ros_pose_tensor.at(5);\n    ros_pose.orientation.w = ros_pose_tensor.at(6);\n\n    // Add the completed Pose to the PoseArray\n    destination.poses.push_back(ros_pose);\n  }\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!input_timestamp) {    // Fallback to label 'timestamp'\n    input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>();\n  }\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp.value()->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp.value()->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Set frame ID\n  destination.header.frame_id = source.frame_id;\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPoseArray\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosPoseArray,\n  geometry_msgs::msg::PoseArray>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosPoseArray::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPoseArray\"),\n    \"[convert_to_custom] Conversion started\");\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPoseArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  auto message = nvidia::gxf::Entity::New(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext());\n  if (!message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error initializing new message entity: \" <<\n      GxfResultStr(message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPoseArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  for (auto ros_pose : source.poses) {\n    auto gxf_pose_tensor = message->add<nvidia::gxf::Tensor>();\n\n    // Initializing GXF tensor\n    auto result = gxf_pose_tensor.value()->reshape<double>(\n      nvidia::gxf::Shape{kExpectedPoseAsTensorSize}, nvidia::gxf::MemoryStorageType::kDevice,\n      allocator_handle);\n\n    if (!result) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[convert_to_custom] Error initializing GXF pose tensor of shape (\" <<\n        kExpectedPoseAsTensorSize << \",): \" <<\n        GxfResultStr(result.error());\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosPoseArray\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n\n    // Get pointer to first element of pose tensor\n    auto maybe_data = gxf_pose_tensor.value()->data<double>();\n    if (!maybe_data) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[convert_to_ros_message] Could not get data pointer from Pose Tensor: \" <<\n        gxf_pose_tensor.value();\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosPoseArray\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n\n    // Allocate array for copying ROS Pose data as contiguous block\n    std::array<double, kExpectedPoseAsTensorSize> ros_pose_tensor{\n      ros_pose.position.x,\n      ros_pose.position.y,\n      ros_pose.position.z,\n\n      ros_pose.orientation.x,\n      ros_pose.orientation.y,\n      ros_pose.orientation.z,\n      ros_pose.orientation.w\n    };\n\n    // Populate ROS Pose data into GXF Tensor\n    auto nitros_cuda_stream =\n      nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext()\n      .getCudaStreamFromNitrosGraph();\n    const cudaMemcpyKind operation = cudaMemcpyHostToDevice;\n    const cudaError_t cuda_error = cudaMemcpyAsync(\n      gxf_pose_tensor.value()->pointer(),\n      ros_pose_tensor.data(),\n      gxf_pose_tensor.value()->size(),\n      operation, nitros_cuda_stream\n    );\n\n    if (cuda_error != cudaSuccess) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[convert_to_custom] cudaMemcpy failed for copying data from \"\n        \"ROS Pose Tensor to GXF Tensor: \" <<\n        cudaGetErrorName(cuda_error) <<\n        \" (\" << cudaGetErrorString(cuda_error) << \")\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosPoseArray\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n    cudaError_t cuda_result = cudaStreamSynchronize(nitros_cuda_stream);\n    CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n  }\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  auto output_timestamp = message->add<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!output_timestamp) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Failed to add a timestamp component to message: \" <<\n      GxfResultStr(output_timestamp.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPoseArray\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  output_timestamp.value()->acqtime = input_timestamp;\n\n  // Set frame ID\n  destination.frame_id = source.header.frame_id;\n\n  // Set message entity\n  destination.handle = message->eid();\n  GxfEntityRefCountInc(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), message->eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPoseArray\"),\n    \"[convert_to_custom] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_array_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_array_type/test/isaac_ros_nitros_pose_array_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosPoseArray type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom geometry_msgs.msg import PoseArray\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\n\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosPoseArrayTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_pose_array_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosPoseArrayForwardNode',\n                name='NitrosPoseArrayForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_pose_array'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosPoseArrayTest.generate_test_description(\n        [container],\n        node_startup_delay=2.5\n    )\n\n\nclass IsaacROSNitrosPoseArrayTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosPoseArray type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case()\n    def test_nitros_pose_array_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the pose array from NitrosPoseArray type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', PoseArray)],\n            received_messages=received_messages\n        )\n\n        pose_array_pub = self.node.create_publisher(\n            PoseArray, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            pose_array = JSONConversion.load_pose_array_from_json(\n                test_folder / 'pose_array.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 2\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                pose_array.header.stamp = timestamp\n\n                pose_array_pub.publish(pose_array)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_pose_array = received_messages['output']\n\n            # Header\n            # Frame ID is to be passed from NitrosPoseArray to ROS message\n            # self.assertEqual(pose_array.header.frame_id, received_pose_array.header.frame_id)\n\n            # Poses\n            self.assertEqual(len(pose_array.poses), len(\n                received_pose_array.poses), 'Number of poses does not match')\n\n            for pose, received_pose in zip(pose_array.poses, received_pose_array.poses):\n                self.assertEqual(\n                    round(pose.position.x, 2), round(received_pose.position.x, 2),\n                    'Position x value does not match')\n                self.assertEqual(\n                    round(pose.position.y, 2), round(received_pose.position.y, 2),\n                    'Position y value does not match')\n                self.assertEqual(\n                    round(pose.position.z, 2), round(received_pose.position.z, 2),\n                    'Position z value does not match')\n\n                self.assertEqual(\n                    round(pose.orientation.x, 2), round(received_pose.orientation.x, 2),\n                    'Orientation x value does not match')\n                self.assertEqual(\n                    round(pose.orientation.y, 2), round(received_pose.orientation.y, 2),\n                    'Orientation x value does not match')\n                self.assertEqual(\n                    round(pose.orientation.z, 2), round(received_pose.orientation.z, 2),\n                    'Orientation x value does not match')\n                self.assertEqual(\n                    round(pose.orientation.w, 2), round(received_pose.orientation.w, 2),\n                    'Orientation w value does not match')\n\n            print('The received pose array is verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(pose_array_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_array_type/test/src/nitros_pose_array_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_pose_array_type/nitros_pose_array.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_pose_array_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_pose_array\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosPoseArrayForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosPoseArrayForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosPoseArray>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosPoseArrayForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_array_type/test/test_cases/profile/pose_array.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"tf_camera\"\n  },\n  \"poses\": [\n    {\n      \"position\": {\n        \"x\": 1.0,\n        \"y\": 2.0,\n        \"z\": 3.0\n      },\n      \"orientation\": {\n        \"x\": 0.0,\n        \"y\": 0.0,\n        \"z\": 0.0,\n        \"w\": 1.0\n      }\n    },\n    {\n      \"position\": {\n        \"x\": 4.0,\n        \"y\": 5.0,\n        \"z\": 6.0\n      },\n      \"orientation\": {\n        \"x\": 1.0,\n        \"y\": 0.0,\n        \"z\": 0.0,\n        \"w\": 0.0\n      }\n    }\n  ]\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_cov_stamped_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_pose_cov_stamped_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(yaml-cpp)\n\n# NitrosPoseCovStamped\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_pose_cov_stamped.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosPoseCovStampedForwardNode\n  ament_auto_add_library(isaac_ros_nitros_pose_cov_stamped_forward_node SHARED test/src/nitros_pose_cov_stamped_forward_node.cpp)\n  target_link_libraries(isaac_ros_nitros_pose_cov_stamped_forward_node ${PROJECT_NAME})\n  set_target_properties(isaac_ros_nitros_pose_cov_stamped_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(isaac_ros_nitros_pose_cov_stamped_forward_node\n    \"nvidia::isaac_ros::nitros::NitrosPoseCovStampedForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosPoseCovStampedForwardNode;\\\n    $<TARGET_FILE:isaac_ros_nitros_pose_cov_stamped_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_pose_cov_stamped_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_cov_stamped_type/include/isaac_ros_nitros_pose_cov_stamped_type/nitros_pose_cov_stamped.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_POSE_COV_STAMPED_TYPE__NITROS_POSE_COV_STAMPED_HPP_\n#define ISAAC_ROS_NITROS_POSE_COV_STAMPED_TYPE__NITROS_POSE_COV_STAMPED_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosPoseCovStamped\n *   ROS type:    geometry_msgs::msg::PoseWithCovarianceStamped\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"geometry_msgs/msg/pose_with_covariance_stamped.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosPoseCovStamped;\n\n// Formats\nstruct nitros_pose_cov_stamped_t\n{\n  using MsgT = NitrosPoseCovStamped;\n  static const inline std::string supported_type_name = \"nitros_pose_cov_stamped\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosPoseCovStamped)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_pose_cov_stamped_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_messages\", \"gxf/lib/libgxf_isaac_messages.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosPoseCovStamped,\n  geometry_msgs::msg::PoseWithCovarianceStamped>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosPoseCovStamped;\n  using ros_message_type = geometry_msgs::msg::PoseWithCovarianceStamped;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosPoseCovStamped,\n  geometry_msgs::msg::PoseWithCovarianceStamped);\n\n#endif  // ISAAC_ROS_NITROS_POSE_COV_STAMPED_TYPE__NITROS_POSE_COV_STAMPED_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_cov_stamped_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2023-2024, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_pose_cov_stamped_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS NITROS Pose With Covariance Stamped Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Ashwin Varghese Kuruttukulam</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>sensor_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>gxf_isaac_messages</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>gxf_isaac_gems</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_cov_stamped_type/src/nitros_pose_cov_stamped.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n#include <cuda_runtime.h>\n\n#include <string>\n#include <unordered_map>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gems/core/tensor/tensor.hpp\"\n#include \"extensions/messages/pose3d_cov_message.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_pose_cov_stamped_type/nitros_pose_cov_stamped.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nnamespace\n{\nconstexpr char kEntityName[] = \"memory_pool\";\nconstexpr char kComponentName[] = \"unbounded_allocator\";\nconstexpr char kComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\nconstexpr int kCovarianceMatrixSize = 6;\nconstexpr int kCovarianceArrSize = 36;\n}  // namespace\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosPoseCovStamped,\n  geometry_msgs::msg::PoseWithCovarianceStamped>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosPoseCovStamped::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPoseCovStamped\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  auto maybe_pose_cov_stamped_parts = nvidia::isaac::GetPose3dCovMessage(\n    msg_entity.value());\n  if (!maybe_pose_cov_stamped_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get pose_3d_cov message\"\n      \" from message entity: \" <<\n      GxfResultStr(maybe_pose_cov_stamped_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPoseCovStamped\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto pose_cov_stamped_parts = maybe_pose_cov_stamped_parts.value();\n  destination.pose.pose.position.x = pose_cov_stamped_parts.pose->translation.x();\n  destination.pose.pose.position.y = pose_cov_stamped_parts.pose->translation.y();\n  destination.pose.pose.position.z = pose_cov_stamped_parts.pose->translation.z();\n  destination.pose.pose.orientation.x = pose_cov_stamped_parts.pose->rotation.quaternion().x();\n  destination.pose.pose.orientation.y = pose_cov_stamped_parts.pose->rotation.quaternion().y();\n  destination.pose.pose.orientation.z = pose_cov_stamped_parts.pose->rotation.quaternion().z();\n  destination.pose.pose.orientation.w = pose_cov_stamped_parts.pose->rotation.quaternion().w();\n\n  auto pose3d_cov_tensor_view = pose_cov_stamped_parts.covariance;\n  auto pose3d_cov_dimensions = pose3d_cov_tensor_view.dimensions();\n  if (pose3d_cov_dimensions[0] != kCovarianceMatrixSize ||\n    pose3d_cov_dimensions[1] != kCovarianceMatrixSize)\n  {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Tensor dimensions is not equal to \" << kCovarianceMatrixSize;\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPoseCovStamped\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  for (int i = 0; i < pose3d_cov_dimensions[0]; i++) {\n    for (int j = 0; j < pose3d_cov_dimensions[1]; j++) {\n      destination.pose.covariance[i + kCovarianceMatrixSize * j] =\n        pose3d_cov_tensor_view(i, j);\n    }\n  }\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = pose_cov_stamped_parts.timestamp;\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Set frame ID\n  destination.header.frame_id = source.frame_id;\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPoseCovStamped\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosPoseCovStamped,\n  geometry_msgs::msg::PoseWithCovarianceStamped>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosPoseCovStamped::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPoseCovStamped\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPoseCovStamped\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  // create Pose3dCovMessage\n  auto maybe_pose_cov_stamped_parts = nvidia::isaac::CreatePose3dCovMessage(\n    context, allocator_handle);\n  if (!maybe_pose_cov_stamped_parts) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_gxf_message] Failed to create Pose3dCovMessage \" << GxfResultStr(\n      maybe_pose_cov_stamped_parts.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPoseCovStamped\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto pose_cov_stamped_parts = maybe_pose_cov_stamped_parts.value();\n  // populate pose data\n  *(pose_cov_stamped_parts.pose) = ::nvidia::isaac::Pose3d{\n    ::nvidia::isaac::SO3d::FromQuaternion(\n      ::nvidia::isaac::Quaterniond{\n      source.pose.pose.orientation.w,\n      source.pose.pose.orientation.x,\n      source.pose.pose.orientation.y,\n      source.pose.pose.orientation.z}),\n    ::nvidia::isaac::Vector3d(\n      source.pose.pose.position.x,\n      source.pose.pose.position.y,\n      source.pose.pose.position.z)};\n\n  // populate pose covariance data\n  auto pose3d_cov_tensor_view = pose_cov_stamped_parts.covariance;\n  auto pose3d_cov_dimensions = pose3d_cov_tensor_view.dimensions();\n  if (pose3d_cov_dimensions[0] != kCovarianceMatrixSize ||\n    pose3d_cov_dimensions[1] != kCovarianceMatrixSize)\n  {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_gxf_message] Pose3dCovMessage 2D array expected shape is\" <<\n      kCovarianceMatrixSize << \"x\" << kCovarianceMatrixSize <<\n      \", but actual shape is\" << pose3d_cov_dimensions[0] << \"x\" << pose3d_cov_dimensions[1];\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPoseCovStamped\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  if (source.pose.covariance.size() != kCovarianceArrSize) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_gxf_message] Expected PoseWithCovarianceStamped covariance array length is \" <<\n      kCovarianceArrSize << \", but actual length is\" << source.pose.covariance.size();\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosPoseCovStamped\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  for (int i = 0; i < pose3d_cov_dimensions[0]; i++) {\n    for (int j = 0; j < pose3d_cov_dimensions[1]; j++) {\n      pose3d_cov_tensor_view(i, j) =\n        static_cast<double>(source.pose.covariance[i + kCovarianceMatrixSize * j]);\n    }\n  }\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  pose_cov_stamped_parts.timestamp->acqtime = input_timestamp;\n\n  // Set frame ID\n  destination.frame_id = source.header.frame_id;\n\n  // Set Entity Id\n  destination.handle = pose_cov_stamped_parts.entity.eid();\n  GxfEntityRefCountInc(context, pose_cov_stamped_parts.entity.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosPoseCovStamped\"),\n    \"[convert_to_custom] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_cov_stamped_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_cov_stamped_type/test/isaac_ros_nitros_pose_cov_stamped_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosPoseCovStamped type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom geometry_msgs.msg import PoseWithCovarianceStamped\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosPoseCovStampedTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_pose_cov_stamped_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosPoseCovStampedForwardNode',\n                name='NitrosPoseCovStampedForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_pose_cov_stamped'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosPoseCovStampedTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosPoseCovStampedTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosPoseCovStamped type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_pose_cov_stamped')\n    def test_nitros_pose_cov_stamped_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the tag from NitrosPoseCovStamped type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', PoseWithCovarianceStamped)],\n            received_messages=received_messages\n        )\n\n        pose_cov_stamped_pub = self.node.create_publisher(\n            PoseWithCovarianceStamped, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            print(test_folder)\n            pose_cov_stamped = self.load_pose_cov_stamped_from_json(\n                test_folder / 'pose_cov_stamped.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 10\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                timestamp = self.node.get_clock().now().to_msg()\n                pose_cov_stamped.header.stamp = timestamp\n\n                pose_cov_stamped_pub.publish(pose_cov_stamped)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_pose_cov_stamped = received_messages['output']\n\n            self.assertEqual(pose_cov_stamped.pose.pose.position.x,\n                             received_pose_cov_stamped.pose.pose.position.x)\n            self.assertEqual(pose_cov_stamped.pose.pose.position.y,\n                             received_pose_cov_stamped.pose.pose.position.y)\n            self.assertEqual(pose_cov_stamped.pose.pose.position.z,\n                             received_pose_cov_stamped.pose.pose.position.z)\n\n            self.assertAlmostEqual(pose_cov_stamped.pose.pose.orientation.x,\n                                   received_pose_cov_stamped.pose.pose.orientation.x, None,\n                                   'did not receive expected orientation', 0.001)\n            self.assertAlmostEqual(pose_cov_stamped.pose.pose.orientation.y,\n                                   received_pose_cov_stamped.pose.pose.orientation.y, None,\n                                   'did not receive expected orientation', 0.001)\n            self.assertAlmostEqual(pose_cov_stamped.pose.pose.orientation.z,\n                                   received_pose_cov_stamped.pose.pose.orientation.z, None,\n                                   'did not receive expected orientation', 0.001)\n            self.assertAlmostEqual(pose_cov_stamped.pose.pose.orientation.w,\n                                   received_pose_cov_stamped.pose.pose.orientation.w, None,\n                                   'did not receive expected orientation', 0.001)\n            for i in range(36):\n                self.assertEqual(pose_cov_stamped.pose.covariance[i],\n                                 received_pose_cov_stamped.pose.covariance[i])\n            print('The received pose cov stamped message has been verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(pose_cov_stamped_pub)\n\n    @staticmethod\n    def load_pose_cov_stamped_from_json(\n            json_filepath: pathlib.Path) -> PoseWithCovarianceStamped:\n        \"\"\"\n        Load a PoseWithCovarianceStamped message from a JSON filepath.\n\n        Parameters\n        ----------\n        json_filepath : Path\n            The path to a JSON file containing the PoseWithCovarianceStamped fields\n\n        Returns\n        -------\n        PoseWithCovarianceStamped\n            Generated PoseWithCovarianceStamped message\n\n        \"\"\"\n        pose_cov_stamped_json = JSONConversion.load_from_json(\n            json_filepath)\n\n        pose_cov_stamped = PoseWithCovarianceStamped()\n        pose_cov_stamped.header.frame_id = pose_cov_stamped_json[\n            'header']['frame_id']\n        pose_cov_stamped.header.stamp.sec = pose_cov_stamped_json[\n            'header']['stamp']['sec']\n        pose_cov_stamped.header.stamp.nanosec = pose_cov_stamped_json[\n            'header']['stamp']['nanosec']\n        pose_cov_stamped.pose.pose.position.x = pose_cov_stamped_json[\n            'pose']['pose']['position']['x']\n        pose_cov_stamped.pose.pose.position.y = pose_cov_stamped_json[\n            'pose']['pose']['position']['y']\n        pose_cov_stamped.pose.pose.position.z = pose_cov_stamped_json[\n            'pose']['pose']['position']['z']\n        pose_cov_stamped.pose.pose.orientation.x = pose_cov_stamped_json[\n            'pose']['pose']['orientation']['x']\n        pose_cov_stamped.pose.pose.orientation.y = pose_cov_stamped_json[\n            'pose']['pose']['orientation']['y']\n        pose_cov_stamped.pose.pose.orientation.z = pose_cov_stamped_json[\n            'pose']['pose']['orientation']['z']\n        pose_cov_stamped.pose.pose.orientation.w = pose_cov_stamped_json[\n            'pose']['pose']['orientation']['w']\n        for i in range(36):\n            pose_cov_stamped.pose.covariance[i] = pose_cov_stamped_json['pose']['covariance'][i]\n        return pose_cov_stamped\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_cov_stamped_type/test/src/nitros_pose_cov_stamped_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_pose_cov_stamped_type/nitros_pose_cov_stamped.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_pose_cov_stamped_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_pose_cov_stamped\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosPoseCovStampedForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosPoseCovStampedForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosPoseCovStamped>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosPoseCovStampedForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_pose_cov_stamped_type/test/test_cases/nitros_pose_cov_stamped/profile/pose_cov_stamped.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"pose_header\",\n    \"stamp\": {\n      \"sec\": 20121,\n      \"nanosec\": 45845\n      }\n  },\n  \"pose\": {\n      \"pose\": {\n        \"position\": {\n          \"x\": 1.1,\n          \"y\": 1.2,\n          \"z\": 1.3\n        },\n        \"orientation\": {\n          \"x\": 0.3728217,\n          \"y\": 0.0,\n          \"z\": 0.3728217,\n          \"w\": 0.8497105\n        }\n      },\n      \"covariance\":[ 1,  2,  3,  4,  5,  6,\n                     7,  8,  9, 10, 11, 12,\n                    13, 14, 15, 16, 17, 18,\n                    19, 20, 21, 22, 23, 24,\n                    25, 26, 27, 28, 29, 30,\n                    31, 32, 33, 34, 35, 36]\n    }\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_std_msg_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_std_msg_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(yaml-cpp)\n\n# NitrosInt64\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_int64.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosInt64ForwardNode\n  ament_auto_add_library(nitros_int64_forward_node SHARED test/src/nitros_int64_forward_node.cpp)\n  target_link_libraries(nitros_int64_forward_node ${PROJECT_NAME})\n  set_target_properties(nitros_int64_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(nitros_int64_forward_node \"nvidia::isaac_ros::nitros::NitrosInt64ForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosInt64ForwardNode;$<TARGET_FILE:nitros_int64_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_int64_type_test_pol.py TIMEOUT \"15\")\n  endif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_std_msg_type/include/isaac_ros_nitros_std_msg_type/nitros_int64.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_STD_MSG_TYPE__NITROS_INT64_HPP_\n#define ISAAC_ROS_NITROS_STD_MSG_TYPE__NITROS_INT64_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosInt64\n *   ROS type:    std_msgs::msg::Int64\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"std_msgs/msg/int64.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosInt64;\n\n// Formats\nstruct nitros_int64_t\n{\n  using MsgT = NitrosInt64;\n  static const inline std::string supported_type_name = \"nitros_int64\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosInt64)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_int64_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosInt64,\n  std_msgs::msg::Int64>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosInt64;\n  using ros_message_type = std_msgs::msg::Int64;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosInt64,\n  std_msgs::msg::Int64);\n\n#endif  // ISAAC_ROS_NITROS_STD_MSG_TYPE__NITROS_INT64_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_std_msg_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_std_msg_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS Nitros Std Msg Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>geometry_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_std_msg_type/src/nitros_int64.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <string>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_std_msg_type/nitros_int64.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosInt64,\n  std_msgs::msg::Int64>::convert_to_ros_message(\n  const custom_type & source,\n  ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosInt64::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosInt64\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto gxf_message = nvidia::gxf::Entity::Shared(context, source.handle);\n  if (!gxf_message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Error getting message entity: \" <<\n      GxfResultStr(gxf_message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosInt64\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  auto data = gxf_message.value().get<int64_t>(\"payload\");\n  if (!data) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Error getting data from message entity: \" <<\n      GxfResultStr(data.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosInt64\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  destination.data = *data->get();\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosInt64,\n  std_msgs::msg::Int64>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosInt64::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosInt64\"),\n    \"[convert_to_custom] Conversion started\");\n\n  // Create an entity for storing the actual data in the context\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto gxf_message = nvidia::gxf::Entity::New(context);\n  if (!gxf_message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error initializing new message entity: \" <<\n      GxfResultStr(gxf_message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosInt64\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto payload = gxf_message.value().add<int64_t>(\"payload\");\n  if (!payload) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error creating component for int64_t payload: \" <<\n      GxfResultStr(payload.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosInt64\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  *payload->get() = source.data;\n\n  // Set the entity's pointer (EID) in a Nitros type data struct\n  destination.handle = gxf_message->eid();\n  destination.data_format_name = \"nitros_int64\";\n\n  // Increase the reference count for the created entity so it doesn't get destroyed after\n  // exiting this function\n  GxfEntityRefCountInc(context, gxf_message->eid());\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_std_msg_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_std_msg_type/test/isaac_ros_nitros_int64_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosInt64 type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\n\nimport pytest\nimport rclpy\n\nfrom std_msgs.msg import Int64\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosInt64Test.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_std_msg_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosInt64ForwardNode',\n                name='NitrosInt64ForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_int64'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosInt64Test.generate_test_description(\n        [container],\n        node_startup_delay=2.5\n    )\n\n\nclass IsaacROSNitrosInt64Test(IsaacROSBaseTest):\n    \"\"\"Validate NitrosInt64 type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case()\n    def test_nitros_int64_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the int64 from NitrosInt64 type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', Int64)],\n            received_messages=received_messages\n        )\n\n        int64_msg_pub = self.node.create_publisher(\n            Int64, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            int64_msg = Int64()\n            int64_msg.data = 42\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 2\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n                int64_msg_pub.publish(int64_msg)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_int64_msg = received_messages['output']\n\n            self.assertEqual(received_int64_msg.data, 42, 'Number stored in Int64 does not match')\n\n            print('The received int64 was verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(int64_msg_pub)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_std_msg_type/test/src/nitros_int64_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_std_msg_type/nitros_int64.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_std_msg_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_int64\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosInt64ForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosInt64ForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosInt64>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosInt64ForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_std_msg_type/test/test_cases/profile/pose_array.json",
    "content": "{\n  \"header\": {\n    \"frame_id\": \"tf_camera\"\n  },\n  \"poses\": [\n    {\n      \"position\": {\n        \"x\": 1.0,\n        \"y\": 2.0,\n        \"z\": 3.0\n      },\n      \"orientation\": {\n        \"x\": 0.0,\n        \"y\": 0.0,\n        \"z\": 0.0,\n        \"w\": 1.0\n      }\n    },\n    {\n      \"position\": {\n        \"x\": 4.0,\n        \"y\": 5.0,\n        \"z\": 6.0\n      },\n      \"orientation\": {\n        \"x\": 1.0,\n        \"y\": 0.0,\n        \"z\": 0.0,\n        \"w\": 0.0\n      }\n    }\n  ]\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_tensor_list_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\nset(CMAKE_POSITION_INDEPENDENT_CODE ON)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n  add_definitions(-DUSE_NVTX)\n  link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n  link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# Dependencies\nfind_package(yaml-cpp)\n\n# NitrosTensorList\nament_auto_add_library(${PROJECT_NAME} SHARED\n  src/nitros_data_type.cpp\n  src/nitros_tensor_builder.cpp\n  src/nitros_tensor_list_builder.cpp\n  src/nitros_tensor_list_view.cpp\n  src/nitros_tensor_list.cpp\n)\ntarget_link_libraries(${PROJECT_NAME}\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosTensorListForwardNode\n  ament_auto_add_library(nitros_tensor_list_forward_node SHARED test/src/nitros_tensor_list_forward_node.cpp)\n  target_link_libraries(nitros_tensor_list_forward_node ${PROJECT_NAME})\n  set_target_properties(nitros_tensor_list_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(nitros_tensor_list_forward_node \"nvidia::isaac_ros::nitros::NitrosTensorListForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosTensorListForwardNode;$<TARGET_FILE:nitros_tensor_list_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_tensor_list_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/include/isaac_ros_nitros_tensor_list_type/nitros_data_type.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_DATA_TYPE_HPP_\n#define ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_DATA_TYPE_HPP_\n\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nenum class NitrosDataType\n{\n  kCustom,\n  kInt8,\n  kUnsigned8,\n  kInt16,\n  kUnsigned16,\n  kInt32,\n  kUnsigned32,\n  kInt64,\n  kUnsigned64,\n  kFloat32,\n  kFloat64,\n  kComplex64,\n  kComplex128,\n};\n\nnvidia::gxf::PrimitiveType GetPrimitiveType(NitrosDataType nitros_data_type);\n\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_DATA_TYPE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/include/isaac_ros_nitros_tensor_list_type/nitros_tensor.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_HPP_\n#define ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_HPP_\n\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosTensor)\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/include/isaac_ros_nitros_tensor_list_type/nitros_tensor_builder.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_BUILDER_HPP_\n#define ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_BUILDER_HPP_\n\n#include <cuda_runtime.h>\n\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_shape.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_data_type.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nclass NitrosTensorBuilder\n{\npublic:\n  NitrosTensorBuilder();\n\n  NitrosTensorBuilder & WithShape(NitrosTensorShape tensor_shape);\n\n  NitrosTensorBuilder & WithDataType(NitrosDataType data_type);\n\n  NitrosTensorBuilder & WithData(void * data);\n\n  NitrosTensorBuilder & WithEvent(cudaEvent_t event);\n\n  NitrosTensorBuilder & WithReleaseCallback(std::function<void()> release_callback);\n\n  NitrosTensor Build();\n\nprivate:\n  NitrosTensor nitros_tensor_{};\n\n  NitrosTensorShape shape_{};\n\n  NitrosDataType data_type_{};\n\n  void * data_{};\n\n  cudaEvent_t event_{};\n\n  std::function<void()> release_callback_{nullptr};\n};\n\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_BUILDER_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/include/isaac_ros_nitros_tensor_list_type/nitros_tensor_list.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_LIST_HPP_\n#define ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_LIST_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosTensorList\n *   ROS type:    isaac_ros_tensor_list_interfaces::msg::TensorList\n */\n\n#include <cuda_runtime.h>\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n#include \"isaac_ros_nitros/types/nitros_type_base.hpp\"\n#include \"isaac_ros_tensor_list_interfaces/msg/tensor_list.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosTensorList;\n\n// Formats\nstruct nitros_tensor_list_nchw_t\n{\n  using MsgT = NitrosTensorList;\n  static const inline std::string supported_type_name = \"nitros_tensor_list_nchw\";\n};\n\nstruct nitros_tensor_list_nhwc_t\n{\n  using MsgT = NitrosTensorList;\n  static const inline std::string supported_type_name = \"nitros_tensor_list_nhwc\";\n};\n\nstruct nitros_tensor_list_nchw_rgb_f32_t\n{\n  using MsgT = NitrosTensorList;\n  static const inline std::string supported_type_name = \"nitros_tensor_list_nchw_rgb_f32\";\n};\n\nstruct nitros_tensor_list_nhwc_rgb_f32_t\n{\n  using MsgT = NitrosTensorList;\n  static const inline std::string supported_type_name = \"nitros_tensor_list_nhwc_rgb_f32\";\n};\n\nstruct nitros_tensor_list_nchw_bgr_f32_t\n{\n  using MsgT = NitrosTensorList;\n  static const inline std::string supported_type_name = \"nitros_tensor_list_nchw_bgr_f32\";\n};\n\nstruct nitros_tensor_list_nhwc_bgr_f32_t\n{\n  using MsgT = NitrosTensorList;\n  static const inline std::string supported_type_name = \"nitros_tensor_list_nhwc_bgr_f32\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosTensorList)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_tensor_list_nchw_t)\nNITROS_FORMAT_ADD(nitros_tensor_list_nhwc_t)\nNITROS_FORMAT_ADD(nitros_tensor_list_nchw_rgb_f32_t)\nNITROS_FORMAT_ADD(nitros_tensor_list_nhwc_rgb_f32_t)\nNITROS_FORMAT_ADD(nitros_tensor_list_nchw_bgr_f32_t)\nNITROS_FORMAT_ADD(nitros_tensor_list_nhwc_bgr_f32_t)\nNITROS_FORMAT_FACTORY_END()\n// Default compatible data format\nNITROS_DEFAULT_COMPATIBLE_FORMAT(nitros_tensor_list_nchw_t)\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosTensorList,\n  isaac_ros_tensor_list_interfaces::msg::TensorList>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosTensorList;\n  using ros_message_type = isaac_ros_tensor_list_interfaces::msg::TensorList;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosTensorList,\n  isaac_ros_tensor_list_interfaces::msg::TensorList);\n\n#endif  // ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_LIST_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/include/isaac_ros_nitros_tensor_list_type/nitros_tensor_list_builder.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_LIST_BUILDER_HPP_\n#define ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_LIST_BUILDER_HPP_\n\n#include <string>\n\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_shape.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_data_type.hpp\"\n\n#include \"std_msgs/msg/header.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nclass NitrosTensorListBuilder\n{\npublic:\n  NitrosTensorListBuilder();\n\n  NitrosTensorListBuilder & WithHeader(std_msgs::msg::Header header);\n\n  NitrosTensorListBuilder & AddTensor(std::string name, NitrosTensor tensor);\n\n  NitrosTensorList Build();\n\nprivate:\n  NitrosTensorList nitros_tensor_list_{};\n};\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_LIST_BUILDER_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/include/isaac_ros_nitros_tensor_list_type/nitros_tensor_list_view.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_LIST_VIEW_HPP_\n#define ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_LIST_VIEW_HPP_\n\n#include <string>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/expected.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/tensor.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros/types/nitros_type_view_factory.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_shape.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nusing PrimitiveType = gxf::PrimitiveType;\n\nNITROS_TYPE_VIEW_FACTORY_BEGIN(NitrosTensorList)\nMARK_PUBLIC_SECTION()\nclass NitrosTensorView\n{\npublic:\n  explicit NitrosTensorView(const gxf::Tensor & tensor, const std::string & name = \"\")\n  : tensor_{tensor}, name_{name} {FillStrides();}\n  inline const unsigned char * GetBuffer() const {return tensor_.pointer();}\n  inline const std::string GetName() const {return name_;}\n  inline uint32_t GetRank() const {return tensor_.rank();}\n  inline uint64_t GetBytesPerElement() const {return tensor_.bytes_per_element();}\n  inline uint64_t GetElementCount() const {return tensor_.element_count();}\n  inline size_t GetDimension(size_t index) const {return tensor_.shape().dimension(index);}\n  inline size_t GetTensorSize() const {return tensor_.size();}\n  inline NitrosTensorShape GetShape() const {return NitrosTensorShape(tensor_.shape());}\n  inline PrimitiveType GetElementType() const {return tensor_.element_type();}\n  inline std::vector<uint64_t> GetStrides() const {return strides_;}\n\nprivate:\n  const gxf::Tensor & tensor_{};\n  const std::string name_{};\n  std::vector<uint64_t> strides_{};\n  inline void FillStrides()\n  {\n    for (size_t i = 0; i < tensor_.shape().rank(); i++) {\n      strides_.push_back(tensor_.stride(i));\n    }\n  }\n};\n\nMARK_PUBLIC_SECTION()\n// Public methods\nsize_t GetTensorCount() const;\nconst std::vector<NitrosTensorListView::NitrosTensorView> GetAllTensor() const;\nconst NitrosTensorView GetAnyNamedTensor(std::string tensor_name) const;\nconst NitrosTensorView GetNamedTensor(std::string tensor_name) const;\n\nMARK_PRIVATE_SECTION()\nvoid GetAllTensorEntity();\nFixedVector<gxf::Handle<gxf::Tensor>, kMaxComponents> tensor_list_;\nNITROS_TYPE_VIEW_FACTORY_END(NitrosTensorList)\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_LIST_VIEW_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/include/isaac_ros_nitros_tensor_list_type/nitros_tensor_shape.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_SHAPE_HPP_\n#define ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_SHAPE_HPP_\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/std/tensor.hpp\"\n#pragma GCC diagnostic pop\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nclass NitrosTensorShape\n{\npublic:\n  NitrosTensorShape(std::initializer_list<int32_t> dimensions)\n  : shape_{dimensions} {}\n\n  explicit NitrosTensorShape(nvidia::gxf::Shape shape)\n  : shape_{shape} {}\n\n  const nvidia::gxf::Shape & shape() const {return shape_;}\n\nprivate:\n  nvidia::gxf::Shape shape_{};\n};\n\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n#endif  // ISAAC_ROS_NITROS_TENSOR_LIST_TYPE__NITROS_TENSOR_SHAPE_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2022, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_tensor_list_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS Nitros Tensor List Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>CY Chen</author>\n  <author>Swapnesh Wani</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>isaac_ros_tensor_list_interfaces</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>ament_index_cpp</build_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/src/nitros_data_type.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_tensor_list_type/nitros_data_type.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nnamespace\n{\nusing PrimitiveType = nvidia::gxf::PrimitiveType;\nstd::unordered_map<NitrosDataType, PrimitiveType> data_type_map{\n  {NitrosDataType::kCustom, PrimitiveType::kCustom},\n  {NitrosDataType::kInt8, PrimitiveType::kInt8},\n  {NitrosDataType::kUnsigned8, PrimitiveType::kUnsigned8},\n  {NitrosDataType::kInt16, PrimitiveType::kInt16},\n  {NitrosDataType::kUnsigned16, PrimitiveType::kUnsigned16},\n  {NitrosDataType::kInt32, PrimitiveType::kInt32},\n  {NitrosDataType::kUnsigned32, PrimitiveType::kUnsigned32},\n  {NitrosDataType::kInt64, PrimitiveType::kInt64},\n  {NitrosDataType::kUnsigned64, PrimitiveType::kUnsigned64},\n  {NitrosDataType::kFloat32, PrimitiveType::kFloat32},\n  {NitrosDataType::kFloat64, PrimitiveType::kFloat64},\n  {NitrosDataType::kComplex64, PrimitiveType::kComplex64},\n  {NitrosDataType::kComplex128, PrimitiveType::kComplex128},\n};\n}  // namespace\n\nnvidia::gxf::PrimitiveType GetPrimitiveType(NitrosDataType nitros_data_type)\n{\n  return data_type_map.at(nitros_data_type);\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/src/nitros_tensor_builder.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <cuda_runtime.h>\n\n#include <string>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_builder.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nNitrosTensorBuilder::NitrosTensorBuilder()\n: nitros_tensor_{}\n{\n  auto message = nvidia::gxf::Entity::New(GetTypeAdapterNitrosContext().getContext());\n  if (!message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[constructor] Error initializing new message entity: \" <<\n      GxfResultStr(message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTensorBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  nitros_tensor_.handle = message->eid();\n  GxfEntityRefCountInc(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), message->eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTensorBuilder\"),\n    \"[constructor] NitrosTensor initialized\");\n}\n\nNitrosTensorBuilder & NitrosTensorBuilder::WithShape(NitrosTensorShape tensor_shape)\n{\n  shape_ = tensor_shape;\n  return *this;\n}\n\nNitrosTensorBuilder & NitrosTensorBuilder::WithDataType(NitrosDataType data_type)\n{\n  data_type_ = data_type;\n  return *this;\n}\n\nNitrosTensorBuilder & NitrosTensorBuilder::WithData(void * data)\n{\n  data_ = data;\n  return *this;\n}\n\nNitrosTensorBuilder & NitrosTensorBuilder::WithEvent(cudaEvent_t event)\n{\n  event_ = event;\n  return *this;\n}\n\nNitrosTensorBuilder & NitrosTensorBuilder::WithReleaseCallback(\n  std::function<void()> release_callback)\n{\n  release_callback_ = release_callback;\n  return *this;\n}\n\nnvidia::gxf::Expected<void> ReleaseTensorCallback(\n  std::function<void()> release_callback,\n  void * ptr)\n{\n  if (release_callback) {\n    release_callback();\n  } else {\n    cudaFree(ptr);\n  }\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTensorBuilder\"),\n    \"[ReleaseTensorCallback] Released the cuda memory [%p]\", ptr);\n  return nvidia::gxf::Success;\n}\n\nNitrosTensor NitrosTensorBuilder::Build()\n{\n  auto message = nvidia::gxf::Entity::Shared(\n    GetTypeAdapterNitrosContext().getContext(), nitros_tensor_.handle);\n\n  auto gxf_tensor = message->add<nvidia::gxf::Tensor>();\n\n  auto gxf_data_type = GetPrimitiveType(data_type_);\n\n  // If CUDA event provided, synchronize on that event before building\n  if (event_) {\n    cudaError_t cuda_error = cudaEventSynchronize(event_);\n\n    if (cuda_error != cudaSuccess) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[Build] cudaEventSynchronize failed: \" <<\n        cudaGetErrorName(cuda_error) <<\n        \" (\" << cudaGetErrorString(cuda_error) << \")\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosTensorBuilder\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n\n    cuda_error = cudaEventDestroy(event_);\n    if (cuda_error != cudaSuccess) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[Build] cudaEventDestroy failed: \" <<\n        cudaGetErrorName(cuda_error) <<\n        \" (\" << cudaGetErrorString(cuda_error) << \")\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosTensorBuilder\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n\n  gxf_tensor.value()->wrapMemory(\n    shape_.shape(),\n    gxf_data_type,\n    nvidia::gxf::PrimitiveTypeSize(gxf_data_type),\n    nvidia::gxf::Unexpected{GXF_UNINITIALIZED_VALUE},\n    nvidia::gxf::MemoryStorageType::kDevice, data_,\n    std::bind(&ReleaseTensorCallback, release_callback_, std::placeholders::_1));\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\n      \"NitrosTensorBuilder\"), \"[Build] Tensor built\");\n\n  return nitros_tensor_;\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/src/nitros_tensor_list.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <cuda_runtime.h>\n\n#include <string>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_common/cuda_stream.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\n\nconstexpr char kEntityName[] = \"memory_pool\";\nconstexpr char kComponentName[] = \"unbounded_allocator\";\nconstexpr char kComponentTypeName[] = \"nvidia::gxf::UnboundedAllocator\";\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosTensorList,\n  isaac_ros_tensor_list_interfaces::msg::TensorList>::convert_to_ros_message(\n  const custom_type & source,\n  ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosTensorList::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTensorList\"),\n    \"[convert_to_ros_message] Conversion started for handle = %ld\", source.handle);\n\n  auto msg_entity = nvidia::gxf::Entity::Shared(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), source.handle);\n\n  auto gxf_tensors = msg_entity->findAll<nvidia::gxf::Tensor>();\n  if (!gxf_tensors) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] failed to get all GXF tensors: \" <<\n      GxfResultStr(gxf_tensors.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTensorList\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  for (auto gxf_tensor_handle : gxf_tensors.value()) {\n    auto gxf_tensor = gxf_tensor_handle.value();\n    // Create ROS 2 Tensor and populate the message object's fields\n    auto ros_tensor = isaac_ros_tensor_list_interfaces::msg::Tensor();\n    ros_tensor.name = gxf_tensor.name();\n    ros_tensor.data_type = static_cast<int32_t>(gxf_tensor->element_type());\n    ros_tensor.shape.rank = gxf_tensor->shape().rank();\n\n    // Moving data from GXF tensor to ROS tensor message\n    ros_tensor.data.resize(gxf_tensor->size());\n\n    switch (gxf_tensor->storage_type()) {\n      case nvidia::gxf::MemoryStorageType::kHost:\n        {\n          // This is using non pinned memory, so not need for synchronization inside the same\n          // stream\n          std::memcpy(ros_tensor.data.data(), gxf_tensor->pointer(), gxf_tensor->size());\n        }\n        break;\n      case nvidia::gxf::MemoryStorageType::kDevice:\n        {\n          cudaError_t cuda_error = cudaMemcpyAsync(\n            ros_tensor.data.data(), gxf_tensor->pointer(),\n            gxf_tensor->size(), cudaMemcpyDeviceToHost, source.cuda_stream);\n          if (cuda_error != cudaSuccess) {\n            std::stringstream error_msg;\n            error_msg <<\n              \"[convert_to_ros_message] cudaMemcpy failed for conversion from \"\n              \"gxf::Tensor to ROS Tensor: \" <<\n              cudaGetErrorName(cuda_error) <<\n              \" (\" << cudaGetErrorString(cuda_error) << \")\";\n            RCLCPP_ERROR(\n              rclcpp::get_logger(\"NitrosTensorList\"), error_msg.str().c_str());\n            throw std::runtime_error(error_msg.str().c_str());\n          }\n          cudaError_t cuda_result = cudaStreamSynchronize(source.cuda_stream);\n          CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n        }\n        break;\n      default:\n        std::string error_msg =\n          \"[convert_to_ros_message] MemoryStorageType not supported: conversion from \"\n          \"gxf::Tensor to ROS Tensor failed!\";\n        RCLCPP_ERROR(\n          rclcpp::get_logger(\"NitrosTensorList\"), error_msg.c_str());\n        throw std::runtime_error(error_msg.c_str());\n    }\n\n    for (size_t i = 0; i < gxf_tensor->shape().rank(); i++) {\n      ros_tensor.shape.dims.push_back(gxf_tensor->shape().dimension(i));\n      ros_tensor.strides.push_back(gxf_tensor->stride(i));\n    }\n    destination.tensors.push_back(ros_tensor);\n  }\n\n  // Populate timestamp information back into ROS header\n  auto input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!input_timestamp) {    // Fallback to label 'timestamp'\n    input_timestamp = msg_entity->get<nvidia::gxf::Timestamp>();\n  }\n  if (input_timestamp) {\n    destination.header.stamp.sec = static_cast<int32_t>(\n      input_timestamp.value()->acqtime / static_cast<uint64_t>(1e9));\n    destination.header.stamp.nanosec = static_cast<uint32_t>(\n      input_timestamp.value()->acqtime % static_cast<uint64_t>(1e9));\n  }\n\n  // Set frame ID\n  destination.header.frame_id = source.frame_id;\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTensorList\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosTensorList,\n  isaac_ros_tensor_list_interfaces::msg::TensorList>::convert_to_custom(\n  const ros_message_type & source, custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosTensorList::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTensorList\"),\n    \"[convert_to_custom] Conversion started\");\n\n  // Get pointer to allocator component\n  gxf_uid_t cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kEntityName, kComponentName, kComponentTypeName, cid);\n\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTensorList\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  auto message = nvidia::gxf::Entity::New(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext());\n  if (!message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Error initializing new message entity: \" <<\n      GxfResultStr(message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTensorList\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  for (size_t i = 0; i < source.tensors.size(); i++) {\n    auto ros_tensor = source.tensors[i];\n    auto gxf_tensor = message->add<nvidia::gxf::Tensor>(ros_tensor.name.c_str());\n    std::array<int32_t, nvidia::gxf::Shape::kMaxRank> dims;\n    std::copy(\n      std::begin(ros_tensor.shape.dims), std::end(ros_tensor.shape.dims),\n      std::begin(dims));\n\n    nvidia::gxf::Expected<void> result;\n    nvidia::gxf::MemoryStorageType storage_type = nvidia::gxf::MemoryStorageType::kDevice;\n    // Initializing GXF tensor\n    nvidia::gxf::PrimitiveType type =\n      static_cast<nvidia::gxf::PrimitiveType>(ros_tensor.data_type);\n    RCLCPP_DEBUG(\n      rclcpp::get_logger(\"NitrosTensorList\"),\n      \"[convert_to_custom] dims[0]=%d, rank=%d, storage_type=%d\",\n      dims[0], ros_tensor.shape.rank, (int)storage_type);\n    switch (type) {\n      case nvidia::gxf::PrimitiveType::kUnsigned8:\n        result = gxf_tensor.value()->reshape<uint8_t>(\n          nvidia::gxf::Shape(dims, ros_tensor.shape.rank), storage_type, allocator_handle);\n        break;\n      case nvidia::gxf::PrimitiveType::kInt8:\n        result = gxf_tensor.value()->reshape<int8_t>(\n          nvidia::gxf::Shape(dims, ros_tensor.shape.rank), storage_type, allocator_handle);\n        break;\n      case nvidia::gxf::PrimitiveType::kUnsigned16:\n        result = gxf_tensor.value()->reshape<uint16_t>(\n          nvidia::gxf::Shape(dims, ros_tensor.shape.rank), storage_type, allocator_handle);\n        break;\n      case nvidia::gxf::PrimitiveType::kInt16:\n        result = gxf_tensor.value()->reshape<int16_t>(\n          nvidia::gxf::Shape(dims, ros_tensor.shape.rank), storage_type, allocator_handle);\n        break;\n      case nvidia::gxf::PrimitiveType::kUnsigned32:\n        result = gxf_tensor.value()->reshape<uint32_t>(\n          nvidia::gxf::Shape(dims, ros_tensor.shape.rank), storage_type, allocator_handle);\n        break;\n      case nvidia::gxf::PrimitiveType::kInt32:\n        result = gxf_tensor.value()->reshape<int32_t>(\n          nvidia::gxf::Shape(dims, ros_tensor.shape.rank), storage_type, allocator_handle);\n        break;\n      case nvidia::gxf::PrimitiveType::kUnsigned64:\n        result = gxf_tensor.value()->reshape<uint64_t>(\n          nvidia::gxf::Shape(dims, ros_tensor.shape.rank), storage_type, allocator_handle);\n        break;\n      case nvidia::gxf::PrimitiveType::kInt64:\n        result = gxf_tensor.value()->reshape<int64_t>(\n          nvidia::gxf::Shape(dims, ros_tensor.shape.rank), storage_type, allocator_handle);\n        break;\n      case nvidia::gxf::PrimitiveType::kFloat32:\n        result = gxf_tensor.value()->reshape<float>(\n          nvidia::gxf::Shape(dims, ros_tensor.shape.rank), storage_type, allocator_handle);\n        break;\n      case nvidia::gxf::PrimitiveType::kFloat64:\n        result = gxf_tensor.value()->reshape<double>(\n          nvidia::gxf::Shape(dims, ros_tensor.shape.rank), storage_type, allocator_handle);\n        break;\n      default:\n        std::string error_msg = \"[convert_to_custom] Tensor data type not supported.\";\n        RCLCPP_ERROR(\n          rclcpp::get_logger(\"NitrosTensorList\"), error_msg.c_str());\n        throw std::runtime_error(error_msg.c_str());\n    }\n    if (!result) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[convert_to_custom] Error initializing GXF tensor of type \" <<\n        static_cast<int>(type) << \": \" <<\n        GxfResultStr(result.error());\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosTensorList\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n\n    const cudaMemcpyKind operation = cudaMemcpyHostToDevice;\n    auto nitros_cuda_stream =\n      nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext()\n      .getCudaStreamFromNitrosGraph();\n    const cudaError_t cuda_error = cudaMemcpyAsync(\n      gxf_tensor.value()->pointer(),\n      ros_tensor.data.data(),\n      gxf_tensor.value()->size(),\n      operation,\n      nitros_cuda_stream);\n\n    if (cuda_error != cudaSuccess) {\n      std::stringstream error_msg;\n      error_msg <<\n        \"[convert_to_custom] cudaMemcpy failed for copying data from \"\n        \"ROS Tensor to GXF Tensor: \" <<\n        cudaGetErrorName(cuda_error) <<\n        \" (\" << cudaGetErrorString(cuda_error) << \")\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosTensorList\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n    cudaError_t cuda_result = cudaStreamSynchronize(nitros_cuda_stream);\n    CHECK_CUDA_ERROR(cuda_result, \"Stream was not able to be synchronized\");\n  }\n\n  // Add timestamp to the message\n  uint64_t input_timestamp =\n    source.header.stamp.sec * static_cast<uint64_t>(1e9) +\n    source.header.stamp.nanosec;\n  auto output_timestamp = message->add<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!output_timestamp) {\n    std::stringstream error_msg;\n    error_msg << \"[convert_to_custom] Failed to add a timestamp component to message: \" <<\n      GxfResultStr(output_timestamp.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTensorList\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  output_timestamp.value()->acqtime = input_timestamp;\n\n  // Set frame ID\n  destination.frame_id = source.header.frame_id;\n\n  // Set message entity\n  destination.handle = message->eid();\n  GxfEntityRefCountInc(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), message->eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTensorList\"),\n    \"[convert_to_custom] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/src/nitros_tensor_list_builder.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <cuda_runtime.h>\n\n#include <string>\n#include <vector>\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"gxf/std/tensor.hpp\"\n#include \"gxf/std/timestamp.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list_builder.hpp\"\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_builder.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nNitrosTensorListBuilder::NitrosTensorListBuilder()\n: nitros_tensor_list_{}\n{\n  auto message = nvidia::gxf::Entity::New(GetTypeAdapterNitrosContext().getContext());\n  if (!message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[constructor] Error initializing new message entity: \" <<\n      GxfResultStr(message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTensorListBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  nitros_tensor_list_.handle = message->eid();\n  GxfEntityRefCountInc(\n    nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), message->eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTensorListBuilder\"),\n    \"[constructor] NitrosTensorList initialized\");\n}\n\nNitrosTensorListBuilder & NitrosTensorListBuilder::WithHeader(std_msgs::msg::Header header)\n{\n  auto message = nvidia::gxf::Entity::Shared(\n    GetTypeAdapterNitrosContext().getContext(), nitros_tensor_list_.handle);\n\n  // Set timestamp\n  auto output_timestamp = message->add<nvidia::gxf::Timestamp>(\"timestamp\");\n  if (!output_timestamp) {\n    std::stringstream error_msg;\n    error_msg << \"[WithHeader] Failed to add a timestamp component to message: \" <<\n      GxfResultStr(output_timestamp.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTensorListBuilder\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  output_timestamp.value()->acqtime = header.stamp.sec * static_cast<uint64_t>(1e9) +\n    header.stamp.nanosec;\n\n  // Set frame ID\n  nitros_tensor_list_.frame_id = header.frame_id;\n\n  return *this;\n}\n\nNitrosTensorListBuilder & NitrosTensorListBuilder::AddTensor(std::string name, NitrosTensor tensor)\n{\n  // Get the gxf::Tensor attached to the NitrosTensor\n  auto tensor_message = nvidia::gxf::Entity::Shared(\n    GetTypeAdapterNitrosContext().getContext(), tensor.handle);\n\n  auto gxf_tensor = tensor_message->get<nvidia::gxf::Tensor>();\n\n  // Move the gxf::Tensor to the NitrosTensorList\n  auto tensor_list_message = nvidia::gxf::Entity::Shared(\n    GetTypeAdapterNitrosContext().getContext(), nitros_tensor_list_.handle);\n  auto new_gxf_tensor = tensor_list_message->add<nvidia::gxf::Tensor>(name.c_str());\n  *new_gxf_tensor.value() = std::move(*gxf_tensor.value());\n\n  return *this;\n}\n\nNitrosTensorList NitrosTensorListBuilder::Build()\n{\n  return nitros_tensor_list_;\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/src/nitros_tensor_list_view.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include <string>\n#include <vector>\n\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list_view.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nvoid NitrosTensorListView::InitView()\n{\n  GetAllTensorEntity();\n}\n\nvoid NitrosTensorListView::GetAllTensorEntity()\n{\n  auto gxf_tensors = msg_entity_->findAll<gxf::Tensor>();\n  if (!gxf_tensors) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[GetTensorCount] failed to get all GXF tensors: \" <<\n      GxfResultStr(gxf_tensors.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTensorListView\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  tensor_list_ = gxf_tensors.value();\n}\n\nsize_t NitrosTensorListView::GetTensorCount() const\n{\n  return tensor_list_.size();\n}\n\nconst std::vector<NitrosTensorListView::NitrosTensorView> NitrosTensorListView::GetAllTensor() const\n{\n  std::vector<NitrosTensorListView::NitrosTensorView> tensor_view_list;\n  auto gxf_tensors = msg_entity_->findAll<gxf::Tensor>();\n  if (!gxf_tensors) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[GetTensorCount] failed to get all GXF tensors: \" <<\n      GxfResultStr(gxf_tensors.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTensorListView\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n\n  for (auto gxf_tensor : gxf_tensors.value()) {\n    tensor_view_list.push_back(\n      NitrosTensorListView::NitrosTensorView(*(gxf_tensor.value()), gxf_tensor.value().name()));\n  }\n  return tensor_view_list;\n}\n\nconst NitrosTensorListView::NitrosTensorView NitrosTensorListView::GetAnyNamedTensor(\n  std::string tensor_name) const\n{\n  auto gxf_tensor = msg_entity_->get<gxf::Tensor>(tensor_name.c_str());\n  if (!gxf_tensor) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[GetAnyNamedTensor] failed to get GXF tensor of name : [\" << tensor_name << \"] : \" <<\n      GxfResultStr(gxf_tensor.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTensorListView\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  return NitrosTensorListView::NitrosTensorView(*(gxf_tensor.value()), tensor_name);\n}\n\nconst NitrosTensorListView::NitrosTensorView NitrosTensorListView::GetNamedTensor(\n  std::string tensor_name) const\n{\n  size_t tensor_count = 0;\n  // Throw error in case of more than 1 tensor with the same name\n  for (auto tensor : tensor_list_) {\n    std::string name = std::string(tensor.value().name());\n    if (name != tensor_name) {continue;}\n    if (name == tensor_name) {++tensor_count;}\n    if (tensor_count > 1) {\n      std::stringstream error_msg;\n      error_msg << \"[GetNamedTensor] failed to get unique GXF tensor of name : [\" <<\n        tensor_name << \\\n        \"]. More than 1 exists.\";\n      RCLCPP_ERROR(\n        rclcpp::get_logger(\"NitrosTensorListView\"), error_msg.str().c_str());\n      throw std::runtime_error(error_msg.str().c_str());\n    }\n  }\n  return GetAnyNamedTensor(tensor_name);\n}\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2022-2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/test/isaac_ros_nitros_tensor_list_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport time\n\nfrom isaac_ros_tensor_list_interfaces.msg import Tensor, TensorList, TensorShape\nfrom isaac_ros_test import IsaacROSBaseTest\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\nimport numpy as np\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with NitrosNode test node.\"\"\"\n    test_ns = IsaacROSNitrosTensorListTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='image_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_tensor_list_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosTensorListForwardNode',\n                name='NitrosTensorListForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_tensor_list_nchw'\n                }]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosTensorListTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosTensorListTest(IsaacROSBaseTest):\n    \"\"\"\n    Proof-of-Life Test for Isaac ROS Nitros Node.\n\n    1. Sets up ROS publisher to send TensorList values\n    2. Sets up ROS subscriber to listen to output channel of NitrosNode\n    3. Verify received messages\n    \"\"\"\n\n    def test_forward_node(self) -> None:\n        self.node._logger.info('Starting Isaac ROS NitrosNode POL Test')\n\n        # Subscriber\n        received_messages = {}\n\n        subscriber_topic_namespace = self.generate_namespace('topic_forward_output')\n        test_subscribers = [\n            (subscriber_topic_namespace, TensorList)\n        ]\n\n        subs = self.create_logging_subscribers(\n            subscription_requests=test_subscribers,\n            received_messages=received_messages,\n            use_namespace_lookup=False,\n            accept_multiple_messages=True,\n            add_received_message_timestamps=True\n        )\n\n        # Publisher\n        publisher_topic_namespace = self.generate_namespace('topic_forward_input')\n        pub = self.node.create_publisher(\n            TensorList,\n            publisher_topic_namespace,\n            self.DEFAULT_QOS)\n\n        try:\n            # Construct test tensor list\n            DATA_TYPE = 9\n            INPUT_TENSOR_DIMENSIONS = [1, 3, 100, 100]\n            INPUT_TENSOR_NAME = 'input'\n            INPUT_TENSOR_STRIDE = 4\n\n            tensor_list = TensorList()\n            tensor = Tensor()\n            tensor_shape = TensorShape()\n\n            tensor_shape.rank = len(INPUT_TENSOR_DIMENSIONS)\n            tensor_shape.dims = INPUT_TENSOR_DIMENSIONS\n\n            tensor.shape = tensor_shape\n            tensor.name = INPUT_TENSOR_NAME\n            tensor.data_type = DATA_TYPE\n            tensor.strides = []\n\n            data_length = INPUT_TENSOR_STRIDE * np.prod(INPUT_TENSOR_DIMENSIONS)\n            tensor.data = np.random.randint(256, size=data_length).tolist()\n\n            tensor_list.tensors = [tensor]\n            timestamp = self.node.get_clock().now().to_msg()\n            tensor_list.header.stamp = timestamp\n\n            # Start sending messages\n            self.node.get_logger().info('Start publishing messages')\n            sent_count = 0\n            end_time = time.time() + 2.0\n            while time.time() < end_time:\n                sent_count += 1\n                pub.publish(tensor_list)\n                rclpy.spin_once(self.node, timeout_sec=0.2)\n\n            # Conclude the test\n            received_count = len(received_messages[subscriber_topic_namespace])\n            self.node._logger.info(\n                f'Test Results:\\n'\n                f'# of Messages Sent: {sent_count}\\n'\n                f'# of Messages Received: {received_count}\\n'\n                f'# of Messages Dropped: {sent_count - received_count}\\n'\n                f'Message Drop Rate: {((sent_count-received_count)/sent_count)*100}%'\n            )\n\n            received_tensor_list = received_messages[subscriber_topic_namespace][-1][0]\n            self.assertEqual(str(timestamp), str(received_tensor_list.header.stamp),\n                             'Timestamps do not match.')\n\n            self.assertGreater(len(received_messages[subscriber_topic_namespace]), 0)\n            for i in range(data_length):\n                self.assertEqual(\n                    received_tensor_list.tensors[0].data[i],\n                    tensor.data[i])\n\n            self.node._logger.info('Source and received tensor lists are matched.')\n\n        finally:\n            [self.node.destroy_subscription(sub) for sub in subs]\n            self.assertTrue(self.node.destroy_publisher(pub))\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_tensor_list_type/test/src/nitros_tensor_list_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_tensor_list_type/nitros_tensor_list.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_tensor_list_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_tensor_list_nchw\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosTensorListForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosTensorListForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosTensorList>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosTensorListForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_twist_type/.vscode/settings.json",
    "content": "{\n    \"files.associations\": {\n        \"array\": \"cpp\",\n        \"atomic\": \"cpp\",\n        \"bit\": \"cpp\",\n        \"*.tcc\": \"cpp\",\n        \"cctype\": \"cpp\",\n        \"clocale\": \"cpp\",\n        \"cmath\": \"cpp\",\n        \"compare\": \"cpp\",\n        \"concepts\": \"cpp\",\n        \"cstdarg\": \"cpp\",\n        \"cstddef\": \"cpp\",\n        \"cstdint\": \"cpp\",\n        \"cstdio\": \"cpp\",\n        \"cstdlib\": \"cpp\",\n        \"cwchar\": \"cpp\",\n        \"cwctype\": \"cpp\",\n        \"deque\": \"cpp\",\n        \"string\": \"cpp\",\n        \"unordered_map\": \"cpp\",\n        \"vector\": \"cpp\",\n        \"exception\": \"cpp\",\n        \"algorithm\": \"cpp\",\n        \"functional\": \"cpp\",\n        \"iterator\": \"cpp\",\n        \"memory\": \"cpp\",\n        \"memory_resource\": \"cpp\",\n        \"numeric\": \"cpp\",\n        \"random\": \"cpp\",\n        \"string_view\": \"cpp\",\n        \"system_error\": \"cpp\",\n        \"tuple\": \"cpp\",\n        \"type_traits\": \"cpp\",\n        \"utility\": \"cpp\",\n        \"initializer_list\": \"cpp\",\n        \"iosfwd\": \"cpp\",\n        \"limits\": \"cpp\",\n        \"new\": \"cpp\",\n        \"numbers\": \"cpp\",\n        \"ostream\": \"cpp\",\n        \"stdexcept\": \"cpp\",\n        \"streambuf\": \"cpp\",\n        \"typeinfo\": \"cpp\"\n    }\n}"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_twist_type/CMakeLists.txt",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\ncmake_minimum_required(VERSION 3.22.1)\nproject(isaac_ros_nitros_twist_type LANGUAGES C CXX)\n\nif(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"Clang\")\n  add_compile_options(-Wall -Wextra -Wpedantic)\nendif()\n\nfind_package(ament_cmake_auto REQUIRED)\nament_auto_find_build_dependencies()\n\n# Ensure CUDA imported targets (e.g., CUDA::nvtx3) are available\nfind_package(CUDAToolkit REQUIRED)\n\n# NVTX\noption(USE_NVTX \"Enable NVTX markers for improved profiling (if available)\" ON)\nif(USE_NVTX)\n    add_definitions(-DUSE_NVTX)\n    link_directories(\"${CUDA_TOOLKIT_ROOT_DIR}/lib64\")\n    link_libraries(\"CUDA::nvtx3\")\nendif()\n\n# Dependencies\nfind_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(yaml-cpp)\n\n# NitrosTwist\nament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_twist.cpp)\ntarget_link_libraries(${PROJECT_NAME}\n  Eigen3::Eigen\n  yaml-cpp\n)\nset_target_properties(${PROJECT_NAME} PROPERTIES\n  BUILD_WITH_INSTALL_RPATH TRUE\n  BUILD_RPATH_USE_ORIGIN TRUE\n  INSTALL_RPATH_USE_LINK_PATH TRUE)\n\nif(BUILD_TESTING)\n  # Install test/config directory\n  install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test)\n\n  # NitrosTwistForwardNode\n  ament_auto_add_library(isaac_ros_nitros_twist_forward_node SHARED test/src/nitros_twist_forward_node.cpp)\n  target_link_libraries(isaac_ros_nitros_twist_forward_node ${PROJECT_NAME})\n  set_target_properties(isaac_ros_nitros_twist_forward_node PROPERTIES\n    BUILD_WITH_INSTALL_RPATH TRUE\n    BUILD_RPATH_USE_ORIGIN TRUE\n    INSTALL_RPATH_USE_LINK_PATH TRUE)\n  rclcpp_components_register_nodes(isaac_ros_nitros_twist_forward_node\n    \"nvidia::isaac_ros::nitros::NitrosTwistForwardNode\")\n  set(node_plugins \"${node_plugins}nvidia::isaac_ros::nitros::NitrosTwistForwardNode;\\\n    $<TARGET_FILE:isaac_ros_nitros_twist_forward_node>\\n\")\n\n  find_package(ament_lint_auto REQUIRED)\n\n  # Ignore copyright notices since we use custom NVIDIA Isaac ROS Software License\n  set(ament_cmake_copyright_FOUND TRUE)\n  ament_lint_auto_find_test_dependencies()\n\n\n  # The FindPythonInterp and FindPythonLibs modules are removed\n  if(POLICY CMP0148)\n    cmake_policy(SET CMP0148 OLD)\n  endif()\n\n  find_package(launch_testing_ament_cmake REQUIRED)\n  add_launch_test(test/isaac_ros_nitros_twist_type_test_pol.py TIMEOUT \"15\")\nendif()\n\n\n# Embed versioning information into installed files\nament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)\ninclude(\"${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake\")\ngenerate_version_info(${PROJECT_NAME})\n\nament_auto_package()\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_twist_type/include/isaac_ros_nitros_twist_type/nitros_twist.hpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#ifndef ISAAC_ROS_NITROS_TWIST_TYPE__NITROS_TWIST_HPP_\n#define ISAAC_ROS_NITROS_TWIST_TYPE__NITROS_TWIST_HPP_\n/*\n * Type adaptation for:\n *   Nitros type: NitrosTwist\n *   ROS type:    geometry_msgs::msg::Twist\n */\n\n#include <string>\n\n#include \"isaac_ros_nitros/types/nitros_format_agent.hpp\"\n\n#include \"rclcpp/type_adapter.hpp\"\n#include \"geometry_msgs/msg/twist.hpp\"\n\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\n// Type forward declaration\nstruct NitrosTwist;\n\n// Formats\nstruct nitros_twist_t\n{\n  using MsgT = NitrosTwist;\n  static const inline std::string supported_type_name = \"nitros_twist\";\n};\n\n// NITROS data type registration factory\nNITROS_TYPE_FACTORY_BEGIN(NitrosTwist)\n// Supported data formats\nNITROS_FORMAT_FACTORY_BEGIN()\nNITROS_FORMAT_ADD(nitros_twist_t)\nNITROS_FORMAT_FACTORY_END()\n// Required extensions\nNITROS_TYPE_EXTENSION_FACTORY_BEGIN()\nNITROS_TYPE_EXTENSION_ADD(\"isaac_ros_gxf\", \"gxf/lib/serialization/libgxf_serialization.so\")\n// for nvidia::isaac::StringProvider\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_gxf_helpers\", \"gxf/lib/libgxf_isaac_gxf_helpers.so\")\n// for nvidia::isaac::SightReceiver\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_sight\", \"gxf/lib/libgxf_isaac_sight.so\")\n// for nvidia::isaac::PoseTree\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_atlas\", \"gxf/lib/libgxf_isaac_atlas.so\")\nNITROS_TYPE_EXTENSION_ADD(\"gxf_isaac_messages\", \"gxf/lib/libgxf_isaac_messages.so\")\nNITROS_TYPE_EXTENSION_FACTORY_END()\nNITROS_TYPE_FACTORY_END()\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\n\ntemplate<>\nstruct rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosTwist,\n  geometry_msgs::msg::Twist>\n{\n  using is_specialized = std::true_type;\n  using custom_type = nvidia::isaac_ros::nitros::NitrosTwist;\n  using ros_message_type = geometry_msgs::msg::Twist;\n\n  static void convert_to_ros_message(\n    const custom_type & source,\n    ros_message_type & destination);\n\n  static void convert_to_custom(\n    const ros_message_type & source,\n    custom_type & destination);\n};\n\nRCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(\n  nvidia::isaac_ros::nitros::NitrosTwist,\n  geometry_msgs::msg::Twist);\n\n#endif  // ISAAC_ROS_NITROS_TWIST_TYPE__NITROS_TWIST_HPP_\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_twist_type/package.xml",
    "content": "<?xml version=\"1.0\"?>\n\n<!--\nCopyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n\nNVIDIA CORPORATION and its licensors retain all intellectual property\nand proprietary rights in and to this software, related documentation\nand any modifications thereto.  Any use, reproduction, disclosure or\ndistribution of this software and related documentation without an express\nlicense agreement from NVIDIA CORPORATION is strictly prohibited.\n-->\n\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_nitros_twist_type</name>\n  <version>4.4.0</version>\n  <description>Isaac ROS NITROS Twist Type</description>\n\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>NVIDIA Isaac ROS Software License</license>\n  <url type=\"website\">https://developer.nvidia.com/isaac-ros-gems/</url>\n  <author>Ashwin Varghese Kuruttukulam</author>\n\n  <buildtool_depend>ament_cmake</buildtool_depend>\n\n  <depend>rclcpp</depend>\n  <depend>rclcpp_components</depend>\n  <depend>geometry_msgs</depend>\n  <depend>isaac_ros_nitros</depend>\n  <depend>gxf_isaac_messages</depend>\n  <depend>gxf_isaac_atlas</depend>\n  <depend>gxf_isaac_gxf_helpers</depend>\n\n  <build_depend>isaac_ros_common</build_depend>\n  <build_depend>gxf_isaac_gems</build_depend>\n\n  <exec_depend>gxf_isaac_sight</exec_depend>\n\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>isaac_ros_test</test_depend>\n\n  <export>\n    <build_type>ament_cmake</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_twist_type/src/nitros_twist.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wunused-parameter\"\n#pragma GCC diagnostic ignored \"-Wmissing-field-initializers\"\n#pragma GCC diagnostic ignored \"-Wpedantic\"\n#include \"gxf/core/entity.hpp\"\n#include \"gxf/core/gxf.h\"\n#include \"extensions/messages/helpers.hpp\"\n#include \"extensions/messages/composite_message.hpp\"\n#include \"gems/control_types/differential_drive.hpp\"\n#pragma GCC diagnostic pop\n\n#include \"isaac_ros_nitros_twist_type/nitros_twist.hpp\"\n#include \"isaac_ros_nitros/types/type_adapter_nitros_context.hpp\"\n\n#include \"rclcpp/rclcpp.hpp\"\n\nnamespace\n{\nconstexpr int kLinearSpeedIndx = 0;\nconstexpr int kAngularSpeedIndx = 1;\nconstexpr int DifferentialBaseCommandIndices = 2;\n}  // namespace\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosTwist,\n  geometry_msgs::msg::Twist>::convert_to_ros_message(\n  const custom_type & source, ros_message_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosTwist::convert_to_ros_message\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTwist\"),\n    \"[convert_to_ros_message] Conversion started for handle=%ld\", source.handle);\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n  auto msg_entity = nvidia::gxf::Entity::Shared(context, source.handle);\n\n  // Create composite msg and populate values\n  auto maybe_composite_message = nvidia::isaac::GetCompositeMessage(msg_entity.value());\n  if (!maybe_composite_message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get maybe_composite_message gxf message\"\n      \" from message entity: \" <<\n      GxfResultStr(maybe_composite_message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTwist\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto composite_message = maybe_composite_message.value();\n\n  // Populate linear and angular speeds of the ROS msg from gxf msg\n  destination.linear.x = composite_message.view(0, kLinearSpeedIndx);\n  destination.angular.z = composite_message.view(0, kAngularSpeedIndx);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTwist\"),\n    \"[convert_to_ros_message] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n\nvoid rclcpp::TypeAdapter<\n  nvidia::isaac_ros::nitros::NitrosTwist,\n  geometry_msgs::msg::Twist>::convert_to_custom(\n  const ros_message_type & source,\n  custom_type & destination)\n{\n  nvidia::isaac_ros::nitros::nvtxRangePushWrapper(\n    \"NitrosTwist::convert_to_custom\",\n    nvidia::isaac_ros::nitros::CLR_PURPLE);\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTwist\"),\n    \"[convert_to_custom] Conversion started\");\n\n  auto context = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext();\n\n  // Get pointer to allocator component\n  const std::string kAllocatorEntityName = \"memory_pool\";\n  const std::string kAllocatorComponentName = \"unbounded_allocator\";\n  const std::string kAllocatorComponentTypeName = \"nvidia::gxf::UnboundedAllocator\";\n  gxf_uid_t allocator_cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kAllocatorEntityName, kAllocatorComponentName, kAllocatorComponentTypeName, allocator_cid);\n  auto maybe_allocator_handle =\n    nvidia::gxf::Handle<nvidia::gxf::Allocator>::Create(context, allocator_cid);\n  if (!maybe_allocator_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_custom] Failed to get allocator's handle: \" <<\n      GxfResultStr(maybe_allocator_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTwist\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto allocator_handle = maybe_allocator_handle.value();\n\n  // Create composite msg and populate values\n  auto maybe_composite_message = nvidia::isaac::CreateCompositeMessage(\n    context, allocator_handle, 1, DifferentialBaseCommandIndices);\n  if (!maybe_composite_message) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get maybe_composite_message gxf message\"\n      \" from message entity: \" <<\n      GxfResultStr(maybe_composite_message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTwist\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto composite_message = maybe_composite_message.value();\n\n  // Get pointer to composite_schema_server component\n  const std::string kSchemaServerEntityName = \"global_pose_tree\";\n  const std::string kSchemaServerComponentName = \"composite_schema_server\";\n  const std::string kSchemaServerComponentTypeName = \"nvidia::isaac::CompositeSchemaServer\";\n  gxf_uid_t schema_server_cid;\n  nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid(\n    kSchemaServerEntityName, kSchemaServerComponentName,\n    kSchemaServerComponentTypeName, schema_server_cid);\n  auto maybe_schema_server_handle =\n    nvidia::gxf::Handle<nvidia::isaac::CompositeSchemaServer>::Create(context, schema_server_cid);\n  if (!maybe_schema_server_handle) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Failed to get maybe_schema_server_handle gxf message\"\n      \" from message entity: \" <<\n      GxfResultStr(maybe_schema_server_handle.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTwist\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  auto schema_server_handle = maybe_schema_server_handle.value();\n\n  // Add schema to server if it doesn't already exist\n  auto maybe_schema_uid = schema_server_handle->add(\n    nvidia::isaac::DifferentialBaseCommandCompositeSchema());\n  if (!maybe_schema_uid) {\n    std::stringstream error_msg;\n    error_msg <<\n      \"[convert_to_ros_message] Cannot add schema to server\" <<\n      GxfResultStr(maybe_composite_message.error());\n    RCLCPP_ERROR(\n      rclcpp::get_logger(\"NitrosTwist\"), error_msg.str().c_str());\n    throw std::runtime_error(error_msg.str().c_str());\n  }\n  // Set schema uid for the gxf msg\n  composite_message.composite_schema_uid->uid = maybe_schema_uid.value();\n\n  // Populate linear and angular speeds of the gxf msg from ROS msg\n  composite_message.view(0, kLinearSpeedIndx) = source.linear.x;\n  composite_message.view(0, kAngularSpeedIndx) = source.angular.z;\n\n  // Set Entity Id\n  destination.handle = composite_message.message.eid();\n  GxfEntityRefCountInc(context, composite_message.message.eid());\n\n  RCLCPP_DEBUG(\n    rclcpp::get_logger(\"NitrosTwist\"),\n    \"[convert_to_custom] Conversion completed\");\n\n  nvidia::isaac_ros::nitros::nvtxRangePopWrapper();\n}\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_twist_type/test/config/test_forward_node.yaml",
    "content": "%YAML 1.2\n# Copyright (c) 2023, NVIDIA CORPORATION.  All rights reserved.\n#\n# NVIDIA CORPORATION and its licensors retain all intellectual property\n# and proprietary rights in and to this software, related documentation\n# and any modifications thereto.  Any use, reproduction, disclosure or\n# distribution of this software and related documentation without an express\n# license agreement from NVIDIA CORPORATION is strictly prohibited.\n---\nname: forward\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- name: output\n  type: nvidia::gxf::DoubleBufferTransmitter\n- type: nvidia::isaac_ros::MessageForward\n  parameters:\n    in: input\n    out: output\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n---\nname: sink\ncomponents:\n- name: input\n  type: nvidia::gxf::DoubleBufferReceiver\n  parameters:\n    capacity: 1\n- type: nvidia::gxf::MessageAvailableSchedulingTerm\n  parameters:\n    receiver: input\n    min_size: 1\n- name: sink\n  type: nvidia::isaac_ros::MessageRelay\n  parameters:\n    source: input\n---\ncomponents:\n- type: nvidia::gxf::Connection\n  parameters:\n    source: forward/output\n    target: sink/input\n---\ncomponents:\n- type: nvidia::gxf::GreedyScheduler\n  parameters:\n    clock: clock\n    stop_on_deadlock: false\n    check_recession_period_us: 100\n- name: clock\n  type: nvidia::gxf::RealtimeClock"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_twist_type/test/isaac_ros_nitros_twist_type_test_pol.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\"\"\"Proof-of-Life test for the NitrosTwist type adapter.\"\"\"\n\nimport os\nimport pathlib\nimport time\n\nfrom geometry_msgs.msg import Twist\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\n\nimport launch\nfrom launch_ros.actions import ComposableNodeContainer\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing\n\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    test_ns = IsaacROSNitrosTwistTest.generate_namespace()\n    container = ComposableNodeContainer(\n        name='test_container',\n        namespace='isaac_ros_nitros_container',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[\n            ComposableNode(\n                package='isaac_ros_nitros_twist_type',\n                plugin='nvidia::isaac_ros::nitros::NitrosTwistForwardNode',\n                name='NitrosTwistForwardNode',\n                namespace=test_ns,\n                parameters=[{\n                    'compatible_format': 'nitros_twist'\n                }],\n                remappings=[\n                    (test_ns+'/topic_forward_input', test_ns+'/input'),\n                    (test_ns+'/topic_forward_output', test_ns+'/output'),\n                ]\n            ),\n        ],\n        output='both',\n        arguments=['--ros-args', '--log-level', 'info'],\n    )\n\n    return IsaacROSNitrosTwistTest.generate_test_description([\n        container,\n        launch.actions.TimerAction(\n            period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n    ])\n\n\nclass IsaacROSNitrosTwistTest(IsaacROSBaseTest):\n    \"\"\"Validate NitrosTwist type adapter.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_twist')\n    def test_nitros_twist_type_conversions(self, test_folder) -> None:\n        \"\"\"Expect the tag from NitrosTwist type to be compatible with source.\"\"\"\n        self.generate_namespace_lookup(['input', 'output'])\n        received_messages = {}\n\n        received_message_sub = self.create_logging_subscribers(\n            subscription_requests=[('output', Twist)],\n            received_messages=received_messages\n        )\n\n        twist_pub = self.node.create_publisher(\n            Twist, self.namespaces['input'], self.DEFAULT_QOS)\n\n        try:\n            twist = self.load_twist_from_json(test_folder / 'twist.json')\n\n            # Wait at most TIMEOUT seconds for subscriber to respond\n            TIMEOUT = 10\n            end_time = time.time() + TIMEOUT\n\n            done = False\n            while time.time() < end_time:\n\n                twist_pub.publish(twist)\n\n                rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                # If we have received a message on the output topic, break\n                if 'output' in received_messages:\n                    done = True\n                    break\n\n            self.assertTrue(done, \"Didn't receive output on the output topic!\")\n\n            received_twist = received_messages['output']\n\n            # Only test for data passed through gxf\n            self.assertEqual(twist.linear.x,\n                             received_twist.linear.x)\n            self.assertEqual(twist.angular.z,\n                             received_twist.angular.z)\n            print('The received twist message has been verified successfully')\n        finally:\n            self.node.destroy_subscription(received_message_sub)\n            self.node.destroy_publisher(twist_pub)\n\n    @staticmethod\n    def load_twist_from_json(\n            json_filepath: pathlib.Path) -> Twist:\n        \"\"\"\n        Load a Twist message from a JSON filepath.\n\n        Parameters\n        ----------\n        json_filepath : Path\n            The path to a JSON file containing the Twist fields\n\n        Returns\n        -------\n        Twist\n            Generated Twist message\n\n        \"\"\"\n        twist_json = JSONConversion.load_from_json(\n            json_filepath)\n\n        twist = Twist()\n        twist.linear.x = twist_json['linear']['x']\n        twist.linear.y = twist_json['linear']['y']\n        twist.linear.z = twist_json['linear']['z']\n        twist.angular.x = twist_json['angular']['x']\n        twist.angular.y = twist_json['angular']['y']\n        twist.angular.z = twist_json['angular']['z']\n        return twist\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_twist_type/test/src/nitros_twist_forward_node.cpp",
    "content": "// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n// Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n//\n// Licensed under the Apache License, Version 2.0 (the \"License\");\n// you may not use this file except in compliance with the License.\n// You may obtain a copy of the License at\n//\n// http://www.apache.org/licenses/LICENSE-2.0\n//\n// Unless required by applicable law or agreed to in writing, software\n// distributed under the License is distributed on an \"AS IS\" BASIS,\n// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n// See the License for the specific language governing permissions and\n// limitations under the License.\n//\n// SPDX-License-Identifier: Apache-2.0\n\n#include \"isaac_ros_nitros_twist_type/nitros_twist.hpp\"\n#include \"isaac_ros_nitros/nitros_node.hpp\"\n\n#include \"rclcpp_components/register_node_macro.hpp\"\n\nnamespace nvidia\n{\nnamespace isaac_ros\n{\nnamespace nitros\n{\n\nconstexpr char PACKAGE_NAME[] = \"isaac_ros_nitros_twist_type\";\nconstexpr char FORWARD_FORMAT[] = \"nitros_twist\";\n\n#pragma GCC diagnostic push\n#pragma GCC diagnostic ignored \"-Wpedantic\"\nclass NitrosTwistForwardNode : public NitrosNode\n{\npublic:\n  explicit NitrosTwistForwardNode(const rclcpp::NodeOptions & options)\n  : NitrosNode(\n      options,\n      // Application graph filename\n      \"test/config/test_forward_node.yaml\",\n      // I/O configuration map\n        {\n          {\"forward/input\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_input\",\n              .use_compatible_format_only = true,\n            }\n          },\n          {\"sink/sink\",\n            {\n              .type = NitrosPublisherSubscriberType::NEGOTIATED,\n              .qos = rclcpp::QoS(1),\n              .compatible_data_format = FORWARD_FORMAT,\n              .topic_name = \"topic_forward_output\",\n              .use_compatible_format_only = true,\n            }\n          }\n        },\n      // Extension specs\n      {},\n      // Optimizer's rule filenames\n      {},\n      // Extension so file list\n        {\n          {\"gxf_isaac_message_compositor\", \"gxf/lib/libgxf_isaac_message_compositor.so\"}\n        },\n      // Test node package name\n      PACKAGE_NAME)\n  {\n    std::string compatible_format = declare_parameter<std::string>(\"compatible_format\", \"\");\n    if (!compatible_format.empty()) {\n      config_map_[\"forward/input\"].compatible_data_format = compatible_format;\n      config_map_[\"sink/sink\"].compatible_data_format = compatible_format;\n    }\n\n    registerSupportedType<nvidia::isaac_ros::nitros::NitrosTwist>();\n\n    startNitrosNode();\n  }\n};\n#pragma GCC diagnostic pop\n\n}  // namespace nitros\n}  // namespace isaac_ros\n}  // namespace nvidia\n\nRCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosTwistForwardNode)\n"
  },
  {
    "path": "isaac_ros_nitros_type/isaac_ros_nitros_twist_type/test/test_cases/nitros_twist/profile/twist.json",
    "content": "{\n  \"linear\": {\n    \"x\": 1.1,\n    \"y\": 2.4,\n    \"z\": 5.6\n  },\n  \"angular\": {\n    \"x\": 52.3,\n    \"y\": 43.1,\n    \"z\": 32.1\n  }\n}"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/__init__.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/examples/__init__.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/examples/pynitros_dnn_image_encoder_node.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom isaac_ros_nitros_bridge_interfaces.msg import NitrosBridgeImage, NitrosBridgeTensorList\nfrom isaac_ros_pynitros.isaac_ros_pynitros_publisher import PyNitrosPublisher\nfrom isaac_ros_pynitros.isaac_ros_pynitros_subscriber import PyNitrosSubscriber\nfrom isaac_ros_pynitros.pynitros_type_builders.pynitros_tensor_list_builder \\\n    import PyNitrosTensorListBuilder\n\n\nimport rclpy\nfrom rclpy.node import Node\nfrom std_msgs.msg import Header\n\nimport torch\nimport torchvision.transforms as T\n\n\nclass PyNITROSDNNImageEncoderNode(Node):\n    \"\"\"A PyNITROS node listens to image message and publish it again.\"\"\"\n\n    def __init__(self, name='pynitros_image_forward_node'):\n        super().__init__(name)\n        self.declare_parameter('enable_ros_subscribe', False)\n        self.declare_parameter('enable_ros_publish', False)\n        self.declare_parameter('network_image_width', 960)\n        self.declare_parameter('network_image_height', 544)\n        self.declare_parameter('image_mean', [0.5, 0.5, 0.5])\n        self.declare_parameter('image_std', [0.5, 0.5, 0.5])\n\n        self.enable_ros_subscribe = \\\n            self.get_parameter('enable_ros_subscribe').get_parameter_value().bool_value\n        self.enable_ros_publish = \\\n            self.get_parameter('enable_ros_publish').get_parameter_value().bool_value\n\n        self.network_image_width = \\\n            self.get_parameter('network_image_width').get_parameter_value().integer_value\n        self.network_image_height = \\\n            self.get_parameter('network_image_height').get_parameter_value().integer_value\n\n        self.image_mean = \\\n            self.get_parameter('image_mean').get_parameter_value().double_array_value\n        self.image_std = \\\n            self.get_parameter('image_std').get_parameter_value().double_array_value\n\n        self.pynitros_subscriber = PyNitrosSubscriber(\n            self, NitrosBridgeImage, 'pynitros_input_msg',\n            enable_ros_subscribe=self.enable_ros_subscribe)\n        self.pynitros_subscriber.create_subscription(self.listener_callback)\n\n        self.pynitros_publisher = PyNitrosPublisher(\n            self, NitrosBridgeTensorList, 'pynitros_output_msg', 'pynitros_output_msg_ros')\n\n        self.pynitros_tensor_list_builder = PyNitrosTensorListBuilder(\n            num_buffer=40, timeout=5)\n\n    def listener_callback(self, pynitros_image_view):\n        header = Header()\n        header.frame_id = pynitros_image_view.get_frame_id()\n        header.stamp.sec = pynitros_image_view.get_timestamp_seconds()\n        header.stamp.nanosec = pynitros_image_view.get_timestamp_nanoseconds()\n\n        tensor_image = torch.as_tensor(pynitros_image_view, device='cuda', dtype=torch.uint8)\n        reshaped_image = tensor_image.reshape(pynitros_image_view.get_height(),\n                                              pynitros_image_view.get_width(),\n                                              3)\n\n        # Switch from interleaved to planar\n        planar_tensor = reshaped_image.permute(2, 0, 1)\n\n        transforms = T.Compose([\n            T.Resize((self.network_image_height, self.network_image_width)),\n            T.ConvertImageDtype(torch.float),\n            T.Normalize(self.image_mean, self.image_std)\n        ]\n        )\n        norm_tensor = transforms(planar_tensor)\n        out_tensor = norm_tensor.contiguous()\n\n        tensor_list = []\n        pynitros_tensor = self.pynitros_tensor_list_builder.build_tensor(\n            out_tensor.data_ptr(),\n            'input_tensor',\n            out_tensor.shape,\n            9)\n        tensor_list.append(pynitros_tensor)\n\n        # # Build TensorList\n        pynitros_tensor_list = self.pynitros_tensor_list_builder.build(tensor_list,\n                                                                       header,\n                                                                       0,\n                                                                       self.enable_ros_publish)\n\n        self.pynitros_publisher.publish(pynitros_tensor_list)\n\n\ndef main(args=None):\n    try:\n        rclpy.init(args=args)\n        node = PyNITROSDNNImageEncoderNode()\n        rclpy.spin(node)\n    except KeyboardInterrupt:\n        pass\n    finally:\n        node.destroy_node()\n        # only shut down if context is active\n        if rclpy.ok():\n            rclpy.shutdown()\n\n\nif __name__ == '__main__':\n    main()\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/examples/pynitros_image_forward_node.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom isaac_ros_nitros_bridge_interfaces.msg import NitrosBridgeImage\nfrom isaac_ros_pynitros.isaac_ros_pynitros_publisher import PyNitrosPublisher\nfrom isaac_ros_pynitros.isaac_ros_pynitros_subscriber import PyNitrosSubscriber\nfrom isaac_ros_pynitros.pynitros_type_builders.pynitros_image_builder import PyNitrosImageBuilder\n\nimport rclpy\nfrom rclpy.node import Node\nfrom std_msgs.msg import Header\n\n\nclass PyNITROSImageForwardNode(Node):\n    \"\"\"A PyNITROS node listens to image message and publish it again.\"\"\"\n\n    def __init__(self, name='pynitros_image_forward_node'):\n        super().__init__(name)\n        self.declare_parameter('enable_ros_subscribe', False)\n        self.declare_parameter('enable_ros_publish', False)\n\n        self.enable_ros_subscribe = \\\n            self.get_parameter('enable_ros_subscribe').get_parameter_value().bool_value\n        self.enable_ros_publish = \\\n            self.get_parameter('enable_ros_publish').get_parameter_value().bool_value\n\n        self.pynitros_subscriber = PyNitrosSubscriber(\n            self, NitrosBridgeImage, 'pynitros_input_msg',\n            enable_ros_subscribe=self.enable_ros_subscribe)\n\n        self.pynitros_subscriber.create_subscription(self.listener_callback)\n\n        self.pynitros_publisher = PyNitrosPublisher(\n            self, NitrosBridgeImage, 'pynitros_output_msg', 'pynitros_output_msg_ros')\n\n        self.pynitros_image_builder = PyNitrosImageBuilder(\n            num_buffer=40, timeout=5)\n\n    def listener_callback(self, pynitros_image_view):\n        # Build Image\n        header = Header()\n        header.frame_id = pynitros_image_view.get_frame_id()\n        header.stamp.sec = pynitros_image_view.get_timestamp_seconds()\n        header.stamp.nanosec = pynitros_image_view.get_timestamp_nanoseconds()\n        built_image = self.pynitros_image_builder.build(pynitros_image_view.get_buffer(),\n                                                        pynitros_image_view.get_height(),\n                                                        pynitros_image_view.get_width(),\n                                                        pynitros_image_view.get_stride(),\n                                                        pynitros_image_view.get_encoding(),\n                                                        header,\n                                                        0,\n                                                        self.enable_ros_publish)\n\n        # Publish\n        self.pynitros_publisher.publish(built_image)\n\n\ndef main(args=None):\n    try:\n        rclpy.init(args=args)\n        node = PyNITROSImageForwardNode()\n        rclpy.spin(node)\n    except KeyboardInterrupt:\n        pass\n    finally:\n        node.destroy_node()\n        # only shut down if context is active\n        if rclpy.ok():\n            rclpy.shutdown()\n\n\nif __name__ == '__main__':\n    main()\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/examples/pynitros_message_filter_sync_node.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom isaac_ros_nitros_bridge_interfaces.msg import NitrosBridgeImage\nfrom isaac_ros_pynitros.isaac_ros_pynitros_message_filter import PyNitrosMessageFilter\nfrom isaac_ros_pynitros.isaac_ros_pynitros_publisher import PyNitrosPublisher\nfrom isaac_ros_pynitros.isaac_ros_pynitros_subscriber import PyNitrosSubscriber\nfrom isaac_ros_pynitros.pynitros_type_builders.pynitros_image_builder import PyNitrosImageBuilder\nfrom message_filters import Subscriber, TimeSynchronizer\n\nimport rclpy\nfrom rclpy.node import Node\nfrom sensor_msgs.msg import CameraInfo\nfrom std_msgs.msg import Header\n\n\nclass PyNITROSMessageFilterSyncNode(Node):\n    \"\"\"A PyNITROS node listen to synced image and camera info.\"\"\"\n\n    def __init__(self, name='pynitros_message_filter_sync_node'):\n        super().__init__(name)\n        self.declare_parameter('enable_ros_subscribe', False)\n        self.declare_parameter('enable_ros_publish', False)\n\n        self.enable_ros_subscribe = \\\n            self.get_parameter('enable_ros_subscribe').get_parameter_value().bool_value\n        self.enable_ros_publish = \\\n            self.get_parameter('enable_ros_publish').get_parameter_value().bool_value\n\n        self.pynitros_subscriber = PyNitrosSubscriber(\n            self, NitrosBridgeImage, 'image',\n            enable_ros_subscribe=self.enable_ros_subscribe)\n        self.camera_info_sub = Subscriber(self, CameraInfo, 'camera_info')\n\n        self.message_filter = PyNitrosMessageFilter(self,\n                                                    [self.pynitros_subscriber,\n                                                     self.camera_info_sub],\n                                                    TimeSynchronizer,\n                                                    self.listener_callback,\n                                                    queue_size=1)\n\n        self.pynitros_publisher = PyNitrosPublisher(\n            self, NitrosBridgeImage, 'synced_output', 'synced_output_ros')\n\n        self.pynitros_image_builder = PyNitrosImageBuilder(\n            num_buffer=40, timeout=5)\n\n    def listener_callback(self, pynitros_image_view, camera_info):\n        header = Header()\n        header.frame_id = pynitros_image_view.get_frame_id()\n        header.stamp.sec = pynitros_image_view.get_timestamp_seconds()\n        header.stamp.nanosec = pynitros_image_view.get_timestamp_nanoseconds()\n        # Check if image and camera info are in sync\n        if (header.stamp == camera_info.header.stamp):\n            self.get_logger().info('Image and Camera Info are in sync')\n        else:\n            self.get_logger().error('Image and Camera Info are not in sync')\n        # Build Image\n        built_image = self.pynitros_image_builder.build(pynitros_image_view.get_buffer(),\n                                                        pynitros_image_view.get_height(),\n                                                        pynitros_image_view.get_width(),\n                                                        pynitros_image_view.get_stride(),\n                                                        pynitros_image_view.get_encoding(),\n                                                        header,\n                                                        0,\n                                                        self.enable_ros_publish)\n\n        # Publish\n        self.pynitros_publisher.publish(built_image)\n\n\ndef main(args=None):\n    try:\n        rclpy.init(args=args)\n        node = PyNITROSMessageFilterSyncNode()\n        rclpy.spin(node)\n    except KeyboardInterrupt:\n        pass\n    finally:\n        node.destroy_node()\n        # only shut down if context is active\n        if rclpy.ok():\n            rclpy.shutdown()\n\n\nif __name__ == '__main__':\n    main()\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/examples/pynitros_point_cloud_forward_node.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom isaac_ros_nitros_bridge_interfaces.msg import NitrosBridgePointCloud\nfrom isaac_ros_pynitros.isaac_ros_pynitros_publisher import PyNitrosPublisher\nfrom isaac_ros_pynitros.isaac_ros_pynitros_subscriber import PyNitrosSubscriber\nfrom isaac_ros_pynitros.pynitros_type_builders.pynitros_point_cloud_builder import (\n    PyNitrosPointCloudBuilder)\n\nimport rclpy\nfrom rclpy.node import Node\nfrom std_msgs.msg import Header\n\n\nclass PyNITROSPointCloudForwardNode(Node):\n    \"\"\"A PyNITROS node that listens to point cloud messages and forwards them.\"\"\"\n\n    def __init__(self, name='pynitros_point_cloud_forward_node'):\n        super().__init__(name)\n        self.declare_parameter('enable_ros_subscribe', True)\n        self.declare_parameter('enable_ros_publish', True)\n\n        self.enable_ros_subscribe = (\n            self.get_parameter('enable_ros_subscribe').get_parameter_value().bool_value)\n        self.enable_ros_publish = (\n            self.get_parameter('enable_ros_publish').get_parameter_value().bool_value)\n\n        # Choose input topic based on subscribe mode\n        input_topic = (\n            'topic_forward_input_ros'\n            if self.enable_ros_subscribe else\n            'topic_forward_input'\n        )\n        self.pynitros_subscriber = PyNitrosSubscriber(\n            self, NitrosBridgePointCloud, input_topic,\n            enable_ros_subscribe=self.enable_ros_subscribe)\n\n        self.pynitros_subscriber.create_subscription(self.listener_callback)\n\n        self.pynitros_publisher = PyNitrosPublisher(\n            self, NitrosBridgePointCloud, 'topic_forward_output',\n            'topic_forward_output_ros')\n\n        self.pynitros_point_cloud_builder = PyNitrosPointCloudBuilder()\n\n    def listener_callback(self, pynitros_point_cloud_view):\n        \"\"\"Forward the received point cloud message.\"\"\"\n        # Create header inline\n        header = Header()\n        header.frame_id = pynitros_point_cloud_view.get_frame_id()\n        header.stamp.sec = pynitros_point_cloud_view.get_timestamp_seconds()\n        header.stamp.nanosec = pynitros_point_cloud_view.get_timestamp_nanoseconds()\n\n        # Build the point cloud message with all fields read directly from input\n        built_msgs = self.pynitros_point_cloud_builder.build(\n            points_data=pynitros_point_cloud_view.get_points_data(),\n            height=pynitros_point_cloud_view.get_height(),\n            width=pynitros_point_cloud_view.get_width(),\n            fields=pynitros_point_cloud_view.get_fields(),\n            is_bigendian=pynitros_point_cloud_view.is_bigendian(),\n            point_step=pynitros_point_cloud_view.get_point_step(),\n            row_step=pynitros_point_cloud_view.get_row_step(),\n            is_dense=pynitros_point_cloud_view.is_dense(),\n            header=header,\n            device_id=0,\n            enable_ros_publish=self.enable_ros_publish\n        )\n\n        # Publish\n        self.pynitros_publisher.publish(built_msgs)\n\n\ndef main(args=None):\n    node = None\n    try:\n        rclpy.init(args=args)\n        node = PyNITROSPointCloudForwardNode()\n        rclpy.spin(node)\n    except KeyboardInterrupt:\n        pass\n    finally:\n        if node is not None:\n            node.destroy_node()\n        # only shut down if context is active\n        if rclpy.ok():\n            rclpy.shutdown()\n\n\nif __name__ == '__main__':\n    main()\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/examples/pynitros_tensor_list_forward_node.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom isaac_ros_nitros_bridge_interfaces.msg import NitrosBridgeTensorList\nfrom isaac_ros_pynitros.isaac_ros_pynitros_publisher import PyNitrosPublisher\nfrom isaac_ros_pynitros.isaac_ros_pynitros_subscriber import PyNitrosSubscriber\nfrom isaac_ros_pynitros.pynitros_type_builders.pynitros_tensor_list_builder \\\n    import PyNitrosTensorListBuilder\n\nimport rclpy\nfrom rclpy.node import Node\nfrom std_msgs.msg import Header\n\n\nclass PyNITROSTensorListForwardNode(Node):\n    \"\"\"A PyNITROS node listens to tensor list message and publish it again.\"\"\"\n\n    def __init__(self, name='pynitros_tensor_list_forward_node'):\n        super().__init__(name)\n        self.declare_parameter('enable_ros_subscribe', False)\n        self.declare_parameter('enable_ros_publish', False)\n\n        self.enable_ros_subscribe = \\\n            self.get_parameter('enable_ros_subscribe').get_parameter_value().bool_value\n        self.enable_ros_publish = \\\n            self.get_parameter('enable_ros_publish').get_parameter_value().bool_value\n\n        self.pynitros_subscriber = PyNitrosSubscriber(\n            self, NitrosBridgeTensorList, 'pynitros_input_msg',\n            enable_ros_subscribe=self.enable_ros_subscribe)\n        self.pynitros_subscriber.create_subscription(self.listener_callback)\n\n        self.pynitros_publisher = PyNitrosPublisher(\n            self, NitrosBridgeTensorList, 'pynitros_output_msg', 'pynitros_output_msg_ros')\n\n        self.pynitros_tensor_list_builder = PyNitrosTensorListBuilder(num_buffer=20)\n\n    def listener_callback(self, pynitros_tensor_list_view):\n        tensor_list_view = pynitros_tensor_list_view.get_all_tensors()\n        tensor_list = []\n\n        # Build Tensors\n        for tensor_view in tensor_list_view:\n            tensor = self.pynitros_tensor_list_builder.build_tensor(\n                tensor_view.get_buffer(),\n                tensor_view.get_name(),\n                tensor_view.get_shape(),\n                tensor_view.get_element_type())\n            tensor_list.append(tensor)\n\n        # Build TensorList\n        header = Header()\n        header.frame_id = pynitros_tensor_list_view.get_frame_id()\n        header.stamp.sec = pynitros_tensor_list_view.get_timestamp_seconds()\n        header.stamp.nanosec = pynitros_tensor_list_view.get_timestamp_nanoseconds()\n        pynitros_tensors = self.pynitros_tensor_list_builder.build(tensor_list,\n                                                                   header,\n                                                                   0,\n                                                                   self.enable_ros_publish)\n\n        # Publish\n        self.pynitros_publisher.publish(pynitros_tensors)\n\n\ndef main(args=None):\n    try:\n        rclpy.init(args=args)\n        node = PyNITROSTensorListForwardNode()\n        rclpy.spin(node)\n    except KeyboardInterrupt:\n        pass\n    finally:\n        node.destroy_node()\n        # only shut down if context is active\n        if rclpy.ok():\n            rclpy.shutdown()\n\n\nif __name__ == '__main__':\n    main()\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/isaac_ros_pynitros_message_filter.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport cuda.bindings.driver as driver\nimport cuda.bindings.runtime as runtime\nfrom isaac_ros_pynitros.isaac_ros_pynitros_subscriber import PyNitrosSubscriber\nfrom isaac_ros_pynitros.pynitros_type_views.pynitros_type_view_base import PyNitrosTypeViewBase\n\nfrom message_filters import ApproximateTimeSynchronizer, Subscriber, TimeSynchronizer\n\n\nclass PyNitrosMessageFilter():\n    \"\"\"The PyNITROS Message filter synchronize to multiple subscribed topics.\"\"\"\n\n    def __init__(self, node, subscribers, synchronizer, callback, queue_size=10, slop=0.1):\n        \"\"\"\n        Initialize PyNitrosSubscriber.\n\n        Parameters\n        ----------\n        node : rclpy.node.Node\n            ROS2 node creates this message filter.\n        subscribers : list of PyNitrosSubscriber or rclpy.subscription.Subscriber\n            List of subscribers to synchronize.\n        synchronizer : TimeSynchronizer or ApproximateTimeSynchronizer\n            Synchronizer type to use.\n        callback : function\n            User-defined callback function to process synchronized messages.\n        queue_size : int\n            Queue size of the synchronizer.\n        slop : float\n            Slop of the ApproximateTimeSynchronizer.\n\n        \"\"\"\n        rclpy_subs = []\n        self.subscribers = subscribers\n        self.node = node\n        for sub in subscribers:\n            if (isinstance(sub, PyNitrosSubscriber)):\n                if (sub.enable_ros_subscribe):\n                    raw_msg_type = PyNitrosSubscriber.BRIDGE_TYPE_TO_RAW_TYPE[sub.message_type]\n                    rclpy_subs.append(Subscriber(self.node, raw_msg_type, sub.sub_topic_name,\n                                                 qos_profile=sub.qos_profile))\n                else:\n                    rclpy_subs.append(Subscriber(self.node, sub.message_type, sub.sub_topic_name,\n                                                 qos_profile=sub.qos_profile))\n            else:\n                rclpy_subs.append(sub)\n\n        self._cuda_memblock_fd_to_ptr = {}\n        self._event_handle_to_event = {}\n        self._cuda_memblock_handle = []\n        self.input_callback = callback\n\n        if (synchronizer is TimeSynchronizer):\n            rclpy_synchronizer = TimeSynchronizer(rclpy_subs, queue_size)\n        elif (synchronizer is ApproximateTimeSynchronizer):\n            rclpy_synchronizer = ApproximateTimeSynchronizer(rclpy_subs, queue_size, slop)\n        else:\n            raise ValueError('Unsupported synchronizer type')\n        rclpy_synchronizer.registerCallback(self.filter_callback)\n\n    def filter_callback(self, *args):\n        msg_views = []\n        for index in range(len(args)):\n            cur_message = args[index]\n            cur_subscriber = self.subscribers[index]\n\n            if (isinstance(cur_subscriber, PyNitrosSubscriber)):\n                # Create PyNITROS view from bridge msg\n                bridge_message_type = cur_subscriber.message_type\n                if (cur_subscriber.enable_ros_subscribe):\n                    pynitros_view = PyNitrosSubscriber.BRIDGE_TYPE_TO_PYNITROS_VIEW[\n                        bridge_message_type](cur_message)\n                else:\n                    if cur_message.cuda_event_handle:\n                        event_handle_tuple = tuple(cur_message.cuda_event_handle)\n                        if event_handle_tuple not in self._event_handle_to_event:\n                            # Save event handle to local map\n                            event_handle = runtime.cudaIpcEventHandle_t()\n                            event_handle.reserved = cur_message.cuda_event_handle\n\n                            err, cuda_event = runtime.cudaIpcOpenEventHandle(event_handle)\n                            self._event_handle_to_event[event_handle_tuple] = cuda_event\n                            self.ASSERT_CUDA_SUCCESS(err)\n                        else:\n                            cuda_event = self._event_handle_to_event[event_handle_tuple]\n                        # Synchronize to upstream Event\n                        err, = runtime.cudaEventSynchronize(cuda_event)\n                        self.ASSERT_CUDA_SUCCESS(err)\n\n                    # Create PyNITROS view\n                    sender_pid, memblock_fd = \\\n                        PyNitrosSubscriber.BRIDGE_TYPE_TO_PYNITROS_VIEW[\n                            bridge_message_type].get_pid_fd(cur_message)\n                    if (sender_pid, memblock_fd) not in self._cuda_memblock_fd_to_ptr:\n                        try:\n                            pynitros_view = PyNitrosSubscriber.BRIDGE_TYPE_TO_PYNITROS_VIEW[\n                                bridge_message_type](cur_message)\n                        except ValueError:\n                            self.node.get_logger().warn(\n                                'Failed to create PyNITROS view from bridge msg')\n                            return\n                        gpu_ptr = pynitros_view.get_buffer()\n                        gpu_handle = pynitros_view.get_handle()\n                        self._cuda_memblock_fd_to_ptr[(sender_pid, memblock_fd)] = gpu_ptr\n                        self._cuda_memblock_handle.append(gpu_handle)\n                    else:\n                        # Found gpu_ptr from map and create PyNITROS view\n                        gpu_ptr = self._cuda_memblock_fd_to_ptr[(sender_pid, memblock_fd)]\n                        try:\n                            pynitros_view = PyNitrosSubscriber.BRIDGE_TYPE_TO_PYNITROS_VIEW[\n                                bridge_message_type](cur_message, gpu_ptr)\n                        except ValueError:\n                            self.node.get_logger().warn(\n                                'Failed to create PyNITROS view from bridge msg')\n                            return\n                msg_views.append(pynitros_view)\n            else:\n                # Passthrough raw message\n                msg_views.append(cur_message)\n\n        self.input_callback(*msg_views)\n        for pynitros_view in msg_views:\n            if isinstance(pynitros_view, PyNitrosTypeViewBase):\n                pynitros_view.postprocess()\n\n    def ASSERT_CUDA_SUCCESS(self, err):\n        if isinstance(err, driver.CUresult):\n            if err != driver.CUresult.CUDA_SUCCESS:\n                raise RuntimeError(\n                    f'[Cuda Error: {err}], {driver.cuGetErrorString(err)}')\n        elif isinstance(err, runtime.cudaError_t):\n            if (err != 0):\n                raise RuntimeError(f'CudaRT Error: {err}')\n        else:\n            raise RuntimeError('Unknown error type: {}'.format(err))\n\n    def __del__(self):\n        for gpu_handle in self._cuda_memblock_handle:\n            err, = driver.cuMemRelease(gpu_handle)\n            self.ASSERT_CUDA_SUCCESS(err)\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/isaac_ros_pynitros_publisher.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom typing import Union\n\nfrom isaac_ros_nitros_bridge_interfaces.msg import (\n    NitrosBridgeImage, NitrosBridgePointCloud, NitrosBridgeTensorList)\nfrom isaac_ros_tensor_list_interfaces.msg import TensorList\n\nfrom rclpy.qos import QoSProfile\nfrom sensor_msgs.msg import Image\nfrom sensor_msgs.msg import PointCloud2\n\n\nclass PyNitrosPublisher():\n    \"\"\"Publish built messages from PyNITROS Builder.\"\"\"\n\n    def __init__(self, node, message_type, pub_topic, pub_topic_raw,\n                 qos_profile: Union[QoSProfile, int] = 10):\n        \"\"\"\n        Initialize PyNitrosPublisher.\n\n        Parameters\n        ----------\n        node : rclpy.node.Node\n            ROS2 node creates this publisher.\n        message_type : PyNitros message type\n            NITROS Bridge message type to publish.\n        pub_topic : str\n            ROS2 topic name of NITROS bridge message.\n        pub_topic_raw : str\n            ROS2 topic name of raw ROS message.\n        qos_profile : Union[QoSProfile, int]\n            QoS profile to use for the publisher.\n\n        \"\"\"\n        self.node = node\n        self.msg_map = {NitrosBridgeImage: Image,\n                        NitrosBridgeTensorList: TensorList,\n                        NitrosBridgePointCloud: PointCloud2}\n\n        # Regular PyNITROS Pub\n        self.message_type = message_type\n        self.pub_topic = pub_topic\n\n        # If enable_ros_publish == True\n        self.pub_topic_raw = pub_topic_raw\n\n        # Setup both publishers\n        self.publisher_ = self.node.create_publisher(\n            self.message_type, self.pub_topic, qos_profile)\n        self.publisher_raw_ = self.node.create_publisher(\n            self.msg_map[self.message_type], self.pub_topic_raw, qos_profile)\n\n    def publish(self, pynitros_built_msg):\n        \"\"\"Publish PyNITROS built messages.\"\"\"\n        bridge_msg = pynitros_built_msg[0]\n        msg_raw = pynitros_built_msg[1]\n\n        # Publish PyNITROS bridge msg\n        self.publisher_.publish(bridge_msg)\n\n        # Publish raw image if enable_ros_publish is true\n        if msg_raw:\n            self.publisher_raw_.publish(msg_raw)\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/isaac_ros_pynitros_subscriber.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom typing import Union\n\nimport cuda.bindings.driver as driver\nimport cuda.bindings.runtime as runtime\nfrom isaac_ros_nitros_bridge_interfaces.msg import (\n    NitrosBridgeImage, NitrosBridgePointCloud, NitrosBridgeTensorList)\nfrom isaac_ros_pynitros.pynitros_type_views.pynitros_image_view import PyNitrosImageView\nfrom isaac_ros_pynitros.pynitros_type_views.pynitros_point_cloud_view import PyNitrosPointCloudView\nfrom isaac_ros_pynitros.pynitros_type_views.pynitros_tensor_list_view import PyNitrosTensorListView\nfrom isaac_ros_tensor_list_interfaces.msg import TensorList\n\nfrom rclpy.qos import QoSProfile\nfrom sensor_msgs.msg import Image\nfrom sensor_msgs.msg import PointCloud2\n\n\nclass PyNitrosSubscriber():\n    \"\"\"\n    The PyNITROS Subscriber listens to PyNITROS Publisher or the NITROS Bridge node.\n\n    When a message arrives, an internal callback is triggered to convert the GPU data handle\n    back into a valid GPU pointer. The internal callback will wrap the incoming message\n    into a PyNITROS View and invoke the user-defined callback.\n    If enable_ros_subscribe is set to True, the subscriber will exclusively subscribe to\n    ROS messages. In this case, the internal callback will perform CPU->GPU data transfer\n    to wrap the message into a PyNITROS View.\n    \"\"\"\n\n    BRIDGE_TYPE_TO_PYNITROS_VIEW = {\n        NitrosBridgeImage: PyNitrosImageView,\n        NitrosBridgeTensorList: PyNitrosTensorListView,\n        NitrosBridgePointCloud: PyNitrosPointCloudView\n    }\n\n    BRIDGE_TYPE_TO_RAW_TYPE = {\n        NitrosBridgeImage: Image,\n        NitrosBridgeTensorList: TensorList,\n        NitrosBridgePointCloud: PointCloud2\n    }\n\n    def __init__(\n            self, node, message_type, sub_topic_name, enable_ros_subscribe=True,\n            qos_profile: Union[QoSProfile, int] = 10):\n        \"\"\"\n        Initialize PyNitrosSubscriber.\n\n        Parameters\n        ----------\n        node : rclpy.node.Node\n            ROS2 node creates this subscriber.\n        message_type : PyNitros message type\n            NITROS Bridge message type to subscribe.\n        sub_topic_name : str\n            Topic name of the subscribed message.\n        enable_ros_subscribe : bool\n            If True, the subscriber will exclusively subscribe to ROS messages.\n        qos_profile : Union[QoSProfile, int]\n            QoS profile to use for the subscriber.\n\n        \"\"\"\n        self.node = node\n        self.message_type = message_type\n        self.sub_topic_name = sub_topic_name\n        self.enable_ros_subscribe = enable_ros_subscribe\n        self.qos_profile = qos_profile\n\n        self.event = None\n        self._cuda_memblock_fd_to_ptr = {}\n        self._cuda_memblock_handle = []\n        self._cuda_memblock_mapping = []\n\n        # Initialize the CUDA driver API\n        err, = driver.cuInit(0)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n    def create_subscription(self, input_callback):\n        namespaced_sub_topic_name = ''\n        if (self.node.get_namespace() == '/'):\n            namespaced_sub_topic_name = self.sub_topic_name\n        else:\n            namespaced_sub_topic_name = '/'.join(\n                [self.node.get_namespace(), self.sub_topic_name.strip('/')])\n\n        if self.enable_ros_subscribe:\n            self.subscription = self.node.create_subscription(\n                self.BRIDGE_TYPE_TO_RAW_TYPE[self.message_type],\n                namespaced_sub_topic_name,\n                self.listener_callback_ros, qos_profile=self.qos_profile)\n        else:\n            self.subscription = self.node.create_subscription(\n                self.message_type,\n                namespaced_sub_topic_name,\n                self.listener_callback, qos_profile=self.qos_profile)\n\n        self.input_callback = input_callback\n\n    def listener_callback_ros(self, raw_msg):\n        pynitros_view = self.BRIDGE_TYPE_TO_PYNITROS_VIEW[self.message_type](raw_msg)\n        self.input_callback(pynitros_view)\n        pynitros_view.postprocess()\n\n    def listener_callback(self, nitros_bridge_msg):\n        if nitros_bridge_msg.cuda_event_handle and not self.event:\n            self.event_handle = runtime.cudaIpcEventHandle_t()\n            self.event_handle.reserved = nitros_bridge_msg.cuda_event_handle\n\n            err, self.event = runtime.cudaIpcOpenEventHandle(self.event_handle)\n            self.ASSERT_CUDA_SUCCESS(err)\n\n        # Synchronize to upstream Event\n        if self.event:\n            err, = runtime.cudaEventSynchronize(self.event)\n            self.ASSERT_CUDA_SUCCESS(err)\n\n        sender_pid, memblock_fd = \\\n            self.BRIDGE_TYPE_TO_PYNITROS_VIEW[self.message_type].get_pid_fd(nitros_bridge_msg)\n        if (sender_pid, memblock_fd) not in self._cuda_memblock_fd_to_ptr:\n            # Get PyNITROS view from bridge message\n            try:\n                pynitros_view = self.BRIDGE_TYPE_TO_PYNITROS_VIEW[\n                    self.message_type](nitros_bridge_msg)\n            except ValueError:\n                self.node.get_logger().warn('Failed to create PyNITROS view from bridge msg')\n                return\n            gpu_ptr = pynitros_view.get_buffer()\n            gpu_handle = pynitros_view.get_handle()\n            mapped_size = pynitros_view.get_mapped_size()\n            self._cuda_memblock_fd_to_ptr[(sender_pid, memblock_fd)] = gpu_ptr\n            self._cuda_memblock_handle.append(gpu_handle)\n            self._cuda_memblock_mapping.append((gpu_ptr, mapped_size))\n        else:\n            gpu_ptr = self._cuda_memblock_fd_to_ptr[(sender_pid, memblock_fd)]\n            try:\n                pynitros_view = self.BRIDGE_TYPE_TO_PYNITROS_VIEW[\n                    self.message_type](nitros_bridge_msg, gpu_ptr)\n            except ValueError:\n                self.node.get_logger().warn('Failed to create PyNITROS view from bridge msg')\n                return\n\n        # Invoke user-defined callback\n        self.input_callback(pynitros_view)\n\n        # Decrease Refcount\n        pynitros_view.postprocess()\n\n    def ASSERT_CUDA_SUCCESS(self, err):\n        if isinstance(err, driver.CUresult):\n            if err != driver.CUresult.CUDA_SUCCESS:\n                raise RuntimeError(\n                    f'[Cuda Error: {err}], {driver.cuGetErrorString(err)}')\n        elif isinstance(err, runtime.cudaError_t):\n            if (err != 0):\n                raise RuntimeError(f'CudaRT Error: {err}')\n        else:\n            raise RuntimeError('Unknown error type: {}'.format(err))\n\n    def __del__(self):\n        if self.event:\n            err, = runtime.cudaEventDestroy(self.event)\n            self.ASSERT_CUDA_SUCCESS(err)\n\n        for gpu_ptr, mapped_size in self._cuda_memblock_mapping:\n            err, = driver.cuMemUnmap(gpu_ptr, mapped_size)\n            self.ASSERT_CUDA_SUCCESS(err)\n            err, = driver.cuMemAddressFree(gpu_ptr, mapped_size)\n            self.ASSERT_CUDA_SUCCESS(err)\n\n        for gpu_handle in self._cuda_memblock_handle:\n            err, = driver.cuMemRelease(gpu_handle)\n            self.ASSERT_CUDA_SUCCESS(err)\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/pynitros_type_builders/__init__.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/pynitros_type_builders/pynitros_image_builder.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport os\nimport uuid\n\nimport cuda.bindings.driver as driver\nimport cuda.bindings.runtime as runtime\nfrom isaac_ros_nitros_bridge_interfaces.msg import NitrosBridgeImage\nimport numpy as np\nfrom sensor_msgs.msg import Image\n\nfrom .pynitros_type_builder_base import PyNitrosTypeBuilder\n\n\nclass PyNitrosImageBuilder(PyNitrosTypeBuilder):\n    \"\"\"Class to build NitrosBridgeImage messages.\"\"\"\n\n    def __init__(self, num_buffer=10, timeout=1):\n        \"\"\"\n        Initialize the PyNitrosTensorListBuilder.\n\n        Parameters\n        ----------\n        num_buffer: int\n            Number of buffers to create\n        timeout: int\n            Timeout in ms for recycling buffers with refcount 0\n\n        \"\"\"\n        super().__init__(num_buffer, timeout)\n\n    def build(self, image_data, image_height, image_width, image_step,\n              image_encoding, image_header,\n              device_id, enable_ros_publish, event=None):\n        \"\"\"\n        Build NITROS Image message.\n\n        Parameters\n        ----------\n        image_data: cuda.CUdeviceptr\n            Device pointer to image data\n        image_height: int\n            Height of the image\n        image_width: int\n            Width of the image\n        image_step: int\n            Step of the image in bytes\n        image_encoding: str\n            Encoding of the image\n        image_header: Header\n            Header for the image\n        device_id: int\n            Device id the tensors are residing on\n        enable_ros_publish: bool\n            Enable ROS publishing\n        event: cudaEvent\n            Event with unfinished CUDA operations to synchronize on\n\n        Returns\n        -------\n        tuple: (NitrosBridgeImage, Image)\n            NitrosBridgeImage: NitrosBridgeImage message\n            Image: Image message, None if enable_ros_publish is False\n\n        \"\"\"\n        if self._first_frame:\n            buffer_size = image_step * image_height\n            super().create_memblock_pool(buffer_size)\n            self._first_frame = False\n\n        # Synchronize to user Event\n        if event:\n            err, = runtime.cudaEventSynchronize(self.event)\n            super().ASSERT_CUDA_SUCCESS(err)\n\n        # Create Nitros Bridge Image\n        nitros_bridge_image_msg = NitrosBridgeImage()\n        nitros_bridge_image_msg.header = image_header\n        nitros_bridge_image_msg.height = image_height\n        nitros_bridge_image_msg.width = image_width\n        nitros_bridge_image_msg.encoding = image_encoding\n        nitros_bridge_image_msg.is_bigendian = 0\n        nitros_bridge_image_msg.step = image_step\n\n        memblock_fd, new_memblock_uuid, image_event_handle = self._setup_image_memblock(\n            image_data)\n        nitros_bridge_image_msg.data = [os.getpid(), memblock_fd]\n        nitros_bridge_image_msg.cuda_event_handle = image_event_handle.reserved\n        nitros_bridge_image_msg.device_id = device_id\n        nitros_bridge_image_msg.uid = str(new_memblock_uuid)\n\n        # Setup Shareable Memory To Transport Image\n        image_msg_raw = None\n\n        if enable_ros_publish:\n            image_msg_raw = Image()\n            image_size = image_step * image_height\n            # Set image properties\n            image_msg_raw.header = image_header\n            image_msg_raw.height = image_height\n            image_msg_raw.width = image_width\n            image_msg_raw.encoding = image_encoding\n            image_msg_raw.is_bigendian = 0\n            image_msg_raw.step = image_step\n            # Assign to the private data field to reduce overhead of msg data assignment\n            image_msg_raw._data = np.empty(image_size, dtype=np.uint8)\n            err, = runtime.cudaMemcpy(image_msg_raw.data, image_data, image_size,\n                                      runtime.cudaMemcpyKind.cudaMemcpyDeviceToHost)\n            super().ASSERT_CUDA_SUCCESS(err)\n\n        # Return Nitros Bridge Image\n        return (nitros_bridge_image_msg, image_msg_raw)\n\n    def _setup_image_memblock(self, image_tensor_ptr):\n        \"\"\"Set up shareable CUDA memblocks for images.\"\"\"\n        cuda_memblock_fd, cuda_memblock_uuid, cuda_memblock_refcount = super().get_free_memblock()\n        cuda_memblock_virtual_ptr = self._cuda_memblock_fd_to_ptr[cuda_memblock_fd]\n        cuda_event, cuda_event_handle = self._cuda_memblock_event[cuda_memblock_fd]\n\n        # Update memblock uuid header\n        new_cuda_memblock_uuid = uuid.uuid4()\n        cuda_memblock_cpu_mem = self._cuda_memblock_to_cpu_mem[cuda_memblock_fd]\n\n        cuda_memblock_cpu_mem.lock.acquire()\n        cuda_memblock_cpu_mem.put_value(\n            (cuda_memblock_refcount, new_cuda_memblock_uuid))\n        cuda_memblock_cpu_mem.lock.release()\n\n        err, = driver.cuMemcpyDtoDAsync(driver.CUdeviceptr(\n            cuda_memblock_virtual_ptr), image_tensor_ptr, self.buffer_size, self._stream)\n        super().ASSERT_CUDA_SUCCESS(err)\n\n        err, = runtime.cudaEventRecord(cuda_event, self._stream)\n        super().ASSERT_CUDA_SUCCESS(err)\n\n        return (cuda_memblock_fd, new_cuda_memblock_uuid, cuda_event_handle)\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/pynitros_type_builders/pynitros_point_cloud_builder.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport os\nimport uuid\n\nimport cuda.bindings.driver as cuda\nimport cuda.bindings.runtime as cudart\nfrom isaac_ros_nitros_bridge_interfaces.msg import NitrosBridgePointCloud\nimport numpy as np\nfrom sensor_msgs.msg import PointCloud2\n\nfrom .pynitros_type_builder_base import PyNitrosTypeBuilder\n\n\nclass PyNitrosPointCloudBuilder(PyNitrosTypeBuilder):\n    \"\"\"Class to build NitrosBridgePointCloud messages.\"\"\"\n\n    def __init__(self, num_buffer=10, timeout=1):\n        \"\"\"\n        Initialize the PyNitrosPointCloudBuilder.\n\n        Parameters\n        ----------\n        num_buffer: int\n            Number of buffers to create\n        timeout: int\n            Timeout in ms for recycling buffers with refcount 0\n\n        \"\"\"\n        super().__init__(num_buffer, timeout)\n\n    def build(self, points_data, height, width, fields, is_bigendian, point_step,\n              row_step, is_dense, header, device_id, enable_ros_publish, event=None):\n        \"\"\"\n        Build NitrosBridgePointCloud message.\n\n        Parameters\n        ----------\n        points_data: cuda.CUdeviceptr\n            Device pointer to point cloud data\n        height: int\n            Height of the point cloud\n        width: int\n            Width of the point cloud\n        fields: list\n            List of PointField objects\n        is_bigendian: bool\n            Whether the point cloud is big endian\n        point_step: int\n            Step size in bytes for each point\n        row_step: int\n            Step size in bytes for each row\n        is_dense: bool\n            Whether the point cloud is dense\n        header: Header\n            Header for the point cloud\n        device_id: int\n            device id the point cloud is residing on\n        enable_ros_publish: bool\n            enable ROS publishing\n        event: cudaEvent\n            event with unfinished CUDA operations to synchronize on\n\n        Returns\n        -------\n        tuple: (NitrosBridgePointCloud, PointCloud2)\n            NitrosBridgePointCloud: NitrosBridgePointCloud message\n            PointCloud2: PointCloud2 message, None if enable_ros_publish is False\n\n        \"\"\"\n        # Synchronize to user Event\n        if event:\n            err, = cudart.cudaEventSynchronize(event)\n            super().ASSERT_CUDA_SUCCESS(err)\n\n        nitros_bridge_point_cloud_msg = NitrosBridgePointCloud()\n        nitros_bridge_point_cloud_msg.header = header\n        nitros_bridge_point_cloud_msg.height = height\n        nitros_bridge_point_cloud_msg.width = width\n        nitros_bridge_point_cloud_msg.fields = fields\n        nitros_bridge_point_cloud_msg.is_bigendian = is_bigendian\n        nitros_bridge_point_cloud_msg.point_step = point_step\n        nitros_bridge_point_cloud_msg.row_step = row_step\n        nitros_bridge_point_cloud_msg.is_dense = is_dense\n\n        point_cloud_msg_raw = None\n        if enable_ros_publish:\n\n            point_cloud_msg_raw = PointCloud2()\n            point_cloud_msg_raw.header = header\n            point_cloud_msg_raw.height = height\n            point_cloud_msg_raw.width = width\n            point_cloud_msg_raw.is_bigendian = is_bigendian\n            point_cloud_msg_raw.is_dense = is_dense\n            point_cloud_msg_raw.fields = fields\n            point_cloud_msg_raw.point_step = point_step\n            point_cloud_msg_raw.row_step = row_step\n\n        # Calculate required buffer size for this frame using ROS2 standard\n        required_buffer_size = row_step * height  # Use ROS2 standard calculation\n\n        if self._first_frame:\n            super().create_memblock_pool(required_buffer_size)\n            self._first_frame = False\n\n        # Set the Nitros-specific fields\n        # Note: points_data, num_points, use_color are not part of the message definition\n        # These are handled internally by the builder for GPU memory management\n\n        if enable_ros_publish and point_cloud_msg_raw is not None:\n            # Copy data from device to host for ROS message\n            data_size = row_step * height  # Use ROS2 standard calculation\n            host_buf = np.empty(data_size, dtype=np.uint8)\n            err, = cudart.cudaMemcpy(\n                host_buf.ctypes.data,\n                points_data,\n                data_size,\n                cudart.cudaMemcpyKind.cudaMemcpyDeviceToHost)\n            super().ASSERT_CUDA_SUCCESS(err)\n            point_cloud_msg_raw.data = host_buf.tobytes()\n\n        memblock_fd, new_memblock_uuid, point_cloud_event_handle = (\n            self._setup_point_cloud_memblock(points_data, row_step * height))\n\n        # Set Nitros-specific fields for GPU memory management\n        nitros_bridge_point_cloud_msg.data = [os.getpid(), memblock_fd]  # [PID, FD]\n        nitros_bridge_point_cloud_msg.cuda_event_handle = point_cloud_event_handle.reserved\n        nitros_bridge_point_cloud_msg.device_id = device_id\n        nitros_bridge_point_cloud_msg.uid = str(new_memblock_uuid)\n\n        return (nitros_bridge_point_cloud_msg, point_cloud_msg_raw)\n\n    def _setup_point_cloud_memblock(self, gpu_ptr, data_size):\n        \"\"\"Set up sharable device memory blocks.\"\"\"\n        cuda_memblock_fd, cuda_memblock_uuid, cuda_memblock_refcount = super().get_free_memblock()\n        cuda_memblock_virtual_ptr = self._cuda_memblock_fd_to_ptr[cuda_memblock_fd]\n        cuda_event, cuda_event_handle = self._cuda_memblock_event[cuda_memblock_fd]\n\n        # Update memblock uuid header\n        new_cuda_memblock_uuid = uuid.uuid4()\n        cuda_memblock_cpu_mem = self._cuda_memblock_to_cpu_mem[cuda_memblock_fd]\n\n        cuda_memblock_cpu_mem.lock.acquire()\n        cuda_memblock_cpu_mem.put_value(\n            (cuda_memblock_refcount, new_cuda_memblock_uuid))\n        cuda_memblock_cpu_mem.lock.release()\n\n        # Copy point cloud data into cuda mem space\n        # data_size is now passed as a parameter (calculated as row_step * height)\n\n        # Validate that data size doesn't exceed buffer capacity\n        if data_size > self.buffer_size:\n            raise ValueError(\n                f'Point cloud data size ({data_size} bytes) exceeds buffer capacity '\n                f'({self.buffer_size} bytes). This should not happen as the pool should '\n                'have been resized.')\n\n        # Copy data to the memblock\n        err, = cuda.cuMemcpyDtoDAsync(\n            cuda_memblock_virtual_ptr, gpu_ptr, data_size, self._stream)\n        super().ASSERT_CUDA_SUCCESS(err)\n\n        # Record event after the copy\n        err, = cudart.cudaEventRecord(cuda_event, self._stream)\n        super().ASSERT_CUDA_SUCCESS(err)\n\n        # Synchronize stream to ensure the event is recorded\n        err, = cudart.cudaStreamSynchronize(self._stream)\n        super().ASSERT_CUDA_SUCCESS(err)\n\n        return (cuda_memblock_fd, new_cuda_memblock_uuid, cuda_event_handle)\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/pynitros_type_builders/pynitros_tensor_list_builder.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport math\nimport os\nimport uuid\n\nimport cuda.bindings.driver as driver\nimport cuda.bindings.runtime as runtime\n\nfrom isaac_ros_nitros_bridge_interfaces.msg import NitrosBridgeTensorList\nfrom isaac_ros_pynitros.utils.tensor_data_type import TensorDataTypeUtils\nfrom isaac_ros_tensor_list_interfaces.msg import Tensor, TensorList, TensorShape\nimport numpy as np\n\nfrom .pynitros_type_builder_base import PyNitrosTypeBuilder\n\n\nclass PyNitrosTensorListBuilder(PyNitrosTypeBuilder):\n    \"\"\"Class to build NitrosBridgeTensorList messages.\"\"\"\n\n    def __init__(self, num_buffer=10, timeout=1):\n        \"\"\"\n        Initialize the PyNitrosTensorListBuilder.\n\n        Parameters\n        ----------\n        buffer_size: int\n            Size of each buffer in bytes\n        num_buffer: int\n            Number of buffers to create\n        timeout: int\n            Timeout in ms for recycling buffers with refcount 0\n\n        \"\"\"\n        super().__init__(num_buffer, timeout)\n\n    def build(self, built_tensors, tensors_header,\n              device_id, enable_ros_publish, event=None):\n        \"\"\"\n        Build NitrosBridgeTensorList message.\n\n        Parameters\n        ----------\n        built_tensors: list\n            List of built tensors\n        tensors_header: Header\n            Header for the tensor list\n        device_id: int\n            device id the tensors are residing on\n        enable_ros_publish: bool\n            enable ROS publishing\n        event: cudaEvent\n            event with unfinished CUDA operations to synchronize on\n\n        Returns\n        -------\n        tuple: (NitrosBridgeTensorList, TensorList)\n            NitrosBridgeTensorList: NitrosBridgeTensorList message\n            TensorList: TensorList message, None if enable_ros_publish is False\n\n        \"\"\"\n        nitros_bridge_tensor_list_msg = NitrosBridgeTensorList()\n        nitros_bridge_tensor_list_msg.header = tensors_header\n        nitros_bridge_tensor_list_msg.tensors = []\n\n        tensor_ptrs_sizes = []\n        tensor_list_msg_raw = None\n        if enable_ros_publish:\n            tensor_list_msg_raw = TensorList()\n            tensor_list_msg_raw.header = tensors_header\n            tensor_list_msg_raw.tensors = []\n\n        if self._first_frame:\n            total_tensor_size = 0\n            for tensor in built_tensors:\n                total_tensor_size += tensor.strides[0] * tensor.shape.dims[0]\n            super().create_memblock_pool(total_tensor_size)\n            self._first_frame = False\n\n        # Synchronize to upstream Event\n        if event:\n            err, = runtime.cudaEventSynchronize(self.event)\n            super().ASSERT_CUDA_SUCCESS(err)\n\n        # Set the data field\n        index = 0\n        for tensor in built_tensors:\n            nitros_bridge_tensor_list_msg.tensors.append(tensor)\n            tensor_size = tensor.strides[0] * tensor.shape.dims[0]\n\n            tensor_gpu_ptr = int.from_bytes(tensor.data, byteorder='big')\n            tensor_ptrs_sizes.append((tensor_gpu_ptr, tensor_size))\n            if enable_ros_publish:\n                raw_tensor = Tensor()\n                raw_tensor.name = tensor.name\n                raw_tensor.shape = tensor.shape\n                raw_tensor.data_type = tensor.data_type\n                raw_tensor.strides = tensor.strides\n                # Assign to the private data field to reduce overhead of msg data assignment\n                raw_tensor._data = np.empty(tensor_size, dtype=np.uint8)\n                err, = runtime.cudaMemcpy(raw_tensor.data, tensor_gpu_ptr, tensor_size,\n                                          runtime.cudaMemcpyKind.cudaMemcpyDeviceToHost)\n                super().ASSERT_CUDA_SUCCESS(err)\n                tensor_list_msg_raw.tensors.append(raw_tensor)\n            index += 1\n\n        memblock_fd, new_memblock_uuid, tensor_event_handle = self._setup_tensors_memblock(\n            tensor_ptrs_sizes)\n        nitros_bridge_tensor_list_msg.pid = os.getpid()\n        nitros_bridge_tensor_list_msg.fd = memblock_fd\n        nitros_bridge_tensor_list_msg.cuda_event_handle = tensor_event_handle.reserved\n        nitros_bridge_tensor_list_msg.device_id = device_id\n        nitros_bridge_tensor_list_msg.uid = str(new_memblock_uuid)\n\n        return (nitros_bridge_tensor_list_msg, tensor_list_msg_raw)\n\n    def build_tensor(self, tensor_data, tensor_name, tensor_shape, tensor_data_type):\n        \"\"\"\n        Build a NitrosBridgeTensor message.\n\n        Parameters\n        ----------\n        tensor_data: device ptr\n            Device pointer to the tensor data\n        tensor_name: str\n            Name of the tensor\n        tensor_shape: list\n            Shape of the tensor\n        tensor_data_type: int\n            Data type of the tensor\n\n        Returns\n        -------\n        tensor: NitrosBridgeTensor message\n\n        \"\"\"\n        # Build Tensor Messages\n        tensor_shape_msg = TensorShape()\n        tensor_shape_msg.rank = len(tensor_shape)\n        tensor_shape_msg.dims = tensor_shape\n\n        nitros_bridge_tensor_msg = Tensor()\n        nitros_bridge_tensor_msg.name = tensor_name\n        nitros_bridge_tensor_msg.shape = tensor_shape_msg\n        nitros_bridge_tensor_msg.data_type = tensor_data_type\n\n        strides = []\n        element_size = TensorDataTypeUtils.get_size_in_bytes(tensor_data_type)\n        for i in range(len(tensor_shape)):\n            if i == (len(tensor_shape)-1):\n                strides.append(element_size)\n            else:\n                strides.append(math.prod(tensor_shape[i+1:])*element_size)\n\n        nitros_bridge_tensor_msg.strides = strides\n        nitros_bridge_tensor_msg.data = tensor_data.to_bytes(8, byteorder='big')\n\n        return nitros_bridge_tensor_msg\n\n    def _setup_tensors_memblock(self, tensor_ptrs_sizes):\n        \"\"\"Set up sharable device memory blocks.\"\"\"\n        cuda_memblock_fd, cuda_memblock_uuid, cuda_memblock_refcount = super().get_free_memblock()\n        cuda_memblock_virtual_ptr = self._cuda_memblock_fd_to_ptr[cuda_memblock_fd]\n        cuda_event, cuda_event_handle = self._cuda_memblock_event[cuda_memblock_fd]\n\n        # Update memblock uuid header\n        new_cuda_memblock_uuid = uuid.uuid4()\n        cuda_memblock_cpu_mem = self._cuda_memblock_to_cpu_mem[cuda_memblock_fd]\n\n        cuda_memblock_cpu_mem.lock.acquire()\n        cuda_memblock_cpu_mem.put_value(\n            (cuda_memblock_refcount, new_cuda_memblock_uuid))\n        cuda_memblock_cpu_mem.lock.release()\n\n        # Copy tensors back-to-back into cuda mem space\n        offset = 0\n        for cur_tensor_ptr, cur_tensor_size in tensor_ptrs_sizes:\n            cur_gpu_ptr = driver.CUdeviceptr(cuda_memblock_virtual_ptr + offset)\n            err, = driver.cuMemcpyDtoDAsync(\n                cur_gpu_ptr, cur_tensor_ptr, cur_tensor_size, self._stream)\n            super().ASSERT_CUDA_SUCCESS(err)\n\n            offset += cur_tensor_size\n\n        err, = runtime.cudaEventRecord(cuda_event, self._stream)\n        super().ASSERT_CUDA_SUCCESS(err)\n\n        return (cuda_memblock_fd, new_cuda_memblock_uuid, cuda_event_handle)\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/pynitros_type_builders/pynitros_type_builder_base.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\nimport math\nimport os\nimport time\nimport uuid\n\nimport cuda.bindings.driver as driver\nimport cuda.bindings.runtime as runtime\nfrom isaac_ros_pynitros.utils.cpu_shared_mem import CPUSharedMem\nimport rclpy\n\n\nclass PyNitrosTypeBuilder():\n    \"\"\"\n    Base class of PyNITROS message builders.\n\n    The builder creates a pool of sharable GPU memory buffers during initialization.\n    to hold the GPU data of the message. The inherited class should implement\n    the build method to build different type of NITROS Bridge messages.\n    \"\"\"\n\n    def __init__(self, num_buffer=10, timeout=1):\n        \"\"\"\n        Initialize the PyNitrosTypeBuilder.\n\n        Parameters\n        ----------\n        num_buffer: int\n            Number of buffers to create\n        timeout: int\n            Timeout in ms for recycling buffers with refcount 0\n\n        \"\"\"\n        self.num_buffer = num_buffer\n        self.timeout = timeout\n\n        err, = driver.cuInit(0)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        # Create CUDA Stream\n        err, self._stream = runtime.cudaStreamCreate()\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        # Maps CUDA Memblocks to their respective shared CPU memory\n        self._cuda_memblock_to_cpu_mem = {}\n        self._cuda_memblock_event = {}\n\n        # Cuda MemBlock Housekeeping\n        self._cuda_memblock_fd_to_ptr = {}\n        self._cuda_memblock_info = []\n        self._cuda_memblock_start_time = {}\n        self._mem_block_index = 0\n\n        self._first_frame = True\n\n    def create_memblock_pool(self, buffer_size):\n        \"\"\"\n        Create a pool of CUDA IPC memory blocks.\n\n        This pool is created the first time message is built.\n        \"\"\"\n        self.buffer_size = buffer_size\n        for _ in range(self.num_buffer):\n            # Create memory block\n            cuda_memblock_fd, cuda_memblock_virtual_ptr = self._create_cuda_memblock()\n\n            # Add to mem pool mapping\n            cpu_memblock_info = CPUSharedMem()\n            cpu_memblock_info.create_shm(str(os.getpid()) + str(cuda_memblock_fd))\n\n            cpu_memblock_info.lock.acquire()\n            cpu_memblock_info.put_value((0, uuid.uuid4()))\n            cpu_memblock_info.lock.release()\n\n            self._cuda_memblock_to_cpu_mem[cuda_memblock_fd] = cpu_memblock_info\n\n            # Add fd, ptr info\n            self._cuda_memblock_fd_to_ptr[cuda_memblock_fd] = int(\n                cuda_memblock_virtual_ptr)\n\n            # Set timeout value\n            self._cuda_memblock_start_time[cuda_memblock_fd] = 0\n\n            # Setup event, record stream, and perform copy\n            err, event = runtime.cudaEventCreateWithFlags(\n                runtime.cudaEventInterprocess | runtime.cudaEventDisableTiming)\n            self.ASSERT_CUDA_SUCCESS(err)\n\n            err, event_handle = runtime.cudaIpcGetEventHandle(event)\n            self.ASSERT_CUDA_SUCCESS(err)\n            self._cuda_memblock_event[cuda_memblock_fd] = (event, event_handle)\n\n    def get_free_memblock(self):\n        \"\"\"\n        Get a free CUDA memblock from the pool.\n\n        If no free memblock is available, it will keep looping until a\n        free memblock is available.\n\n        Returns\n        -------\n        Tuple of (memblock_fd, memblock_uuid, memblock_refcount)\n            memblock_fd: int\n                File descriptor of the memblock\n            memblock_uuid: uuid\n                UID of the memblock, updated everytime the memblock is reused\n            memblock_refcount: int\n                Reference count of the memblock\n\n        \"\"\"\n        cuda_memblock_list = list(self._cuda_memblock_to_cpu_mem.items())\n        last_used_index = self._mem_block_index\n        while True:\n            cuda_memblock_fd, cuda_memblock = cuda_memblock_list[self._mem_block_index]\n            start_time = self._cuda_memblock_start_time[cuda_memblock_fd]\n            current_time = time.time() * 1000\n\n            # TODO(yuankunz): Reset timeout everytime refcount is decreased to 0\n            if current_time - start_time > self.timeout:\n                cuda_memblock.lock.acquire()\n                cuda_memblock_refcount, cuda_memblock_uuid = cuda_memblock.get_value()\n                if cuda_memblock_refcount == 0:\n                    self._cuda_memblock_start_time[cuda_memblock_fd] = time.time() * 1000\n                    cuda_memblock.lock.release()\n                    self._mem_block_index = (self._mem_block_index + 1) % len(cuda_memblock_list)\n                    return cuda_memblock_fd, cuda_memblock_uuid, cuda_memblock_refcount\n                else:\n                    cuda_memblock.lock.release()\n            self._mem_block_index = (self._mem_block_index + 1) % len(cuda_memblock_list)\n            if self._mem_block_index == last_used_index:\n                rclpy.logging.get_logger('PyNITROSBuilder').info(\n                    'No free memblock available, retry after 1ms...')\n                time.sleep(0.001)\n\n    def _create_cuda_memblock(self):\n        \"\"\"Create an IPC CUDA memory pool with the given buffer size and number of buffers.\"\"\"\n        # Allocate GPU Space -> CUmemGenericAllocationHandle\n        allocProp = driver.CUmemAllocationProp()\n        allocProp.type = driver.CUmemAllocationType.CU_MEM_ALLOCATION_TYPE_PINNED\n        allocProp.location.type = driver.CUmemLocationType.CU_MEM_LOCATION_TYPE_DEVICE\n        allocProp.requestedHandleTypes = \\\n            driver.CUmemAllocationHandleType.CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR\n        allocProp.location.id = 0\n\n        # Get granularity\n        err, granularity = driver.cuMemGetAllocationGranularity(\n            allocProp, driver.CUmemAllocationGranularity_flags.CU_MEM_ALLOC_GRANULARITY_MINIMUM)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        # Call cuMemCreate with the address of allocProp\n        rounded_data_size = self._round_up(self.buffer_size, granularity)\n\n        err, handle = driver.cuMemCreate(rounded_data_size, allocProp, 0)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        # Reserve Virtual Address Range\n        err, memblock_virtual_ptr = driver.cuMemAddressReserve(\n            rounded_data_size, 0, 0, 0)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        # Map Handle to Virtual Address Range, Set RW Access\n        err, = driver.cuMemMap(memblock_virtual_ptr,\n                               rounded_data_size, 0, handle, 0)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        self._cuda_memblock_info.append(\n            (handle, memblock_virtual_ptr, rounded_data_size))\n\n        accessDesc = driver.CUmemAccessDesc()\n        accessDesc.location = allocProp.location\n        accessDesc.flags = driver.CUmemAccess_flags.CU_MEM_ACCESS_FLAGS_PROT_READWRITE\n\n        err, = driver.cuMemSetAccess(\n            memblock_virtual_ptr, rounded_data_size, [accessDesc], 1)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        # Export Shareable Handle\n        err, memblock_fd = driver.cuMemExportToShareableHandle(\n            handle, driver.CUmemAllocationHandleType.CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR, 0)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        return (memblock_fd, memblock_virtual_ptr)\n\n    def _round_up(self, size, granularity):\n        \"\"\"Round up the size to the nearest multiple of granularity.\"\"\"\n        mult = math.ceil(size / float(granularity))\n        return mult * granularity\n\n    def ASSERT_CUDA_SUCCESS(self, err):\n        \"\"\"Assert if the CUDA operation is successful.\"\"\"\n        if isinstance(err, driver.CUresult):\n            if err != driver.CUresult.CUDA_SUCCESS:\n                raise RuntimeError(\n                    f'[Cuda Error: {err}], {driver.cuGetErrorString(err)}')\n        elif isinstance(err, runtime.cudaError_t):\n            if (err != 0):\n                raise RuntimeError(f'CudaRT Error: {err}')\n        else:\n            raise RuntimeError('Unknown error type: {}'.format(err))\n\n    def __del__(self):\n        \"\"\"Destructor to free all the allocated resources.\"\"\"\n        for cpu_memblock in self._cuda_memblock_to_cpu_mem.values():\n            cpu_memblock.close()\n\n        for cuda_event, _ in self._cuda_memblock_event.values():\n            err, = runtime.cudaEventDestroy(cuda_event)\n            self.ASSERT_CUDA_SUCCESS(err)\n\n        err, = driver.cuStreamDestroy(self._stream)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        for memblock_fd in self._cuda_memblock_fd_to_ptr.keys():\n            os.close(memblock_fd)\n\n        for cuda_memblock_handle, cuda_memblock_virtual_ptr, data_size in \\\n                self._cuda_memblock_info:\n            err, = driver.cuMemUnmap(cuda_memblock_virtual_ptr, data_size)\n            self.ASSERT_CUDA_SUCCESS(err)\n            err, = driver.cuMemAddressFree(cuda_memblock_virtual_ptr, data_size)\n            self.ASSERT_CUDA_SUCCESS(err)\n            err, = driver.cuMemRelease(cuda_memblock_handle)\n            self.ASSERT_CUDA_SUCCESS(err)\n\n        runtime.cudaDeviceReset()\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/pynitros_type_views/__init__.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/pynitros_type_views/pynitros_image_view.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom cuda.bindings import runtime as runtime\n\nimport torch\n\nfrom .pynitros_type_view_base import PyNitrosTypeViewBase\n\n\nclass PyNitrosImageView(PyNitrosTypeViewBase):\n    \"\"\"PyNITROS View for NITROSBridgeImage.\"\"\"\n\n    def __init__(self, raw_msg, gpu_ptr=None):\n        super().__init__(raw_msg, gpu_ptr)\n\n        if (isinstance(self.raw_msg, self.nitros_bridge_msg_types)):\n            sender_pid, memblock_fd = self.get_pid_fd(self.raw_msg)\n            if (self._open_shm_and_check_uid(sender_pid, memblock_fd) is not True):\n                raise ValueError('UID match failed')\n\n        if (self.gpu_ptr is None):\n            if (isinstance(self.raw_msg, self.nitros_bridge_msg_types)):\n                self.gpu_ptr = self._from_bridge_msg()\n            elif (isinstance(self.raw_msg, self.raw_msg_types)):\n                self.gpu_ptr = self._from_raw_msg()\n            else:\n                raise RuntimeError('Invalid message type')\n\n        self.gpu_ptr = int(self.gpu_ptr)\n        self.shape = torch.Size(\n            [raw_msg.height * raw_msg.step])\n        self.__cuda_array_interface__ = {\n            'shape': self.shape,\n            'typestr': '|u1',\n            'data': (self.gpu_ptr, False),\n            'version': 0\n        }\n\n    @staticmethod\n    def get_pid_fd(bridge_msg):\n        return bridge_msg.data[0], bridge_msg.data[1]\n\n    def _from_bridge_msg(self):\n        sender_pid, memblock_fd = self.get_pid_fd(self.raw_msg)\n        data_size = self.raw_msg.height * self.raw_msg.step\n        return self._import_gpu_ptr_from_fd(sender_pid, memblock_fd, data_size)\n\n    def _from_raw_msg(self):\n        image_size = len(self.raw_msg.data)\n        err, device_ptr = runtime.cudaMalloc(image_size)\n        self.ASSERT_CUDA_SUCCESS(err)\n        err, = runtime.cudaMemcpy(device_ptr, self.raw_msg.data, image_size,\n                                  runtime.cudaMemcpyKind.cudaMemcpyHostToDevice)\n        self.ASSERT_CUDA_SUCCESS(err)\n        return device_ptr\n\n    def get_size_in_bytes(self):\n        return self.raw_msg.step * self.raw_msg.height\n\n    def get_width(self):\n        return self.raw_msg.width\n\n    def get_height(self):\n        return self.raw_msg.height\n\n    def get_stride(self):\n        return self.raw_msg.step\n\n    def get_buffer(self):\n        return self.gpu_ptr\n\n    def get_encoding(self):\n        return self.raw_msg.encoding\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/pynitros_type_views/pynitros_point_cloud_view.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport cuda.bindings.runtime as cudart\n\nfrom .pynitros_type_view_base import PyNitrosTypeViewBase\n\n\nclass PyNitrosPointCloudView(PyNitrosTypeViewBase):\n    \"\"\"PyNITROS view for NitrosBridgePointCloud.\"\"\"\n\n    def __init__(self, raw_msg, gpu_ptr=None):\n        super().__init__(raw_msg, gpu_ptr)\n\n        if isinstance(self.raw_msg, self.nitros_bridge_msg_types):\n            sender_pid, memblock_fd = self.get_pid_fd(self.raw_msg)\n            if (self._open_shm_and_check_uid(sender_pid, memblock_fd)\n                    is not True):\n                raise ValueError('UID match failed')\n\n        # Initialize GPU pointer first\n        if self.gpu_ptr is None:\n            if isinstance(self.raw_msg, self.nitros_bridge_msg_types):\n                self.gpu_ptr = self._from_bridge_msg()\n            elif isinstance(self.raw_msg, self.raw_msg_types):\n                self.gpu_ptr = self._from_raw_msg()\n            else:\n                raise RuntimeError('Invalid message type')\n        self.gpu_ptr = int(self.gpu_ptr)\n\n    @staticmethod\n    def get_pid_fd(bridge_msg):\n        return bridge_msg.data[0], bridge_msg.data[1]\n\n    def _from_bridge_msg(self):\n        sender_pid, memblock_fd = self.get_pid_fd(self.raw_msg)\n        data_size = self.raw_msg.height * self.raw_msg.row_step\n        return self._import_gpu_ptr_from_fd(sender_pid, memblock_fd, data_size)\n\n    def _from_raw_msg(self):\n        data_size = len(self.raw_msg.data)\n        err, device_ptr = cudart.cudaMalloc(data_size)\n        self.ASSERT_CUDA_SUCCESS(err)\n        err, = cudart.cudaMemcpy(device_ptr, self.raw_msg.data, data_size,\n                                 cudart.cudaMemcpyKind.cudaMemcpyHostToDevice)\n        self.ASSERT_CUDA_SUCCESS(err)\n        return device_ptr\n\n    def get_height(self):\n        return self.raw_msg.height\n\n    def get_width(self):\n        return self.raw_msg.width\n\n    def get_fields(self):\n        return self.raw_msg.fields\n\n    def is_bigendian(self):\n        return self.raw_msg.is_bigendian\n\n    def get_point_step(self):\n        return self.raw_msg.point_step\n\n    def get_row_step(self):\n        return self.raw_msg.row_step\n\n    def is_dense(self):\n        return self.raw_msg.is_dense\n\n    def get_point_count(self):\n        \"\"\"Get the number of points in the point cloud.\"\"\"\n        return self.raw_msg.height * self.raw_msg.width\n\n    def has_color(self):\n        \"\"\"Check if the point cloud has color information.\"\"\"\n        # Check if there are color-related fields in the point cloud\n        if hasattr(self.raw_msg, 'fields') and self.raw_msg.fields:\n            for field in self.raw_msg.fields:\n                if field.name.lower() in ['rgb', 'r', 'g', 'b', 'rgba']:\n                    return True\n\n        # Check if point_step indicates color data (16 bytes for XYZ + RGB vs 12 bytes\n        # for XYZ only)\n        if hasattr(self.raw_msg, 'point_step'):\n            return self.raw_msg.point_step > 12\n\n        return False\n\n    def get_points_data(self):\n        \"\"\"Get the GPU pointer to the points data.\"\"\"\n        return self.gpu_ptr\n\n    def get_device_id(self):\n        \"\"\"Get the device ID where the point cloud data resides.\"\"\"\n        return getattr(self.raw_msg, 'device_id', 0)\n\n    def get_buffer(self):\n        return self.gpu_ptr\n\n    def get_event(self):\n        \"\"\"Get the CUDA event associated with this point cloud.\"\"\"\n        if hasattr(self, 'event'):\n            return self.event\n        return None\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/pynitros_type_views/pynitros_tensor_list_view.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport math\n\nimport cuda.bindings.driver as driver\nimport cuda.bindings.runtime as runtime\nfrom isaac_ros_pynitros.utils.tensor_data_type import TensorDataTypeUtils\nimport torch\n\nfrom .pynitros_type_view_base import PyNitrosTypeViewBase\n\n\nclass PyNitrosTensorListView(PyNitrosTypeViewBase):\n    \"\"\"PyNITROS view for NitrosBridgeTensorList.\"\"\"\n\n    def __init__(self, raw_msg, gpu_ptr=None):\n        super().__init__(raw_msg, gpu_ptr)\n        self._tensor_map = {}\n        self._tensors = []\n        self._total_tensor_size = 0\n\n        if (isinstance(self.raw_msg, self.nitros_bridge_msg_types)):\n            sender_pid, memblock_fd = self.get_pid_fd(self.raw_msg)\n            if (self._open_shm_and_check_uid(sender_pid, memblock_fd) is not True):\n                raise ValueError('UID match failed')\n\n        if (self.gpu_ptr is None):\n            if (isinstance(self.raw_msg, self.nitros_bridge_msg_types)):\n                self.gpu_ptr = self._from_bridge_msg()\n            elif (isinstance(self.raw_msg, self.raw_msg_types)):\n                self.gpu_ptr = self._from_raw_msg()\n            else:\n                raise RuntimeError('Invalid message type')\n        self.gpu_ptr = int(self.gpu_ptr)\n        self._prepare_tensor_view()\n\n    @staticmethod\n    def get_pid_fd(bridge_msg):\n        return bridge_msg.pid, bridge_msg.fd\n\n    def _prepare_tensor_view(self):\n        if (not self.gpu_ptr):\n            raise RuntimeError('Invalid GPU pointer')\n        for nitros_bridge_tensor_msg in self.raw_msg.tensors:\n            cur_gpu_ptr = driver.CUdeviceptr(int(self.gpu_ptr) + self._total_tensor_size)\n            pynitros_tensor_view = PyNitrosTensorView(\n                nitros_bridge_tensor_msg, cur_gpu_ptr)\n            tensor_size = pynitros_tensor_view.get_tensor_size()\n            self._total_tensor_size += tensor_size\n\n            self._tensor_map[nitros_bridge_tensor_msg.name] = pynitros_tensor_view\n            self._tensors.append(pynitros_tensor_view)\n\n    def _from_bridge_msg(self):\n        sending_pid = self.raw_msg.pid\n        memblock_fd = self.raw_msg.fd\n        data_size = 0\n        for tensor in self.raw_msg.tensors:\n            data_size += \\\n                math.prod(tensor.shape.dims) * \\\n                TensorDataTypeUtils.get_size_in_bytes(tensor.data_type)\n        virtual_ptr = self._import_gpu_ptr_from_fd(sending_pid, memblock_fd, data_size)\n        return virtual_ptr\n\n    def _from_raw_msg(self):\n        total_tensor_size, offset = 0, 0\n        for tensor in self.raw_msg.tensors:\n            total_tensor_size += \\\n                math.prod(tensor.shape.dims) * \\\n                TensorDataTypeUtils.get_size_in_bytes(tensor.data_type)\n        err, device_ptr = runtime.cudaMalloc(total_tensor_size)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        for tensor in self.raw_msg.tensors:\n            tensor_size = \\\n                math.prod(tensor.shape.dims) * \\\n                TensorDataTypeUtils.get_size_in_bytes(tensor.data_type)\n            err, = runtime.cudaMemcpy(device_ptr+offset, tensor.data, tensor_size,\n                                      runtime.cudaMemcpyKind.cudaMemcpyHostToDevice)\n            self.ASSERT_CUDA_SUCCESS(err)\n            offset += tensor_size\n        return device_ptr\n\n    def get_buffer(self):\n        return self.gpu_ptr\n\n    def get_tensor_count(self):\n        return len(self.tensors)\n\n    def get_named_tensor(self, name):\n        return self._tensor_map[name]\n\n    def get_all_tensors(self):\n        return self._tensors\n\n    def get_size_in_bytes(self):\n        return self._total_tensor_size\n\n\nclass PyNitrosTensorView(PyNitrosTypeViewBase):\n    \"\"\"PyNITROS view for NitrosBridgeTensor.\"\"\"\n\n    def __init__(self, raw_msg, gpu_ptr):\n        super().__init__(raw_msg, gpu_ptr)\n        self.shape = torch.Size(self.raw_msg.shape.dims)\n        self.gpu_ptr = int(self.gpu_ptr)\n        self.element_count = math.prod(self.raw_msg.shape.dims)\n        self.__cuda_array_interface__ = {\n            'shape': self.shape,\n            'strides': self.raw_msg.strides,\n            'typestr': TensorDataTypeUtils.get_typestr(self.raw_msg.data_type),\n            'data': (self.gpu_ptr, False),\n            'version': 0\n        }\n\n    def get_name(self):\n        return self.raw_msg.name\n\n    def get_buffer(self):\n        return self.gpu_ptr\n\n    def get_rank(self):\n        return self.raw_msg.shape.rank\n\n    def get_bytes_per_element(self):\n        return TensorDataTypeUtils.get_size_in_bytes(self.raw_msg.data_type)\n\n    def get_element_count(self):\n        return self.element_count\n\n    def get_tensor_size(self):\n        return self.get_element_count() * self.get_bytes_per_element()\n\n    def get_shape(self):\n        return tuple(self.raw_msg.shape.dims)\n\n    def get_element_type(self):\n        return self.raw_msg.data_type\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/pynitros_type_views/pynitros_type_view_base.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport ctypes\nfrom ctypes import c_long\nimport math\nimport os\n\nimport cuda.bindings.driver as driver\nimport cuda.bindings.runtime as runtime\nfrom isaac_ros_nitros_bridge_interfaces.msg import (\n    NitrosBridgeImage,\n    NitrosBridgePointCloud,\n    NitrosBridgeTensorList\n)\nfrom isaac_ros_pynitros.utils.cpu_shared_mem import CPUSharedMem\nfrom isaac_ros_tensor_list_interfaces.msg import TensorList\nimport rclpy\nfrom sensor_msgs.msg import Image\nfrom sensor_msgs.msg import PointCloud2\n\n\nclass PyNitrosTypeViewBase():\n    \"\"\"\n    Base class of PyNITROS message view.\n\n    PyNITROS message view provides functionalities to access the metadata\n    and the GPU pointer of the underlying NITROS Bridge message. It can\n    also be converted to other data type such as PyTorch tensor or cupy array.\n    \"\"\"\n\n    # Define the syscall numbers for pidfd_open and pidfd_getfd\n    SYS_pidfd_open = 434\n    SYS_pidfd_getfd = 438\n\n    # Define the syscall function\n    syscall = ctypes.CDLL(None).syscall\n    syscall.restype = c_long\n    syscall.argtypes = [c_long, c_long,\n                        c_long, c_long, c_long, c_long]\n\n    nitros_bridge_msg_types = (\n        NitrosBridgeImage,\n        NitrosBridgePointCloud,\n        NitrosBridgeTensorList\n    )\n\n    raw_msg_types = (\n        Image,\n        TensorList,\n        PointCloud2\n    )\n\n    def __init__(self, raw_msg, gpu_ptr):\n        self.gpu_ptr = gpu_ptr\n        self.raw_msg = raw_msg\n        self.cuda_stream = None\n        self.handle = None\n        self._cpu_shared_mem = None\n        self._mapped_size = None\n\n    def _open_shm_and_check_uid(self, sender_pid, memblock_fd):\n        self._cpu_shared_mem = CPUSharedMem()\n        # If UID presents (for backward compatibility),\n        # compare UID with it in shared mem and update refcount\n        if (self.raw_msg.uid):\n            # Create instance of shared cpu mem pool\n            self._cpu_shared_mem.open_shm(str(sender_pid) + str(memblock_fd))\n\n            # Acquire lock, check UID, increase refcount if UID match\n            self._cpu_shared_mem.lock.acquire()\n            memblock_refcount, memblock_uuid = self._cpu_shared_mem.get_value()\n            if str(memblock_uuid) != str(self.raw_msg.uid):\n                self._cpu_shared_mem.lock.release()\n                rclpy.logging.get_logger('PyNITROS Subscriber').info(\n                    f'MEMBLOCK UID: {memblock_uuid} != \\\n                    MSG UID: {self.raw_msg.uid}, dropping message'\n                )\n                return False\n            else:\n                self._cpu_shared_mem.update_refcount(1)\n                self._cpu_shared_mem.lock.release()\n        return True\n\n    def _import_gpu_ptr_from_fd(self, sender_pid, memblock_fd, data_size):\n        \"\"\"Import the shared memory from file descriptor.\"\"\"\n        # FD -> Handle\n        pidfd = self._pidfd_open(sender_pid, 0)\n        if pidfd == -1:\n            raise RuntimeError('pidfd_open failed')\n\n        new_fd = self._pidfd_getfd(pidfd, memblock_fd, 0)\n        os.close(pidfd)\n        if new_fd == -1:\n            raise RuntimeError('pidfd_getfd failed')\n\n        err, self.handle = driver.cuMemImportFromShareableHandle(\n            new_fd,\n            driver.CUmemAllocationHandleType.CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR)\n        os.close(new_fd)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        # Allocate GPU Space -> CUmemGenericAllocationHandle\n        allocProp = driver.CUmemAllocationProp()\n        allocProp.type = driver.CUmemAllocationType.CU_MEM_ALLOCATION_TYPE_PINNED\n        allocProp.location.type = driver.CUmemLocationType.CU_MEM_LOCATION_TYPE_DEVICE\n        allocProp.requestedHandleTypes = \\\n            driver.CUmemAllocationHandleType.CU_MEM_HANDLE_TYPE_POSIX_FILE_DESCRIPTOR\n        allocProp.location.id = 0\n\n        # Get memory alloc granularity\n        err, granularity = driver.cuMemGetAllocationGranularity(\n            allocProp,\n            driver.CUmemAllocationGranularity_flags.CU_MEM_ALLOC_GRANULARITY_MINIMUM)\n        self.ASSERT_CUDA_SUCCESS(err)\n\n        rounded_data_size = self._round_up(data_size, granularity)\n        self._mapped_size = rounded_data_size\n\n        # Handle -> Tensor Reconstruction\n        res, virtual_ptr = driver.cuMemAddressReserve(\n            rounded_data_size, 0, 0, 0)\n        self.ASSERT_CUDA_SUCCESS(res)\n\n        # Map Handle to Virtual Address Range, Set Access\n        res, = driver.cuMemMap(virtual_ptr, rounded_data_size, 0, self.handle, 0)\n        self.ASSERT_CUDA_SUCCESS(res)\n\n        accessDesc = driver.CUmemAccessDesc()\n        accessDesc.location = allocProp.location\n        accessDesc.flags = driver.CUmemAccess_flags.CU_MEM_ACCESS_FLAGS_PROT_READWRITE\n\n        res, = driver.cuMemSetAccess(\n            virtual_ptr, rounded_data_size, [accessDesc], 1)\n        self.ASSERT_CUDA_SUCCESS(res)\n\n        return virtual_ptr\n\n    def _round_up(self, size, granularity):\n        mult = math.ceil(size / float(granularity))\n        return mult * granularity\n\n    def _pidfd_open(self, pid, flags):\n        return self.syscall(self.SYS_pidfd_open, pid, flags, 0, 0, 0, 0)\n\n    def _pidfd_getfd(self, pidfd, targetfd, flags):\n        return self.syscall(self.SYS_pidfd_getfd, pidfd, targetfd, flags, 0, 0, 0)\n\n    def ASSERT_CUDA_SUCCESS(self, err):\n        if isinstance(err, driver.CUresult):\n            if err != driver.CUresult.CUDA_SUCCESS:\n                raise RuntimeError(\n                    f'[Cuda Error: {err}], {driver.cuGetErrorString(err)}')\n        elif isinstance(err, runtime.cudaError_t):\n            if (err != 0):\n                raise RuntimeError(f'CudaRT Error: {err}')\n        else:\n            raise RuntimeError('Unknown error type: {}'.format(err))\n\n    def set_stream(self, stream):\n        \"\"\"Fill the CUDA stream if async CUDA operations are in the fly.\"\"\"\n        self.cuda_stream = stream\n\n    def postprocess(self):\n        \"\"\"Postprocess the view.\"\"\"\n        if self.cuda_stream:\n            err, = runtime.cudaStreamSynchronize(self.cuda_stream)\n            self.ASSERT_CUDA_SUCCESS(err)\n        if (type(self.raw_msg) in self.nitros_bridge_msg_types):\n            # Decrease the refcount\n            if (self.raw_msg.uid):\n                self._cpu_shared_mem.lock.acquire()\n                self._cpu_shared_mem.update_refcount(-1)\n                self._cpu_shared_mem.lock.release()\n                self._cpu_shared_mem.cpu_shared_mem_obj.close()\n                self._cpu_shared_mem.cpu_shared_mem.close_fd()\n        else:\n            # Free the memory\n            runtime.cudaFree(self.gpu_ptr)\n\n    def get_handle(self):\n        return self.handle\n\n    def get_mapped_size(self):\n        return self._mapped_size\n\n    def get_frame_id(self):\n        return self.raw_msg.header.frame_id\n\n    def get_timestamp_seconds(self):\n        return self.raw_msg.header.stamp.sec\n\n    def get_timestamp_nanoseconds(self):\n        return self.raw_msg.header.stamp.nanosec\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/utils/__init__.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/utils/cpu_shared_mem.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nimport mmap\nimport struct\nimport uuid\n\nimport posix_ipc\n\n\nclass CPUSharedMem():\n    \"\"\"Shared CPU Mem for tracking CUDA Mem Block Info.\"\"\"\n\n    def __init__(self):\n        self.map_element_size = struct.calcsize('i16s')\n        self.cpu_shared_mem = None\n        self.cpu_shared_mem_obj = None\n        self.lock = None\n\n    def create_shm(self, name):\n        \"\"\"Create a shared memory object with a given name.\"\"\"\n        self.cpu_shared_mem = posix_ipc.SharedMemory(\n            name, posix_ipc.O_CREAT, size=self.map_element_size)\n\n        # Attach the shared memory to a byte array\n        self.cpu_shared_mem_obj = mmap.mmap(\n            self.cpu_shared_mem.fd, self.map_element_size)\n        self.lock = posix_ipc.Semaphore(\n            name, posix_ipc.O_CREAT, initial_value=1)\n\n    def open_shm(self, name):\n        \"\"\"Open a shared memory object with a given name.\"\"\"\n        self.cpu_shared_mem = posix_ipc.SharedMemory(name)\n\n        # Attach the shared memory to a byte array\n        self.cpu_shared_mem_obj = mmap.mmap(\n            self.cpu_shared_mem.fd, self.map_element_size)\n        self.lock = posix_ipc.Semaphore(name)\n\n    def update_refcount(self, val):\n        \"\"\"Update the reference count of the shared memory.\"\"\"\n        memblock_refcount, memblock_uuid = self.get_value()\n        self.put_value((memblock_refcount + val, memblock_uuid))\n\n    def put_value(self, value):\n        \"\"\"Write a value to the shared memory.\"\"\"\n        packed_value = struct.pack('i16s', value[0], value[1].bytes)\n\n        self.cpu_shared_mem_obj.seek(0)\n        self.cpu_shared_mem_obj.write(packed_value)\n        self.cpu_shared_mem_obj.flush()\n\n    def get_value(self):\n        \"\"\"Read a value from the shared memory.\"\"\"\n        self.cpu_shared_mem_obj.seek(0)\n        packed_value = self.cpu_shared_mem_obj.read()\n\n        unpacked_value = struct.unpack('i16s', packed_value)\n\n        # Extract relevant parts of the unpacked value\n        memblock_refcount = unpacked_value[0]\n        memblock_uuid_bytes = unpacked_value[1]\n\n        # Convert UUID bytes to UUID object\n        memblock_uuid = uuid.UUID(bytes=memblock_uuid_bytes)\n\n        return memblock_refcount, memblock_uuid\n\n    def close(self):\n        \"\"\"Close the shared memory object.\"\"\"\n        if self.cpu_shared_mem_obj:\n            self.cpu_shared_mem_obj.close()\n        if self.cpu_shared_mem:\n            self.cpu_shared_mem.close_fd()\n            self.cpu_shared_mem.unlink()\n        if self.lock:\n            self.lock.unlink()\n"
  },
  {
    "path": "isaac_ros_pynitros/isaac_ros_pynitros/utils/tensor_data_type.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\n\nclass TensorDataTypeUtils():\n    \"\"\"\n    Helper class to provide utility functions for tensor data types.\n\n    Use the following int values to represent tensor data types:\n    -  1: \"int8\"\n    -  2: \"uint8\"\n    -  3: \"int16\"\n    -  4: \"uint16\"\n    -  5: \"int32\"\n    -  6: \"uint32\"\n    -  7: \"int64\"\n    -  8: \"uint64\"\n    -  9: \"float32\"\n    - 10: \"float64\"\n    \"\"\"\n\n    data_type_map = {\n        1: ('|i1', 1),\n        2: ('|u1', 1),\n        3: ('|i2', 2),\n        4: ('|u2', 2),\n        5: ('|i4', 4),\n        6: ('|u4', 4),\n        7: ('|i8', 8),\n        8: ('|u8', 8),\n        9: ('|f4', 4),\n        10: ('|f8', 8)\n    }\n\n    @classmethod\n    def get_typestr(cls, data_type):\n        return cls.data_type_map[data_type][0]\n\n    @classmethod\n    def get_size_in_bytes(cls, data_type):\n        return cls.data_type_map[data_type][1]\n"
  },
  {
    "path": "isaac_ros_pynitros/launch/isaac_ros_pynitros_dnn_image_encoder.launch.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\"\"\"DNN image encoder launch file using PyNITROS.\"\"\"\n\nimport launch\n\nfrom launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction\nfrom launch.substitutions import LaunchConfiguration\nfrom launch_ros.actions import Node\nimport launch_testing.actions\n\n\ndef launch_setup(context):\n    rosbag_path = LaunchConfiguration('rosbag_path').perform(context)\n\n    rosbag_play = ExecuteProcess(\n        cmd=['ros2', 'bag', 'play', rosbag_path, '--loop'],\n        output='screen')\n\n    dnn_image_encoder = Node(name='dnn_image_encoder',\n                             package='isaac_ros_pynitros',\n                             executable='pynitros_dnn_image_encoder_node',\n                             parameters=[{\n                                'enable_ros_subscribe': True,\n                                'enable_ros_publish': True,\n                                'network_image_width': 960,\n                                'network_image_height': 544,\n                                'image_mean': [0.5, 0.5, 0.5],\n                                'image_std': [0.5, 0.5, 0.5]\n                             }],\n                             remappings=[\n                                ('pynitros_input_msg', '/image_rect'),\n                                ('pynitros_output_msg', 'pynitros1_output_msg'),\n                                ('pynitros_output_msg_ros', 'tensor_pub')\n                             ],\n                             output='screen')\n\n    return [rosbag_play,\n            dnn_image_encoder]\n\n\ndef generate_launch_description():\n    launch_args = [\n        DeclareLaunchArgument(\n            'rosbag_path',\n            description='Remapped publish image name of NITROS Bridge ROS2'),\n    ]\n\n    nodes = launch_args + [OpaqueFunction(function=launch_setup)]\n    return launch.LaunchDescription(\n        nodes + [\n            # Start tests after a fixed delay for node startup\n            launch.actions.TimerAction(\n                period=30.0,\n                actions=[launch_testing.actions.ReadyToTest()])\n        ]\n    )\n"
  },
  {
    "path": "isaac_ros_pynitros/launch/isaac_ros_pynitros_quickstart.launch.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\"\"\"\nQuickstart launch file for PyNITROS.\n\nThis launch file use two image forward nodes built with PyNITROS to demonstrate\nthe zero-copy interop between PyNITROS nodes.\n\"\"\"\n\nimport launch\n\nfrom launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction\nfrom launch.substitutions import LaunchConfiguration\nfrom launch_ros.actions import Node\nimport launch_testing.actions\n\n\ndef launch_setup(context):\n    rosbag_path = LaunchConfiguration('rosbag_path').perform(context)\n\n    rosbag_play = ExecuteProcess(\n        cmd=['ros2', 'bag', 'play', rosbag_path, '--loop', '--remap',\n             '/hawk_0_left_rgb_image:=/pynitros1_input_msg'],\n        output='screen')\n\n    pynitros_image_forward_node_1 = Node(\n        name='pynitros_image_forward_node_1',\n        package='isaac_ros_pynitros',\n        executable='pynitros_image_forward_node',\n        parameters=[{\n            'enable_ros_subscribe': True,\n            'enable_ros_publish': False\n        }],\n        remappings=[\n            ('pynitros_input_msg', 'pynitros1_input_msg'),\n            ('pynitros_output_msg', 'pynitros1_output_msg'),\n            ('pynitros_output_msg_ros', 'pynitros1_output_msg_ros')\n        ],\n        output='screen'\n    )\n\n    pynitros_image_forward_node_2 = Node(\n        name='pynitros_image_forward_node_2',\n        package='isaac_ros_pynitros',\n        executable='pynitros_image_forward_node',\n        parameters=[{\n            'enable_ros_subscribe': False,\n            'enable_ros_publish': True\n        }],\n        remappings=[\n            ('pynitros_input_msg', 'pynitros1_output_msg'),\n            ('pynitros_output_msg', 'pynitros2_output_msg'),\n            ('pynitros_output_msg_ros', 'pynitros2_output_msg_ros')\n        ],\n        output='screen'\n    )\n    return [rosbag_play,\n            pynitros_image_forward_node_1,\n            pynitros_image_forward_node_2]\n\n\ndef generate_launch_description():\n    launch_args = [\n        DeclareLaunchArgument(\n            'rosbag_path',\n            description='Remapped publish image name of NITROS Bridge ROS2'),\n    ]\n\n    nodes = launch_args + [OpaqueFunction(function=launch_setup)]\n    return launch.LaunchDescription(\n        nodes + [\n            # Start tests after a fixed delay for node startup\n            launch.actions.TimerAction(\n                period=30.0,\n                actions=[launch_testing.actions.ReadyToTest()])\n        ]\n    )\n"
  },
  {
    "path": "isaac_ros_pynitros/launch/isaac_ros_pynitros_to_nitros.launch.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\"\"\"\nLaunch file for PyNITROS and NITROS interop.\n\nThis launch file use one PyNITROS image forward node\nand one NITROS bridge node to demonstrate the zero-copy interop PyNITROS and NITROS.\n\"\"\"\n\nimport launch\n\nfrom launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction\nfrom launch.substitutions import LaunchConfiguration\nfrom launch_ros.actions import ComposableNodeContainer, Node\nfrom launch_ros.descriptions import ComposableNode\nimport launch_testing.actions\n\n\ndef launch_setup(context):\n    rosbag_path = LaunchConfiguration('rosbag_path').perform(context)\n\n    rosbag_play = ExecuteProcess(\n        cmd=['ros2', 'bag', 'play', rosbag_path, '--loop', '--remap',\n             '/hawk_0_left_rgb_image:=/pynitros_input_msg'],\n        output='screen')\n\n    # Image forward node implemented with PyNITROS\n    pynitros_image_forward_node = Node(\n        name='pynitros_image_forward_node',\n        package='isaac_ros_pynitros',\n        executable='pynitros_image_forward_node',\n        parameters=[{\n            'enable_ros_subscribe': True,\n            'enable_ros_publish': False\n        }],\n        output='screen'\n    )\n\n    # NITROS bridge node converts NITROS Bridge msg to NITROS msg\n    nitros_bridge_node = ComposableNode(\n        name='nitros_bridge_node',\n        package='isaac_ros_nitros_bridge_ros2',\n        plugin='nvidia::isaac_ros::nitros_bridge::ImageConverterNode',\n        remappings=[\n            ('ros2_input_bridge_image', 'pynitros_output_msg'),\n            ('ros2_output_image', 'output'),\n        ])\n\n    container = ComposableNodeContainer(\n        name='ros2_converter_container',\n        namespace='',\n        package='rclcpp_components',\n        executable='component_container_mt',\n        composable_node_descriptions=[nitros_bridge_node],\n        output='screen',\n        arguments=['--ros-args', '--log-level', 'info']\n    )\n    return [rosbag_play, pynitros_image_forward_node, container]\n\n\ndef generate_launch_description():\n    launch_args = [\n        DeclareLaunchArgument(\n            'rosbag_path',\n            description='Remapped publish image name of NITROS Bridge ROS2'),\n    ]\n\n    nodes = launch_args + [OpaqueFunction(function=launch_setup)]\n    return launch.LaunchDescription(\n        nodes + [\n            # Start tests after a fixed delay for node startup\n            launch.actions.TimerAction(\n                period=30.0,\n                actions=[launch_testing.actions.ReadyToTest()])\n        ]\n    )\n"
  },
  {
    "path": "isaac_ros_pynitros/package.xml",
    "content": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3.org/2001/XMLSchema\"?>\n<package format=\"3\">\n  <name>isaac_ros_pynitros</name>\n  <version>4.4.0</version>\n  <description>PyNITROS</description>\n  <maintainer email=\"isaac-ros-maintainers@nvidia.com\">Isaac ROS Maintainers</maintainer>\n  <license>Apache License 2.0</license>\n  <author>Mihir Rao</author>\n  <author>Yuankun Zhu</author>\n  <build_depend>isaac_ros_common</build_depend>\n\n  <exec_depend>python3-cuda-python-pip-shim</exec_depend>\n  <exec_depend>python3-torch-pip-shim</exec_depend>\n  <exec_depend>posix_ipc</exec_depend>\n  <exec_depend>rclpy</exec_depend>\n  <exec_depend>std_msgs</exec_depend>\n  <exec_depend>isaac_ros_nitros_bridge_interfaces</exec_depend>\n  <exec_depend>isaac_ros_tensor_list_interfaces</exec_depend>\n\n  <test_depend>isaac_ros_test</test_depend>\n  <test_depend>ament_copyright</test_depend>\n  <test_depend>ament_flake8</test_depend>\n  <test_depend>ament_pep257</test_depend>\n  <test_depend>ament_lint_auto</test_depend>\n  <test_depend>ament_lint_common</test_depend>\n  <test_depend>python3-pytest</test_depend>\n\n  <export>\n    <build_type>ament_python</build_type>\n  </export>\n</package>\n"
  },
  {
    "path": "isaac_ros_pynitros/resource/isaac_ros_pynitros",
    "content": ""
  },
  {
    "path": "isaac_ros_pynitros/setup.cfg",
    "content": "[develop]\nscript_dir=$base/lib/isaac_ros_pynitros\n[install]\ninstall_scripts=$base/lib/isaac_ros_pynitros\n"
  },
  {
    "path": "isaac_ros_pynitros/setup.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom glob import glob\nfrom os import path\n\nfrom setuptools import find_packages, setup\n\npackage_name = 'isaac_ros_pynitros'\n\nsetup(\n    name=package_name,\n    version='3.0.0',\n    packages=find_packages(),\n    data_files=[\n        ('share/ament_index/resource_index/packages',\n            ['resource/' + package_name]),\n        ('share/' + package_name, ['package.xml']),\n        (path.join('share', package_name, 'launch'), glob('launch/*.launch.py'))\n    ],\n    install_requires=[\n        'cuda_python',\n        'posix_ipc',\n        'setuptools'\n    ],\n    zip_safe=True,\n    maintainer='Isaac ROS Maintainers',\n    maintainer_email='isaac-ros-maintainers@nvidia.com',\n    description='Execute the control graph in isaac ros integration test',\n    license='Apache-2.0',\n    extras_require={\n        'test': [\n            'pytest',\n        ],\n    },\n    entry_points={\n        'console_scripts': [\n            'pynitros_image_forward_node = \\\n                isaac_ros_pynitros.examples.pynitros_image_forward_node:main',\n            'pynitros_tensor_list_forward_node = \\\n                isaac_ros_pynitros.examples.pynitros_tensor_list_forward_node:main',\n            'pynitros_point_cloud_forward_node = \\\n                isaac_ros_pynitros.examples.pynitros_point_cloud_forward_node:main',\n            'pynitros_dnn_image_encoder_node = \\\n                isaac_ros_pynitros.examples.pynitros_dnn_image_encoder_node:main',\n            'pynitros_message_filter_sync_node = \\\n                isaac_ros_pynitros.examples.pynitros_message_filter_sync_node:main',\n        ],\n    },\n)\n"
  },
  {
    "path": "isaac_ros_pynitros/test/isaac_ros_pynitros_dnn_image_encoder.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\"\"\"Proof-of-Life test for the PyNITROS.\"\"\"\n\nimport os\nimport pathlib\nimport subprocess\nimport time\n\nfrom isaac_ros_tensor_list_interfaces.msg import TensorList\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\nimport launch\n\nfrom launch_ros.actions import Node\nimport launch_testing\nimport pytest\nimport rclpy\n\nfrom sensor_msgs.msg import Image\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    ptace_result = subprocess.run(['cat', '/proc/sys/kernel/yama/ptrace_scope'],\n                                  stdout=subprocess.PIPE,\n                                  stderr=subprocess.PIPE,\n                                  text=True)\n    pidfd_getfd_result = subprocess.run(['grep', 'pidfd_getfd', '/proc/kallsyms'],\n                                        stdout=subprocess.PIPE,\n                                        stderr=subprocess.PIPE,\n                                        text=True)\n    if (ptace_result.stdout.strip() == '0' and pidfd_getfd_result.returncode == 0):\n        IsaacROSPyNitrosTest.skip_test = False\n        dnn_image_encoder = Node(name='dnn_image_encoder',\n                                 package='isaac_ros_pynitros',\n                                 namespace='pynitros',\n                                 executable='pynitros_dnn_image_encoder_node',\n                                 parameters=[{\n                                    'enable_ros_subscribe': True,\n                                    'enable_ros_publish': True,\n                                    'network_image_width': 960,\n                                    'network_image_height': 544,\n                                    'image_mean': [0.5, 0.5, 0.5],\n                                    'image_std': [0.5, 0.5, 0.5]\n                                 }],\n                                 remappings=[\n                                    ('pynitros_input_msg', 'pynitros1_input_msg'),\n                                    ('pynitros_output_msg', 'pynitros1_output_msg'),\n                                    ('pynitros_output_msg_ros', 'pynitros1_output_msg_ros')\n                                 ],\n                                 output='screen')\n\n        return IsaacROSPyNitrosTest.generate_test_description([\n            dnn_image_encoder,\n            launch.actions.TimerAction(period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n        ])\n    else:\n        IsaacROSPyNitrosTest.skip_test = True\n        return IsaacROSPyNitrosTest.generate_test_description(\n            [launch_testing.actions.ReadyToTest()])\n\n\nclass IsaacROSPyNitrosTest(IsaacROSBaseTest):\n    \"\"\"Validate Nitros Bridge on Image type.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_image')\n    def test_pynitros_image(self, test_folder) -> None:\n        if self.skip_test:\n            self.skipTest('No ptrace permission! Skipping test.')\n        else:\n            IsaacROSPyNitrosTest.DEFAULT_NAMESPACE = 'pynitros'\n            self.generate_namespace_lookup(['pynitros1_input_msg', 'pynitros1_output_msg_ros'])\n            received_messages = {}\n\n            received_image_sub = self.create_logging_subscribers(\n                subscription_requests=[('pynitros1_output_msg_ros', TensorList)],\n                received_messages=received_messages)\n\n            image_pub = self.node.create_publisher(Image, self.namespaces['pynitros1_input_msg'],\n                                                   self.DEFAULT_QOS)\n\n            try:\n                image = JSONConversion.load_image_from_json(test_folder / 'image.json')\n                timestamp = self.node.get_clock().now().to_msg()\n                image.header.stamp = timestamp\n\n                # Wait at most TIMEOUT seconds for subscriber to respond\n                TIMEOUT = 30\n                end_time = time.time() + TIMEOUT\n\n                done = False\n                while time.time() < end_time:\n                    image_pub.publish(image)\n                    rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                    # If we have received a message on the output topic, break\n                    if 'pynitros1_output_msg_ros' in received_messages:\n                        done = True\n                        break\n\n                self.assertTrue(done, \"Didn't receive output on pynitros1_output_msg_ros topic!\")\n\n                received_image = received_messages['pynitros1_output_msg_ros']\n\n                self.assertEqual(str(timestamp), str(received_image.header.stamp),\n                                 'Timestamps do not match.')\n\n            finally:\n                self.node.destroy_subscription(received_image_sub)\n                self.node.destroy_publisher(image_pub)\n"
  },
  {
    "path": "isaac_ros_pynitros/test/isaac_ros_pynitros_image_test.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\"\"\"Proof-of-Life test for the PyNITROS image type.\"\"\"\n\nimport os\nimport pathlib\nimport subprocess\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\nimport launch\n\nfrom launch_ros.actions import Node\nimport launch_testing\nimport pytest\nimport rclpy\n\nfrom sensor_msgs.msg import Image\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    ptace_result = subprocess.run(['cat', '/proc/sys/kernel/yama/ptrace_scope'],\n                                  stdout=subprocess.PIPE,\n                                  stderr=subprocess.PIPE,\n                                  text=True)\n    pidfd_getfd_result = subprocess.run(['grep', 'pidfd_getfd', '/proc/kallsyms'],\n                                        stdout=subprocess.PIPE,\n                                        stderr=subprocess.PIPE,\n                                        text=True)\n    if (ptace_result.stdout.strip() == '0' and pidfd_getfd_result.returncode == 0):\n        IsaacROSPyNitrosTest.skip_test = False\n        pynitros_image_forward_node_1 = Node(name='pynitros_image_forward_node_1',\n                                             package='isaac_ros_pynitros',\n                                             namespace='pynitros',\n                                             executable='pynitros_image_forward_node',\n                                             parameters=[{\n                                                 'enable_ros_subscribe': True,\n                                                 'enable_ros_publish': False\n                                             }],\n                                             remappings=[\n                                                 ('pynitros_input_msg', 'pynitros1_input_msg'),\n                                                 ('pynitros_output_msg', 'pynitros1_output_msg'),\n                                                 ('pynitros_output_msg_ros',\n                                                  'pynitros1_output_msg_ros')\n                                             ],\n                                             output='screen')\n\n        pynitros_image_forward_node_2 = Node(name='pynitros_image_forward_node_2',\n                                             package='isaac_ros_pynitros',\n                                             executable='pynitros_image_forward_node',\n                                             namespace='pynitros',\n                                             parameters=[{\n                                                 'enable_ros_subscribe': False,\n                                                 'enable_ros_publish': True\n                                             }],\n                                             remappings=[\n                                                 ('pynitros_input_msg', 'pynitros1_output_msg'),\n                                                 ('pynitros_output_msg', 'pynitros2_output_msg'),\n                                                 ('pynitros_output_msg_ros',\n                                                  'pynitros2_output_msg_ros')\n                                             ],\n                                             output='screen')\n\n        return IsaacROSPyNitrosTest.generate_test_description([\n            pynitros_image_forward_node_1, pynitros_image_forward_node_2,\n            launch.actions.TimerAction(period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n        ])\n    else:\n        IsaacROSPyNitrosTest.skip_test = True\n        return IsaacROSPyNitrosTest.generate_test_description(\n            [launch_testing.actions.ReadyToTest()])\n\n\nclass IsaacROSPyNitrosTest(IsaacROSBaseTest):\n    \"\"\"Validate Nitros Bridge on Image type.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_image')\n    def test_pynitros_image(self, test_folder) -> None:\n        if self.skip_test:\n            self.skipTest('No ptrace permission! Skipping test.')\n        else:\n            IsaacROSPyNitrosTest.DEFAULT_NAMESPACE = 'pynitros'\n            self.generate_namespace_lookup(['pynitros1_input_msg', 'pynitros2_output_msg_ros'])\n            received_messages = {}\n\n            received_image_sub = self.create_logging_subscribers(\n                subscription_requests=[('pynitros2_output_msg_ros', Image)],\n                received_messages=received_messages)\n\n            image_pub = self.node.create_publisher(Image, self.namespaces['pynitros1_input_msg'],\n                                                   self.DEFAULT_QOS)\n\n            try:\n                image = JSONConversion.load_image_from_json(test_folder / 'image.json')\n                timestamp = self.node.get_clock().now().to_msg()\n                image.header.stamp = timestamp\n\n                # Wait at most TIMEOUT seconds for subscriber to respond\n                TIMEOUT = 30\n                end_time = time.time() + TIMEOUT\n\n                done = False\n                while time.time() < end_time:\n                    image_pub.publish(image)\n                    rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                    # If we have received a message on the output topic, break\n                    if 'pynitros2_output_msg_ros' in received_messages:\n                        done = True\n                        break\n\n                self.assertTrue(done, \"Didn't receive output on output_image topic!\")\n\n                received_image = received_messages['pynitros2_output_msg_ros']\n\n                self.assertEqual(str(timestamp), str(received_image.header.stamp),\n                                 'Timestamps do not match.')\n\n                self.assertEqual(\n                    len(image.data), len(received_image.data),\n                    'Source and received image sizes do not match: ' +\n                    f'{len(image.data)} != {len(received_image.data)}')\n\n                # Make sure that the source and received images are the same\n                self.assertEqual(\n                    received_image.height, image.height,\n                    'Source and received image heights do not match: ' +\n                    f'{image.height} != {received_image.height}')\n                self.assertEqual(\n                    received_image.width, image.width,\n                    'Source and received image widths do not match: ' +\n                    f'{image.width} != {received_image.width}')\n\n                for i in range(len(image.data)):\n                    self.assertEqual(image.data[i], received_image.data[i],\n                                     'Source and received image pixels do not match')\n\n            finally:\n                self.node.destroy_subscription(received_image_sub)\n                self.node.destroy_publisher(image_pub)\n"
  },
  {
    "path": "isaac_ros_pynitros/test/isaac_ros_pynitros_point_cloud_test.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\"\"\"Proof-of-Life test for the PyNITROS point cloud type.\"\"\"\n\nimport os\nimport pathlib\nimport subprocess\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, PCDLoader\nimport launch\n\nfrom launch_ros.actions import Node\nimport launch_testing\nimport pytest\nimport rclpy\n\nfrom sensor_msgs.msg import PointCloud2\n\n\nclass IsaacROSPyNitrosPointCloudTest(IsaacROSBaseTest):\n    \"\"\"Validate Nitros Bridge on Point Cloud type.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n    skip_test = False  # Initialize the class attribute\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_point_cloud')\n    def test_pynitros_point_cloud(self, test_folder) -> None:\n        if IsaacROSPyNitrosPointCloudTest.skip_test:\n            self.skipTest('No ptrace permission! Skipping test.')\n        else:\n            IsaacROSPyNitrosPointCloudTest.DEFAULT_NAMESPACE = 'pynitros'\n            self.generate_namespace_lookup(['pynitros1_input_msg', 'pynitros2_output_msg_ros'])\n            received_messages = {}\n\n            received_point_cloud_sub = self.create_logging_subscribers(\n                subscription_requests=[\n                    ('pynitros2_output_msg_ros', PointCloud2)],\n                received_messages=received_messages)\n\n            point_cloud_pub = self.node.create_publisher(\n                PointCloud2, self.namespaces['pynitros1_input_msg'],\n                self.DEFAULT_QOS)\n\n            try:\n                # Use the standard PCDLoader approach like other PointCloud2\n                # tests\n                point_cloud = PCDLoader.generate_pointcloud2_from_pcd_file(\n                    test_folder / 'ketchup.pcd', 'test_frame')\n                point_cloud.header.stamp = self.node.get_clock().now().to_msg()\n\n                # Wait at most TIMEOUT seconds for subscriber to respond\n                TIMEOUT = 30\n                end_time = time.time() + TIMEOUT\n\n                done = False\n                while time.time() < end_time:\n                    point_cloud_pub.publish(point_cloud)\n                    rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                    # If we have received a message on the output topic, break\n                    if 'pynitros2_output_msg_ros' in received_messages:\n                        done = True\n                        break\n\n                self.assertTrue(\n                    done,\n                    \"Didn't receive output on output_point_cloud topic!\")\n\n                received_point_cloud = received_messages['pynitros2_output_msg_ros']\n\n                self.assertEqual(\n                    str(point_cloud.header.stamp),\n                    str(received_point_cloud.header.stamp),\n                    'Timestamps do not match.')\n\n                self.assertEqual(\n                    len(point_cloud.data), len(received_point_cloud.data),\n                    'Source and received point cloud sizes do not match: ' +\n                    f'{len(point_cloud.data)} != '\n                    f'{len(received_point_cloud.data)}')\n\n                # Make sure that the source and received point clouds are the same\n                self.assertEqual(received_point_cloud.height, point_cloud.height,\n                                 'Source and received point cloud heights do not match: ' +\n                                 f'{point_cloud.height} != {received_point_cloud.height}')\n                self.assertEqual(received_point_cloud.width, point_cloud.width,\n                                 'Source and received point cloud widths do not match: ' +\n                                 f'{point_cloud.width} != {received_point_cloud.width}')\n                self.assertEqual(received_point_cloud.is_bigendian, point_cloud.is_bigendian,\n                                 'Source and received point cloud is_bigendian do not match: ' +\n                                 f'{point_cloud.is_bigendian} != '\n                                 f'{received_point_cloud.is_bigendian}')\n                self.assertEqual(received_point_cloud.point_step, point_cloud.point_step,\n                                 'Source and received point cloud point_step do not match: ' +\n                                 f'{point_cloud.point_step} != {received_point_cloud.point_step}')\n                self.assertEqual(received_point_cloud.row_step, point_cloud.row_step,\n                                 'Source and received point cloud row_step do not match: ' +\n                                 f'{point_cloud.row_step} != {received_point_cloud.row_step}')\n                self.assertEqual(received_point_cloud.is_dense, point_cloud.is_dense,\n                                 'Source and received point cloud is_dense do not match: ' +\n                                 f'{point_cloud.is_dense} != {received_point_cloud.is_dense}')\n                self.assertEqual(received_point_cloud.fields, point_cloud.fields,\n                                 'Source and received point cloud fields do not match: ' +\n                                 f'{point_cloud.fields} != {received_point_cloud.fields}')\n                for i in range(len(point_cloud.data)):\n                    self.assertEqual(\n                        point_cloud.data[i], received_point_cloud.data[i],\n                        'Source and received point cloud data do not match')\n\n                print('Source and receive point cloud match!')\n\n            finally:\n                self.node.destroy_subscription(received_point_cloud_sub)\n                self.node.destroy_publisher(point_cloud_pub)\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    ptrace_result = subprocess.run(\n        ['cat', '/proc/sys/kernel/yama/ptrace_scope'],\n        stdout=subprocess.PIPE,\n        stderr=subprocess.PIPE,\n        text=True)\n    pidfd_getfd_result = subprocess.run(\n        ['grep', 'pidfd_getfd', '/proc/kallsyms'],\n        stdout=subprocess.PIPE,\n        stderr=subprocess.PIPE,\n        text=True)\n    if (ptrace_result.stdout.strip() == '0' and pidfd_getfd_result.returncode == 0):\n        IsaacROSPyNitrosPointCloudTest.skip_test = False\n        pynitros_point_cloud_forward_node_1 = Node(\n            name='pynitros_point_cloud_forward_node_1',\n            package='isaac_ros_pynitros',\n            namespace='pynitros',\n            executable='pynitros_point_cloud_forward_node',\n            parameters=[{\n                'enable_ros_subscribe': True,\n                'enable_ros_publish': False\n            }],\n            remappings=[\n                ('topic_forward_input_ros', 'pynitros1_input_msg'),\n                ('topic_forward_output', 'pynitros1_output_msg'),\n                ('topic_forward_output_ros', 'pynitros1_output_msg_ros')\n            ],\n            output='screen')\n\n        pynitros_point_cloud_forward_node_2 = Node(\n            name='pynitros_point_cloud_forward_node_2',\n            package='isaac_ros_pynitros',\n            executable='pynitros_point_cloud_forward_node',\n            namespace='pynitros',\n            parameters=[{\n                'enable_ros_subscribe': False,\n                'enable_ros_publish': True\n            }],\n            remappings=[\n                ('topic_forward_input', 'pynitros1_output_msg'),\n                ('topic_forward_output', 'pynitros2_output_msg'),\n                ('topic_forward_output_ros', 'pynitros2_output_msg_ros')\n            ],\n            output='screen')\n\n        return IsaacROSPyNitrosPointCloudTest.generate_test_description([\n            pynitros_point_cloud_forward_node_1,\n            pynitros_point_cloud_forward_node_2,\n            launch.actions.TimerAction(\n                period=2.5,\n                actions=[launch_testing.actions.ReadyToTest()])\n        ])\n    else:\n        IsaacROSPyNitrosPointCloudTest.skip_test = True\n        return IsaacROSPyNitrosPointCloudTest.generate_test_description([\n            launch_testing.actions.ReadyToTest()])\n"
  },
  {
    "path": "isaac_ros_pynitros/test/isaac_ros_pynitros_sync_test.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\"\"\"Proof-of-Life test for the PyNITROS message filter.\"\"\"\n\nimport os\nimport pathlib\nimport subprocess\nimport time\n\nfrom isaac_ros_test import IsaacROSBaseTest, JSONConversion\nimport launch\n\nfrom launch_ros.actions import Node\nimport launch_testing\nimport pytest\nimport rclpy\n\nfrom sensor_msgs.msg import CameraInfo, Image\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    ptace_result = subprocess.run(['cat', '/proc/sys/kernel/yama/ptrace_scope'],\n                                  stdout=subprocess.PIPE,\n                                  stderr=subprocess.PIPE,\n                                  text=True)\n    pidfd_getfd_result = subprocess.run(['grep', 'pidfd_getfd', '/proc/kallsyms'],\n                                        stdout=subprocess.PIPE,\n                                        stderr=subprocess.PIPE,\n                                        text=True)\n    if (ptace_result.stdout.strip() == '0' and pidfd_getfd_result.returncode == 0):\n        IsaacROSPyNitrosTest.skip_test = False\n        pynitros_image_forward_node = Node(name='pynitros_image_forward_node',\n                                           package='isaac_ros_pynitros',\n                                           namespace='pynitros',\n                                           executable='pynitros_image_forward_node',\n                                           parameters=[{\n                                                'enable_ros_subscribe': True,\n                                                'enable_ros_publish': False\n                                           }],\n                                           remappings=[\n                                                ('pynitros_input_msg', 'pynitros1_input_msg'),\n                                                ('pynitros_output_msg', 'pynitros1_output_msg'),\n                                           ],\n                                           output='screen')\n        pynitros_sync_node = Node(name='pynitros_sync_node',\n                                  package='isaac_ros_pynitros',\n                                  namespace='pynitros',\n                                  executable='pynitros_message_filter_sync_node',\n                                  parameters=[{\n                                        'enable_ros_subscribe': False,\n                                        'enable_ros_publish': True\n                                   }],\n                                  remappings=[\n                                        ('image', 'pynitros1_output_msg')\n                                  ],\n                                  output='screen')\n\n        return IsaacROSPyNitrosTest.generate_test_description([\n            pynitros_image_forward_node,\n            pynitros_sync_node,\n            launch.actions.TimerAction(period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n        ])\n    else:\n        IsaacROSPyNitrosTest.skip_test = True\n        return IsaacROSPyNitrosTest.generate_test_description(\n            [launch_testing.actions.ReadyToTest()])\n\n\nclass IsaacROSPyNitrosTest(IsaacROSBaseTest):\n    \"\"\"Validate Nitros Bridge on Image type.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_image')\n    def test_pynitros_image(self, test_folder) -> None:\n        if self.skip_test:\n            self.skipTest('No ptrace permission! Skipping test.')\n        else:\n            IsaacROSPyNitrosTest.DEFAULT_NAMESPACE = 'pynitros'\n            self.generate_namespace_lookup(['pynitros1_input_msg',\n                                            'camera_info',\n                                            'synced_output_ros'])\n            received_messages = {}\n\n            received_image_sub = self.create_logging_subscribers(\n                subscription_requests=[('synced_output_ros', Image)],\n                received_messages=received_messages)\n\n            image_pub = self.node.create_publisher(Image,\n                                                   self.namespaces['pynitros1_input_msg'],\n                                                   self.DEFAULT_QOS)\n            camera_info_pub = self.node.create_publisher(CameraInfo,\n                                                         self.namespaces['camera_info'],\n                                                         self.DEFAULT_QOS)\n            try:\n                image = JSONConversion.load_image_from_json(test_folder / 'image.json')\n                timestamp = self.node.get_clock().now().to_msg()\n                image.header.stamp = timestamp\n\n                camera_info = CameraInfo()\n                camera_info.header.stamp = timestamp\n                # Wait at most TIMEOUT seconds for subscriber to respond\n                TIMEOUT = 30\n                end_time = time.time() + TIMEOUT\n\n                done = False\n                while time.time() < end_time:\n                    image_pub.publish(image)\n                    camera_info_pub.publish(camera_info)\n                    rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                    # If we have received a message on the output topic, break\n                    if 'synced_output_ros' in received_messages:\n                        done = True\n                        break\n\n                self.assertTrue(done, \"Didn't receive output on output_image topic!\")\n\n                received_image = received_messages['synced_output_ros']\n\n                self.assertEqual(str(timestamp), str(received_image.header.stamp),\n                                 'Timestamps do not match.')\n\n                self.assertEqual(\n                    len(image.data), len(received_image.data),\n                    'Source and received image sizes do not match: ' +\n                    f'{len(image.data)} != {len(received_image.data)}')\n\n                # Make sure that the source and received images are the same\n                self.assertEqual(\n                    received_image.height, image.height,\n                    'Source and received image heights do not match: ' +\n                    f'{image.height} != {received_image.height}')\n                self.assertEqual(\n                    received_image.width, image.width,\n                    'Source and received image widths do not match: ' +\n                    f'{image.width} != {received_image.width}')\n\n                for i in range(len(image.data)):\n                    self.assertEqual(image.data[i], received_image.data[i],\n                                     'Source and received image pixels do not match')\n\n            finally:\n                self.node.destroy_subscription(received_image_sub)\n                self.node.destroy_publisher(image_pub)\n"
  },
  {
    "path": "isaac_ros_pynitros/test/isaac_ros_pynitros_tensor_list_test.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\"\"\"Proof-of-Life test for the PyNITROS tensor list type.\"\"\"\n\nimport os\nimport pathlib\nimport subprocess\nimport time\n\nfrom isaac_ros_tensor_list_interfaces.msg import Tensor, TensorList, TensorShape\nfrom isaac_ros_test import IsaacROSBaseTest\nimport launch\n\nfrom launch_ros.actions import Node\nimport launch_testing\nimport numpy as np\nimport pytest\nimport rclpy\n\n\n@pytest.mark.rostest\ndef generate_test_description():\n    \"\"\"Generate launch description with all ROS 2 nodes for testing.\"\"\"\n    ptace_result = subprocess.run(['cat', '/proc/sys/kernel/yama/ptrace_scope'],\n                                  stdout=subprocess.PIPE,\n                                  stderr=subprocess.PIPE,\n                                  text=True)\n    pidfd_getfd_result = subprocess.run(['grep', 'pidfd_getfd', '/proc/kallsyms'],\n                                        stdout=subprocess.PIPE,\n                                        stderr=subprocess.PIPE,\n                                        text=True)\n    if (ptace_result.stdout.strip() == '0' and pidfd_getfd_result.returncode == 0):\n        IsaacROSPyNitrosTest.skip_test = False\n        pynitros_tensor_list_forward_node_1 = Node(\n            name='pynitros_tensor_list_forward_node_1',\n            package='isaac_ros_pynitros',\n            namespace='pynitros',\n            executable='pynitros_tensor_list_forward_node',\n            parameters=[{\n                'enable_ros_subscribe': True,\n                'enable_ros_publish': False\n            }],\n            remappings=[('pynitros_input_msg', 'pynitros1_input_msg'),\n                        ('pynitros_output_msg', 'pynitros1_output_msg'),\n                        ('pynitros_output_msg_ros', 'pynitros1_output_msg_ros')],\n            output='screen')\n\n        pynitros_tensor_list_forward_node_2 = Node(\n            name='pynitros_tensor_list_forward_node_2',\n            package='isaac_ros_pynitros',\n            executable='pynitros_tensor_list_forward_node',\n            namespace='pynitros',\n            parameters=[{\n                'enable_ros_subscribe': False,\n                'enable_ros_publish': True\n            }],\n            remappings=[('pynitros_input_msg', 'pynitros1_output_msg'),\n                        ('pynitros_output_msg', 'pynitros2_output_msg'),\n                        ('pynitros_output_msg_ros', 'pynitros2_output_msg_ros')],\n            output='screen')\n\n        return IsaacROSPyNitrosTest.generate_test_description([\n            pynitros_tensor_list_forward_node_1, pynitros_tensor_list_forward_node_2,\n            launch.actions.TimerAction(period=2.5, actions=[launch_testing.actions.ReadyToTest()])\n        ])\n    else:\n        IsaacROSPyNitrosTest.skip_test = True\n        return IsaacROSPyNitrosTest.generate_test_description(\n            [launch_testing.actions.ReadyToTest()])\n\n\nclass IsaacROSPyNitrosTest(IsaacROSBaseTest):\n    \"\"\"Validate Nitros Bridge on Tensor List type.\"\"\"\n\n    filepath = pathlib.Path(os.path.dirname(__file__))\n\n    @IsaacROSBaseTest.for_each_test_case(subfolder='nitros_image')\n    def test_pynitros_tensor_list(self, test_folder) -> None:\n        if self.skip_test:\n            self.skipTest('No ptrace permission! Skipping test.')\n        else:\n            IsaacROSPyNitrosTest.DEFAULT_NAMESPACE = 'pynitros'\n            self.generate_namespace_lookup(['pynitros1_input_msg', 'pynitros2_output_msg_ros'])\n            received_messages = {}\n\n            received_tensor_list_sub = self.create_logging_subscribers(\n                subscription_requests=[('pynitros2_output_msg_ros', TensorList)],\n                received_messages=received_messages)\n\n            tensor_list_pub = self.node.create_publisher(TensorList,\n                                                         self.namespaces['pynitros1_input_msg'],\n                                                         self.DEFAULT_QOS)\n\n            try:\n                DATA_TYPE = 9\n                INPUT_TENSOR_DIMENSIONS = [1, 3, 100, 100]\n                INPUT_TENSOR_NAME = 'input'\n                INPUT_TENSOR_STRIDE = 4\n\n                tensor_list = TensorList()\n                tensor = Tensor()\n                tensor_shape = TensorShape()\n\n                tensor_shape.rank = len(INPUT_TENSOR_DIMENSIONS)\n                tensor_shape.dims = INPUT_TENSOR_DIMENSIONS\n\n                tensor.shape = tensor_shape\n                tensor.name = INPUT_TENSOR_NAME\n                tensor.data_type = DATA_TYPE\n                data_length = INPUT_TENSOR_STRIDE * np.prod(INPUT_TENSOR_DIMENSIONS)\n                tensor.data = np.random.randint(256, size=data_length).tolist()\n\n                tensor_list.tensors = [tensor, tensor]\n                timestamp = self.node.get_clock().now().to_msg()\n                tensor_list.header.stamp = timestamp\n\n                # Wait at most TIMEOUT seconds for subscriber to respond\n                TIMEOUT = 30\n                end_time = time.time() + TIMEOUT\n\n                done = False\n                while time.time() < end_time:\n                    tensor_list_pub.publish(tensor_list)\n                    rclpy.spin_once(self.node, timeout_sec=0.1)\n\n                    # If we have received a message on the output topic, break\n                    if 'pynitros2_output_msg_ros' in received_messages:\n                        done = True\n                        break\n\n                self.assertTrue(done, \"Didn't receive output on output tensor list topic!\")\n\n                received_tensor_list = received_messages['pynitros2_output_msg_ros']\n                self.assertEqual(len(tensor_list.tensors), len(received_tensor_list.tensors),\n                                 'Source and received tensor list size do not match')\n\n                for i in range(len(tensor_list.tensors)):\n                    source_tensor = tensor_list.tensors[i]\n                    received_tensor = received_tensor_list.tensors[i]\n                    self.assertEqual(source_tensor.name, received_tensor.name,\n                                     'Source and received tensor names do not match')\n                    self.assertEqual(source_tensor.name, received_tensor.name,\n                                     'Source and received tensor names do not match')\n                    self.assertEqual(source_tensor.data_type, received_tensor.data_type,\n                                     'Source and received tensor data types do not match')\n                    self.assertEqual(source_tensor.shape.rank, received_tensor.shape.rank,\n                                     'Source and received tensor ranks do not match')\n                    self.assertEqual(source_tensor.shape.dims, received_tensor.shape.dims,\n                                     'Source and received tensor dimensions do not match')\n                    self.assertEqual(len(source_tensor.data), len(received_tensor.data),\n                                     'Source and received tensor data do not match')\n                    self.assertEqual(str(timestamp), str(received_tensor_list.header.stamp),\n                                     'Timestamps do not match.')\n\n                    for j in range(len(source_tensor.data)):\n                        self.assertEqual(source_tensor.data[j], received_tensor.data[j],\n                                         'Source and received tensor pixels do not match')\n\n            finally:\n                self.node.destroy_subscription(received_tensor_list_sub)\n                self.node.destroy_publisher(tensor_list_pub)\n"
  },
  {
    "path": "isaac_ros_pynitros/test/test_cases/nitros_image/profile/image.json",
    "content": "{\n    \"image\": \"NVIDIAprofile.png\",\n    \"encoding\": \"bgr8\"\n}"
  },
  {
    "path": "isaac_ros_pynitros/test/test_cases/nitros_point_cloud/test_data/ketchup.pcd",
    "content": "# .PCD v0.7 - Point Cloud Data file format\nVERSION 0.7\nFIELDS x y z rgb\nSIZE 4 4 4 4\nTYPE F F F F\nCOUNT 1 1 1 1\nWIDTH 20\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0\nPOINTS 20\nDATA ascii\n-1.634617501 -4.290111561 -1.858700299 0.2619938229\n2.853279639 -7.209591076 -0.6885781819 0.4857394092\n-1.031189043 -0.7250457872 -1.98802603 0.5820572901\n-0.8246446857 -7.238877545 -1.052616484 0.4920581732\n-1.268758149 1.713223204 -1.907102878 0.4820194757\n0.7747915949 -7.244047803 -0.8747465489 0.5830105839\n0.1939418026 -7.285154885 -0.5047866621 0.5729205837\n-1.677812876 -2.032508282 -1.813671809 0.5839017563\n-0.5203375641 0.707792248 -2.066546191 0.5739105831\n0.2619938229 3.527571041 -1.784421252 0.2619938229\n3.149173217 -5.719117107 -0.5116390493 0.2619938229\n2.333993901 -7.365552464 -0.398136829 0.2619938229\n-0.4208003406 1.830185188 -2.065341214 0.2619938229\n2.852645176 -0.4909255341 -0.6731062388 0.2619938229\n-1.764735432 -3.413405289 -1.798219496 0.2619938229\n-2.064404219 -4.691953551 -1.700539264 0.2619938229\n-0.4341473384 2.614184337 -2.041596547 0.2619938229\n-1.367775279 -1.257706668 -1.90602497 0.2619938229\n2.51894068 0.5080515703 -1.072134684 0.2619938229\n3.100033603 -1.851751732 -0.3858886334 0.2619938229"
  },
  {
    "path": "isaac_ros_pynitros/test/test_copyright.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom ament_copyright.main import main\nimport pytest\n\n\n@pytest.mark.copyright\n@pytest.mark.linter\ndef test_copyright():\n    rc = main(argv=['.', 'test'])\n    assert rc == 0, 'Found errors'\n"
  },
  {
    "path": "isaac_ros_pynitros/test/test_flake8.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom ament_flake8.main import main_with_errors\nimport pytest\n\n\n@pytest.mark.flake8\n@pytest.mark.linter\ndef test_flake8():\n    rc, errors = main_with_errors(argv=[])\n    assert rc == 0, \\\n        'Found %d code style errors / warnings:\\n' % len(errors) + \\\n        '\\n'.join(errors)\n"
  },
  {
    "path": "isaac_ros_pynitros/test/test_pep257.py",
    "content": "# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES\n# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n#\n# SPDX-License-Identifier: Apache-2.0\n\nfrom ament_pep257.main import main\nimport pytest\n\n\n@pytest.mark.linter\n@pytest.mark.pep257\ndef test_pep257():\n    rc = main(argv=['.', 'test'])\n    assert rc == 0, 'Found code style errors / warnings'\n"
  }
]