diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..e76c957 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,66 @@ +# Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +cmake_minimum_required(VERSION 3.5) + +project(nvapriltag_ros LANGUAGES CXX CUDA) + +set(CMAKE_CXX_STANDARD 14) +set(CUDA_MIN_VERSION "10.2") + +EXECUTE_PROCESS(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE) +message( STATUS "Architecture: ${ARCHITECTURE}" ) + +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(apriltag_msgs REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(cv_bridge REQUIRED) + +# Eigen +find_package(Eigen3 REQUIRED) +find_package(Threads REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIR}) + +# CUDA +find_package(CUDA ${CUDA_MIN_VERSION} REQUIRED) +include_directories(${CUDA_INCLUDE_DIRS}) + +include_directories(include) + +link_directories(${CUDA_TOOLKIT_ROOT_DIR}/lib64) + +# NVAprilTags +include_directories(nvapriltags/nvapriltags) +add_library(nvapriltags STATIC IMPORTED) +if( ${ARCHITECTURE} STREQUAL "x86_64" ) + set_property(TARGET nvapriltags PROPERTY IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/nvapriltags/lib_x86_64/libapril_tagging.a) +elseif( ${ARCHITECTURE} STREQUAL "aarch64" ) + set_property(TARGET nvapriltags PROPERTY IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/nvapriltags/lib_aarch64_jetpack44/libapril_tagging.a) +endif() + +# AprilTagNode +add_library(AprilTagNode SHARED src/AprilTagNode.cpp) +add_dependencies(AprilTagNode nvapriltags) +ament_target_dependencies(AprilTagNode rclcpp rclcpp_components sensor_msgs apriltag_msgs tf2_msgs image_transport cv_bridge) +target_link_libraries(AprilTagNode nvapriltags ${CUDA_LIBRARIES}) +rclcpp_components_register_nodes(AprilTagNode "AprilTagNode") + +ament_environment_hooks(${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}) + +install(TARGETS AprilTagNode + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..bea32dc --- /dev/null +++ b/LICENSE @@ -0,0 +1,130 @@ +1. Copyright & License Notices + + +1.1. NVIDIA CORPORATION +1.1.1. NVIDIA SOFTWARE LICENSE AGREEMENT +IMPORTANT – READ BEFORE DOWNLOADING, INSTALLING, COPYING OR USING THE LICENSED SOFTWARE + +This Software License Agreement ("SLA”), made and entered into as of the time and date of click through action (“Effective Date”), is a legal agreement between you and NVIDIA Corporation ("NVIDIA") and governs the use of the NVIDIA computer software and the documentation made available for use with such NVIDIA software. By downloading, installing, copying, or otherwise using the NVIDIA software and/or documentation, you agree to be bound by the terms of this SLA. If you do not agree to the terms of this SLA, do not download, install, copy or use the NVIDIA software or documentation. IF YOU ARE ENTERING INTO THIS SLA ON BEHALF OF A COMPANY OR OTHER LEGAL ENTITY, YOU REPRESENT THAT YOU HAVE THE LEGAL AUTHORITY TO BIND THE ENTITY TO THIS SLA, IN WHICH CASE “YOU” WILL MEAN THE ENTITY YOU REPRESENT. IF YOU DON’T HAVE SUCH AUTHORITY, OR IF YOU DON’T ACCEPT ALL THE TERMS AND CONDITIONS OF THIS SLA, THEN NVIDIA DOES NOT AGREE TO LICENSE THE LICENSED SOFTWARE TO YOU, AND YOU MAY NOT DOWNLOAD, INSTALL, COPY OR USE IT. + +1. LICENSE. + +1.1 License Grant. Subject to the terms of the AGREEMENT, NVIDIA hereby grants you a non-exclusive, non-transferable license, without the right to sublicense (except as expressly set forth in a Supplement), during the applicable license term unless earlier terminated as provided below, to have Authorized Users install and use the Software, including modifications (if expressly permitted in a Supplement), in accordance with the Documentation. You are only licensed to activate and use Licensed Software for which you a have a valid license, even if during the download or installation you are presented with other product options. No Orders are binding on NVIDIA until accepted by NVIDIA. Your Orders are subject to the AGREEMENT. + +SLA Supplements: Certain Licensed Software licensed under this SLA may be subject to additional terms and conditions that will be presented to you in a Supplement for acceptance prior to the delivery of such Licensed Software under this SLA and the applicable Supplement. Licensed Software will only be delivered to you upon your acceptance of all applicable terms. + +1.2 Limited Purpose Licenses. If your license is provided for one of the purposes indicated below, then notwithstanding contrary terms in Section 1.1 or in a Supplement, such licenses are for internal use and do not include any right or license to sub-license and distribute the Licensed Software or its output in any way in any public release, however limited, and/or in any manner that provides third parties with use of or access to the Licensed Software or its functionality or output, including (but not limited to) external alpha or beta testing or development phases. Further: + +(i) Evaluation License. You may use evaluation licenses solely for your internal evaluation of the Licensed Software for broader adoption within your Enterprise or in connection with a NVIDIA product purchase decision, and such licenses have an expiration date as indicated by NVIDIA in its sole discretion (or ninety days from the date of download if no other duration is indicated). + +(ii) Educational/Academic License. You may use educational/academic licenses solely for educational purposes and all users must be enrolled or employed by an academic institution. If you do not meet NVIDIA’s academic program requirements for educational institutions, you have no rights under this license. + +(iii) Test/Development License. You may use test/development licenses solely for your internal development, testing and/or debugging of your software applications or for interoperability testing with the Licensed Software, and such licenses have an expiration date as indicated by NVIDIA in its sole discretion (or one year from the date of download if no other duration is indicated). + +1.3 Pre-Release Licenses. With respect to alpha, beta, preview, and other pre-release Software and Documentation (“Pre-Release Licensed Software”) delivered to you under the AGREEMENT you acknowledge and agree that such Pre-Release Licensed Software (i) may not be fully functional, may contain errors or design flaws, and may have reduced or different security, privacy, accessibility, availability, and reliability standards relative to commercially provided NVIDIA software and documentation, and (ii) use of such Pre-Release Licensed Software may result in unexpected results, loss of data, project delays or other unpredictable damage or loss. THEREFORE, PRE-RELEASE LICENSED SOFTWARE IS NOT INTENDED FOR USE, AND SHOULD NOT BE USED, IN PRODUCTION OR BUSINESS-CRITICAL SYSTEMS. NVIDIA has no obligation to make available a commercial version of any Pre-Release Licensed Software and NVIDIA has the right to abandon development of Pre-Release Licensed Software at any time without liability. + +1.4 Enterprise and Contractor Usage. You may allow your Enterprise employees and Contractors to access and use the Licensed Software pursuant to the terms of the AGREEMENT solely to perform work on your behalf, provided further that with respect to Contractors: (i) you obtain a written agreement from each Contractor which contains terms and obligations with respect to access to and use of Licensed Software no less protective of NVIDIA than those set forth in the AGREEMENT, and (ii) such Contractor’s access and use expressly excludes any sublicensing or distribution rights for the Licensed Software. You are responsible for the compliance with the terms and conditions of the AGREEMENT by your Enterprise and Contractors. Any act or omission that, if committed by you, would constitute a breach of the AGREEMENT shall be deemed to constitute a breach of the AGREEMENT if committed by your Enterprise or Contractors. + +1.5 Services. Except as expressly indicated in an Order, NVIDIA is under no obligation to provide support for the Licensed Software or to provide any patches, maintenance, updates or upgrades under the AGREEMENT. Unless patches, maintenance, updates or upgrades are provided with their separate governing terms and conditions, they constitute Licensed Software licensed to you under the AGREEMENT. + +2. LIMITATIONS. + +2.1 License Restrictions. Except as expressly authorized in the AGREEMENT, you agree that you will not (nor authorize third parties to): (i) copy and use Software that was licensed to you for use in one or more NVIDIA hardware products in other unlicensed products (provided that copies solely for backup purposes are allowed); (ii) reverse engineer, decompile, disassemble (except to the extent applicable laws specifically require that such activities be permitted) or attempt to derive the source code, underlying ideas, algorithm or structure of Software provided to you in object code form; (iii) sell, transfer, assign, distribute, rent, loan, lease, sublicense or otherwise make available the Licensed Software or its functionality to third parties (a) as an application services provider or service bureau, (b) by operating hosted/virtual system environments, (c) by hosting, time sharing or providing any other type of services, or (d) otherwise by means of the internet; (iv) modify, translate or otherwise create any derivative works of any Licensed Software; (v) remove, alter, cover or obscure any proprietary notice that appears on or with the Licensed Software or any copies thereof; (vi) use the Licensed Software, or allow its use, transfer, transmission or export in violation of any applicable export control laws, rules or regulations; (vii) distribute, permit access to, or sublicense the Licensed Software as a stand-alone product; (viii) bypass, disable, circumvent or remove any form of copy protection, encryption, security or digital rights management or authentication mechanism used by NVIDIA in connection with the Licensed Software, or use the Licensed Software together with any authorization code, serial number, or other copy protection device not supplied by NVIDIA directly or through an authorized reseller; (ix) use the Licensed Software for the purpose of developing competing products or technologies or assisting a third party in such activities; (x) use the Licensed Software with any system or application where the use or failure of such system or application can reasonably be expected to threaten or result in personal injury, death, or catastrophic loss including, without limitation, use in connection with any nuclear, avionics, navigation, military, medical, life support or other life critical application (“Critical Applications”), unless the parties have entered into a Critical Applications agreement; (xi) distribute any modification or derivative work you make to the Licensed Software under or by reference to the same name as used by NVIDIA; or (xii) use the Licensed Software in any manner that would cause the Licensed Software to become subject to an Excluded License. Nothing in the AGREEMENT shall be construed to give you a right to use, or otherwise obtain access to, any source code from which the Software or any portion thereof is compiled or interpreted. You acknowledge that NVIDIA does not design, test, manufacture or certify the Licensed Software for use in the context of a Critical Application and NVIDIA shall not be liable to you or any third party, in whole or in part, for any claims or damages arising from such use. 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 you and your Enterprise, and their respective employees, contractors, agents, distributors, resellers, end users, officers and directors use of Licensed Software outside of the scope of the AGREEMENT or any other breach of the terms of the AGREEMENT. + +2.2 Third Party License Obligations. The Licensed Software may come bundled with, or otherwise include or be distributed with, third party software licensed by an NVIDIA supplier and/or open source software provided under an open source license (collectively, “Third Party Software”). Notwithstanding anything to the contrary herein, Third Party Software is licensed to you subject to the terms and conditions of the software license agreement accompanying such Third Party Software whether in the form of a discrete agreement, click-through license, or electronic license terms accepted at the time of installation and any additional terms or agreements provided by the third party licensor (“Third Party License Terms”). Use of the Third Party Software by you shall be governed by such Third Party License Terms, or if no Third Party License Terms apply, then the Third Party Software is provided to you as-is, without support or warranty or indemnity obligations, for use in or with the Licensed Software and not otherwise used separately. Copyright to Third Party Software is held by the copyright holders indicated in the Third Party License Terms. + +Audio/Video Encoders and Decoders. You acknowledge and agree that it is your sole responsibility to obtain any additional third party licenses required to make, have made, use, have used, sell, import, and offer for sale your products or services that include or incorporate any Third Party Software and content relating to audio and/or video encoders and decoders from, including but not limited to, Microsoft, Thomson, Fraunhofer IIS, Sisvel S.p.A., MPEG-LA, and Coding Technologies as NVIDIA does not grant to you under the AGREEMENT any necessary patent or other rights with respect to audio and/or video encoders and decoders. + +2.3 Limited Rights. Your rights in the Licensed Software are limited to those expressly granted under the AGREEMENT and no other licenses are granted whether by implication, estoppel or otherwise. NVIDIA reserves all rights, title and interest in and to the Licensed Software not expressly granted under the AGREEMENT. + +3. CONFIDENTIALITY. Neither party will use the other party’s Confidential Information, except as necessary for the performance of the AGREEMENT, nor will either party disclose such Confidential Information to any third party, except to personnel of NVIDIA and its Affiliates, you, your Enterprise, your Enterprise Contractors, and each party’s legal and financial advisors that have a need to know such Confidential Information for the performance of the AGREEMENT, provided that each such personnel, employee and Contractors are subject to a written agreement that includes confidentiality obligations consistent with those set forth herein. Each party will use all reasonable efforts to maintain the confidentiality of all of the other party’s Confidential Information in its possession or control, but in no event less than the efforts that it ordinarily uses with respect to its own Confidential Information of similar nature and importance. The foregoing obligations will not restrict either party from disclosing the other party’s Confidential Information or the terms and conditions of the AGREEMENT as required under applicable securities regulations or pursuant to the order or requirement of a court, administrative agency, or other governmental body, provided that the party required to make such disclosure (i) gives reasonable notice to the other party to enable it to contest such order or requirement prior to its disclosure (whether through protective orders or otherwise), (ii) uses reasonable effort to obtain confidential treatment or similar protection to the fullest extent possible to avoid such public disclosure, and (iii) discloses only the minimum amount of information necessary to comply with such requirements. + +NVIDIA Confidential Information under the AGREEMENT includes output from Licensed Software developer tools identified as “Pro” versions, where the output reveals functionality or performance data pertinent to NVIDIA hardware or software products. + +4. OWNERSHIP. You are not obligated to disclose to NVIDIA any modifications that you, your Enterprise or your Contractors make to the Licensed Software as permitted under the AGREEMENT. As between the parties, all modifications are owned by NVIDIA and licensed to you under the AGREEMENT unless otherwise expressly provided in a Supplement. The Licensed Software and all modifications owned by NVIDIA, and the respective Intellectual Property Rights therein, are and will remain the sole and exclusive property of NVIDIA or its licensors. You shall not engage in any act or omission that would impair NVIDIA’s and/or its licensors’ Intellectual Property Rights in the Licensed Software or any other materials, information, processes or subject matter proprietary to NVIDIA. NVIDIA’s licensors are intended third party beneficiaries with the right to enforce provisions of the AGREEMENT with respect to their Confidential Information and/or Intellectual Property Rights. + +5. FEEDBACK. You may, but you are not obligated, to provide Feedback to NVIDIA. You hereby grant NVIDIA and its Affiliates a perpetual, non-exclusive, worldwide, irrevocable license to use, reproduce, modify, license, sublicense (through multiple tiers of sublicensees), distribute (through multiple tiers of distributors) and otherwise commercialize any Feedback that you voluntarily provide without the payment of any royalties or fees to you. NVIDIA has no obligation to respond to Feedback or to incorporate Feedback into the Licensed Software. + +6. NO WARRANTIES. THE LICENSED SOFTWARE AND ANY CONFIDENTIAL INFORMATION AND/OR SERVICES ARE PROVIDED BY NVIDIA “AS IS” AND “WITH ALL FAULTS,” AND NVIDIA AND ITS AFFILIATES EXPRESSLY DISCLAIM ALL WARRANTIES OF ANY KIND OR NATURE, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, ANY WARRANTIES OF OPERABILITY, CONDITION, VALUE, ACCURACY OF DATA, OR QUALITY, AS WELL AS ANY WARRANTIES OF MERCHANTABILITY, SYSTEM INTEGRATION, WORKMANSHIP, SUITABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE, NON-INFRINGEMENT, OR THE ABSENCE OF ANY DEFECTS THEREIN, WHETHER LATENT OR PATENT. NO WARRANTY IS MADE ON THE BASIS OF TRADE USAGE, COURSE OF DEALING OR COURSE OF TRADE. WITHOUT LIMITING THE FOREGOING, NVIDIA AND ITS AFFILIATES DO NOT WARRANT THAT THE LICENSED SOFTWARE OR ANY CONFIDENTIAL INFORMATION AND/OR SERVICES PROVIDED UNDER THE AGREEMENT WILL MEET YOUR REQUIREMENTS OR THAT THE OPERATION THEREOF WILL BE UNINTERRUPTED OR ERROR-FREE, OR THAT ALL ERRORS WILL BE CORRECTED. + +7. LIMITATION OF LIABILITY. TO THE MAXIMUM EXTENT PERMITTED BY LAW, NVIDIA AND ITS AFFILIATES SHALL NOT BE LIABLE FOR ANY SPECIAL, INCIDENTAL, PUNITIVE OR CONSEQUENTIAL DAMAGES, OR ANY LOST PROFITS, LOSS OF USE, LOSS OF DATA OR LOSS OF GOODWILL, OR THE COSTS OF PROCURING SUBSTITUTE PRODUCTS, ARISING OUT OF OR IN CONNECTION WITH THE AGREEMENT OR THE USE OR PERFORMANCE OF THE LICENSED SOFTWARE AND ANY CONFIDENTIAL INFORMATION AND/OR SERVICES PROVIDED UNDER THE AGREEMENT, 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. IN NO EVENT WILL NVIDIA’S AND ITS AFFILIATES TOTAL CUMULATIVE LIABILITY UNDER OR ARISING OUT OF THE AGREEMENT EXCEED THE NET AMOUNTS RECEIVED BY NVIDIA OR ITS AFFILIATES FOR YOUR USE OF THE PARTICULAR LICENSED SOFTWARE DURING THE TWELVE (12) MONTHS BEFORE THE LIABILITY AROSE (or up to US$10.00 if you acquired the Licensed Software for no charge). THE NATURE OF THE LIABILITY, THE NUMBER OF CLAIMS OR SUITS OR THE NUMBER OF PARTIES WITHIN YOUR ENTERPRISE THAT ACCEPTED THE TERMS OF THE AGREEMENT SHALL NOT ENLARGE OR EXTEND THIS LIMIT. THE FOREGOING LIMITATIONS SHALL APPLY REGARDLESS OF WHETHER NVIDIA, ITS AFFILIATES OR ITS LICENSORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES AND REGARDLESS OF WHETHER ANY REMEDY FAILS ITS ESSENTIAL PURPOSE. YOU ACKNOWLEDGE THAT NVIDIA’S OBLIGATIONS UNDER THE AGREEMENT ARE FOR THE BENEFIT OF YOU ONLY. The disclaimers, exclusions and limitations of liability set forth in the AGREEMENT form an essential basis of the bargain between the parties, and, absent any such disclaimers, exclusions or limitations of liability, the provisions of the AGREEMENT, including, without limitation, the economic terms, would be substantially different. + +8. TERM AND TERMINATION. + +8.1 AGREEMENT, Licenses and Services. This SLA shall become effective upon the Effective Date, each Supplement upon their acceptance, and both this SLA and Supplements shall continue in effect until your last access or use of the Licensed Software and/or services hereunder, unless earlier terminated as provided in this “Term and Termination” section. Each Licensed Software license ends at the earlier of (a) the expiration of the applicable license term, or (b) termination of such license or the AGREEMENT. Each service ends at the earlier of (x) the expiration of the applicable service term, (y) termination of such service or the AGREEMENT, or (z) expiration or termination of the associated license and no credit or refund will be provided upon the expiration or termination of the associated license for any service fees paid. + +8.2 Termination and Effect of Expiration or Termination. NVIDIA may terminate the AGREEMENT in whole or in part: (i) if you breach any term of the AGREEMENT and fail to cure such breach within thirty (30) days following notice thereof from NVIDIA (or immediately if you violate NVIDIA’s Intellectual Property Rights); (ii) if you become the subject of a voluntary or involuntary petition in bankruptcy or any proceeding relating to insolvency, receivership, liquidation or composition for the benefit of creditors, if that petition or proceeding is not dismissed with prejudice within sixty (60) days after filing, or if you cease to do business; or (iii) if you commence or participate in any legal proceeding against NVIDIA, with respect to the Licensed Software that is the subject of the proceeding during the pendency of such legal proceeding. If you or your authorized NVIDIA reseller fail to pay license fees or service fees when due then NVIDIA may, in its sole discretion, suspend or terminate your license grants, services and any other rights provided under the AGREEMENT for the affected Licensed Software, in addition to any other remedies NVIDIA may have at law or equity. Upon any expiration or termination of the AGREEMENT, a license or a service provided hereunder, (a) any amounts owed to NVIDIA become immediately due and payable, (b) you must promptly discontinue use of the affected Licensed Software and/or service, and (c) you must promptly destroy or return to NVIDIA all copies of the affected Licensed Software and all portions thereof in your possession or control, and each party will promptly destroy or return to the other all of the other party’s Confidential Information within its possession or control. Upon written request, you will certify in writing that you have complied with your obligations under this section. Upon expiration or termination of the AGREEMENT all provisions survive except for the license grant provisions. + +9. CONSENT TO COLLECTION AND USE OF INFORMATION. + +You hereby agree and acknowledge that the Software may access and collect data about your Enterprise computer systems as well as configures the systems in order to (a) properly optimize such systems for use with the Software, (b) deliver content through the Software, (c) improve NVIDIA products and services, and (d) deliver marketing communications. Data collected by the Software includes, but is not limited to, system (i) hardware configuration and ID, (ii) operating system and driver configuration, (iii) installed applications, (iv) applications settings, performance, and usage data, and (iv) usage metrics of the Software. To the extent that you use the Software, you hereby consent to all of the foregoing, and represent and warrant that you have the right to grant such consent. In addition, you agree that you are solely responsible for maintaining appropriate data backups and system restore points for your Enterprise systems, and that NVIDIA will have no responsibility for any damage or loss to such systems (including loss of data or access) arising from or relating to (a) any changes to the configuration, application settings, environment variables, registry, drivers, BIOS, or other attributes of the systems (or any part of such systems) initiated through the Software; or (b) installation of any Software or third party software patches initiated through the Software. In certain systems you may change your system update preferences by unchecking "Automatically check for updates" in the "Preferences" tab of the control panel for the Software. + +In connection with the receipt of the Licensed Software or services you may receive access to links to third party websites and services and the availability of those links does not imply any endorsement by NVIDIA. NVIDIA encourages you to review the privacy statements on those sites and services that you choose to visit so that you can understand how they may collect, use and share personal information of individuals. NVIDIA is not responsible or liable for: (i) the availability or accuracy of such links; or (ii) the products, services or information available on or through such links; or (iii) the privacy statements or practices of sites and services controlled by other companies or organizations. + +To the extent that you or members of your Enterprise provide to NVIDIA during registration or otherwise personal data, you acknowledge that such information will be collected, used and disclosed by NVIDIA in accordance with NVIDIA's privacy policy, available at URL http://www.nvidia.com/object/privacy_policy.html. + +10. GENERAL. + +This SLA, any Supplements incorporated hereto, and Orders constitute the entire agreement of the parties with respect to the subject matter hereto and supersede all prior negotiations, conversations, or discussions between the parties relating to the subject matter hereto, oral or written, and all past dealings or industry custom. Any additional and/or conflicting terms and conditions on purchase order(s) or any other documents issued by you are null, void, and invalid. Any amendment or waiver under the AGREEMENT must be in writing and signed by representatives of both parties. + +The AGREEMENT and the rights and obligations thereunder may not be assigned by you, in whole or in part, including by merger, consolidation, dissolution, operation of law, or any other manner, without written consent of NVIDIA, and any purported assignment in violation of this provision shall be void and of no effect. NVIDIA may assign, delegate or transfer the AGREEMENT and its rights and obligations hereunder, and if to a non-Affiliate you will be notified. + +Each party acknowledges and agrees that the other is an independent contractor in the performance of the AGREEMENT, and each party is solely responsible for all of its employees, agents, contractors, and labor costs and expenses arising in connection therewith. The parties are not partners, joint ventures or otherwise affiliated, and neither has any authority to make any statements, representations or commitments of any kind to bind the other party without prior written consent. + +Neither party will be responsible for any failure or delay in its performance under the AGREEMENT (except for any payment obligations) to the extent due to causes beyond its reasonable control for so long as such force majeure event continues in effect. + +The AGREEMENT will be governed by and construed under the laws of the State of Delaware and the United States without regard to the conflicts of law provisions thereof and without regard to the United Nations Convention on Contracts for the International Sale of Goods. The parties consent to the personal jurisdiction of the federal and state courts located in Santa Clara County, California. You acknowledge and agree that a breach of any of your promises or agreements contained in the AGREEMENT may result in irreparable and continuing injury to NVIDIA for which monetary damages may not be an adequate remedy and therefore NVIDIA is entitled to seek injunctive relief as well as such other and further relief as may be appropriate. If any court of competent jurisdiction determines that any provision of the AGREEMENT is illegal, invalid or unenforceable, the remaining provisions will remain in full force and effect. Unless otherwise specified, remedies are cumulative. + +The Licensed Software has been developed entirely at private expense and is “commercial items” consisting of “commercial computer software” and “commercial computer software documentation” provided with RESTRICTED RIGHTS. Use, duplication or disclosure by the U.S. Government or a U.S. Government subcontractor is subject to the restrictions set forth in the AGREEMENT pursuant to DFARS 227.7202-3(a) or as set forth in subparagraphs (c)(1) and (2) of the Commercial Computer Software - Restricted Rights clause at FAR 52.227-19, as applicable. Contractor/manufacturer is NVIDIA, 2788 San Tomas Expressway, Santa Clara, CA 95051. + +You acknowledge that the Licensed Software described under the AGREEMENT is subject to export control under the U.S. Export Administration Regulations (EAR) and economic sanctions regulations administered by the U.S. Department of Treasury’s Office of Foreign Assets Control (OFAC). Therefore, you may not export, reexport or transfer in-country the Licensed Software without first obtaining any license or other approval that may be required by BIS and/or OFAC. You are responsible for any violation of the U.S. or other applicable export control or economic sanctions laws, regulations and requirements related to the Licensed Software. By accepting this SLA, you confirm that you are not a resident or citizen of any country currently embargoed by the U.S. and that you are not otherwise prohibited from receiving the Licensed Software. + +Any notice delivered by NVIDIA to you under the AGREEMENT will be delivered via mail, email or fax. 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. + +GLOSSARY OF TERMS + +Certain capitalized terms, if not otherwise defined elsewhere in this SLA, shall have the meanings set forth below: + +a. “Affiliate” means any legal entity that Owns, is Owned by, or is commonly Owned with a party. “Own” means having more than 50% ownership or the right to direct the management of the entity. + +b. “AGREEMENT” means this SLA and all associated Supplements entered by the parties referencing this SLA. + +c. “Authorized Users” means your Enterprise individual employees and any of your Enterprise’s Contractors, subject to the terms of the “Enterprise and Contractors Usage” section. + +d. “Confidential Information” means the Licensed Software (unless made publicly available by NVIDIA without confidentiality obligations), and any NVIDIA business, marketing, pricing, research and development, know-how, technical, scientific, financial status, proposed new products or other information disclosed by NVIDIA to you which, at the time of disclosure, is designated in writing as confidential or proprietary (or like written designation), or orally identified as confidential or proprietary or is otherwise reasonably identifiable by parties exercising reasonable business judgment, as confidential. Confidential Information does not and will not include information that: (i) is or becomes generally known to the public through no fault of or breach of the AGREEMENT by the receiving party; (ii) is rightfully known by the receiving party at the time of disclosure without an obligation of confidentiality; (iii) is independently developed by the receiving party without use of the disclosing party’s Confidential Information; or (iv) is rightfully obtained by the receiving party from a third party without restriction on use or disclosure. + +e. “Contractor” means an individual who works primarily for your Enterprise on a contractor basis from your secure network. + +f. “Documentation” means the NVIDIA documentation made available for use with the Software, including (without limitation) user manuals, datasheets, operations instructions, installation guides, release notes and other materials provided to you under the AGREEMENT. + +g. “Enterprise” means you or any company or legal entity for which you accepted the terms of this SLA, and their subsidiaries of which your company or legal entity owns more than fifty percent (50%) of the issued and outstanding equity. + +h. “Excluded License” includes, without limitation, a software license that requires as a condition of use, modification, and/or distribution that 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. + +i. “Feedback” means any and all suggestions, feature requests, comments or other feedback regarding the Licensed Software, including possible enhancements or modifications thereto. + +j. “Intellectual Property Rights” means all patent, copyright, trademark, trade secret, trade dress, trade names, utility models, mask work, moral rights, rights of attribution or integrity service marks, master recording and music publishing rights, performance rights, author’s rights, database rights, registered design rights and any applications for the protection or registration of these rights, or other intellectual or industrial property rights or proprietary rights, howsoever arising and in whatever media, whether now known or hereafter devised, whether or not registered, (including all claims and causes of action for infringement, misappropriation or violation and all rights in any registrations and renewals), worldwide and whether existing now or in the future. + +k. “Licensed Software” means Software, Documentation and all modifications owned by NVIDIA. + +l. “Order” means a purchase order issued by you, a signed purchase agreement with you, or other ordering document issued by you to NVIDIA or a NVIDIA authorized reseller (including any on-line acceptance process) that references and incorporates the AGREEMENT and is accepted by NVIDIA. + +m. “Software” means the NVIDIA software programs licensed to you under the AGREEMENT including, without limitation, libraries, sample code, utility programs and programming code. + +n. “Supplement” means the additional terms and conditions beyond those stated in this SLA that apply to certain Licensed Software licensed hereunder. + +Notices +Notice +THE INFORMATION IN THIS GUIDE AND ALL OTHER INFORMATION CONTAINED IN NVIDIA DOCUMENTATION REFERENCED IN THIS GUIDE IS PROVIDED “AS IS.” NVIDIA MAKES NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO THE INFORMATION FOR THE PRODUCT, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT, MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE. Notwithstanding any damages that customer might incur for any reason whatsoever, NVIDIA’s aggregate and cumulative liability towards customer for the product described in this guide shall be limited in accordance with the NVIDIA terms and conditions of sale for the product. + +THE NVIDIA PRODUCT DESCRIBED IN THIS GUIDE IS NOT FAULT TOLERANT AND IS NOT DESIGNED, MANUFACTURED OR INTENDED FOR USE IN CONNECTION WITH THE DESIGN, CONSTRUCTION, MAINTENANCE, AND/OR OPERATION OF ANY SYSTEM WHERE THE USE OR A FAILURE OF SUCH SYSTEM COULD RESULT IN A SITUATION THAT THREATENS THE SAFETY OF HUMAN LIFE OR SEVERE PHYSICAL HARM OR PROPERTY DAMAGE (INCLUDING, FOR EXAMPLE, USE IN CONNECTION WITH ANY NUCLEAR, AVIONICS, LIFE SUPPORT OR OTHER LIFE CRITICAL APPLICATION). NVIDIA EXPRESSLY DISCLAIMS ANY EXPRESS OR IMPLIED WARRANTY OF FITNESS FOR SUCH HIGH RISK USES. NVIDIA SHALL NOT BE LIABLE TO CUSTOMER OR ANY THIRD PARTY, IN WHOLE OR IN PART, FOR ANY CLAIMS OR DAMAGES ARISING FROM SUCH HIGH RISK USES. + +NVIDIA makes no representation or warranty that the product described in this guide will be suitable for any specified use without further testing or modification. Testing of all parameters of each product is not necessarily performed by NVIDIA. It is customer’s sole responsibility to ensure the product is suitable and fit for the application planned by customer and to do the necessary testing for the application in order to avoid a default of the application or the product. Weaknesses in customer’s product designs may affect the quality and reliability of the NVIDIA product and may result in additional or different conditions and/or requirements beyond those contained in this guide. NVIDIA does not accept any liability related to any default, damage, costs or problem which may be based on or attributable to: (i) the use of the NVIDIA product in any manner that is contrary to this guide, or (ii) customer product designs. + +Other than the right for customer to use the information in this guide with the product, no other license, either expressed or implied, is hereby granted by NVIDIA under this guide. Reproduction of information in this guide is permissible only if reproduction is approved by NVIDIA in writing, is reproduced without alteration, and is accompanied by all associated conditions, limitations, and notices. + +Trademarks +NVIDIA, the NVIDIA logo, and cuBLAS, CUDA, cuDNN, cuFFT, cuSPARSE, DIGITS, DGX, DGX-1, DGX Station, GRID, Jetson, Kepler, NGX, NVIDIA GPU Cloud, Maxwell, NCCL, NVLink, Pascal, Tegra, TensorRT, Tesla and Volta are trademarks and/or registered trademarks of NVIDIA Corporation in the Unites States and other countries. Other company and product names may be trademarks of the respective companies with which they are associated. + +Copyright +© 2021 NVIDIA Corporation. All rights reserved. \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..c0c71ea --- /dev/null +++ b/README.md @@ -0,0 +1,50 @@ +# NVAprilTags ROS2 Node + +This ROS2 node uses the NVIDIA GPU-accelerated AprilTags library to detect AprilTags in images and publish their poses, ids, and additional metadata. This has been tested on ROS2 (Foxy) and should run on x86_64 and aarch64 (Jetson hardware). It is modeled after and comparable to the ROS2 node for CPU AprilTags detection here: https://github.com/christianrauch/apriltag_ros.git + +For more information on the Isaac GEM this node is based off of, see the Isaac SDK 2020.2 documentation here: https://docs.nvidia.com/isaac/isaac/packages/fiducials/doc/apriltags.html + +For more information on AprilTags themselves, the paper and the reference CPU implementation: https://april.eecs.umich.edu/software/apriltag.html + +## Topics + +### Subscriptions: +The node subscribes via a `image_transport::CameraSubscriber` to `/apriltag/image`. The set of topic names depends on the type of image transport (parameter `image_transport`) selected (`raw` or `compressed`): +- `/apriltag/image` (`raw`, type: `sensor_msgs/Image`) +- `/apriltag/image/compressed` (`compressed`, type: `sensor_msgs/CompressedImage`) +- `/apriltag/camera_info` (type: `sensor_msgs/CameraInfo`) + +### Publisher: +- `/tf` (type: `tf2_msgs/TFMessage`) +- `/apriltag/detections` (type: `apriltag_msgs/AprilTagDetectionArray`) + +The camera intrinsics `K` in `CameraInfo` are used to compute the marker tag pose `T` from the homography `H`. The image and the camera intrinsics need to have the same timestamp. + +The tag poses are published on the standard TF topic `/tf` with the header set to the image header and `child_frame_id` set to either `tag:` (e.g. "tag36h11:0") or the frame name selected via configuration file. Additional information about detected tags is published as `AprilTagDetectionArray` message, which contains the original homography matrix, the `hamming` distance and the `decision_margin` of the detection. + +## Configuration + +The node is configured via a yaml configurations file. For the complete ROS yaml parameter file syntax, see: https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser. + +The file has the format: +```YAML +apriltag: # namespace + apriltag: # node name + ros__parameters: + # required + image_transport: raw # image format: "raw" or "compressed" (default: raw) + family: # tag family name: 36h11 [only one family supported] + size: # tag edge size in meter (default: 2.0) + + # (optional) list of tags + max_tags: # maximum number of tags to detect in a single frame (default: 20) +``` + +The parameters `family` and `size` are required. `family` (string) defines the tag family for the detector and can only be `36h11` at this time. `size` (float) is the tag edge size in meters, assuming square markers. + +The launch file can be used to start a component manager and load the composable node with configuration: +```bash +ros2 launch nvapriltags_ros2 launch/tag_36h11.launch.py +``` + +You will need to calibrate the intrinsics of your camera if you want the node to determine 3D poses for tags instead of just detection and corners as 2D pixel coordinates. See here: https://navigation.ros.org/tutorials/docs/camera_calibration.html diff --git a/giistr-cla.md b/giistr-cla.md new file mode 100644 index 0000000..77e33bb --- /dev/null +++ b/giistr-cla.md @@ -0,0 +1,58 @@ +## Individual Contributor License Agreement (CLA) + +**Thank you for submitting your contributions to this project.** + +By signing this CLA, you agree that the following terms apply to all of your past, present and future contributions +to the project. + +### License. + +You hereby represent that all present, past and future contributions are governed by the +[MIT License](https://opensource.org/licenses/MIT) +copyright statement. + +This entails that to the extent possible under law, you transfer all copyright and related or neighboring rights +of the code or documents you contribute to the project itself or its maintainers. +Furthermore you also represent that you have the authority to perform the above waiver +with respect to the entirety of you contributions. + +### Moral Rights. + +To the fullest extent permitted under applicable law, you hereby waive, and agree not to +assert, all of your “moral rights” in or relating to your contributions for the benefit of the project. + +### Third Party Content. + +If your Contribution includes or is based on any source code, object code, bug fixes, configuration changes, tools, +specifications, documentation, data, materials, feedback, information or other works of authorship that were not +authored by you (“Third Party Content”) or if you are aware of any third party intellectual property or proprietary +rights associated with your Contribution (“Third Party Rights”), +then you agree to include with the submission of your Contribution full details respecting such Third Party +Content and Third Party Rights, including, without limitation, identification of which aspects of your +Contribution contain Third Party Content or are associated with Third Party Rights, the owner/author of the +Third Party Content and Third Party Rights, where you obtained the Third Party Content, and any applicable +third party license terms or restrictions respecting the Third Party Content and Third Party Rights. For greater +certainty, the foregoing obligations respecting the identification of Third Party Content and Third Party Rights +do not apply to any portion of a Project that is incorporated into your Contribution to that same Project. + +### Representations. + +You represent that, other than the Third Party Content and Third Party Rights identified by +you in accordance with this Agreement, you are the sole author of your Contributions and are legally entitled +to grant the foregoing licenses and waivers in respect of your Contributions. If your Contributions were +created in the course of your employment with your past or present employer(s), you represent that such +employer(s) has authorized you to make your Contributions on behalf of such employer(s) or such employer +(s) has waived all of their right, title or interest in or to your Contributions. + +### Disclaimer. + +To the fullest extent permitted under applicable law, your Contributions are provided on an "as is" +basis, without any warranties or conditions, express or implied, including, without limitation, any implied +warranties or conditions of non-infringement, merchantability or fitness for a particular purpose. You are not +required to provide support for your Contributions, except to the extent you desire to provide support. + +### No Obligation. + +You acknowledge that the maintainers of this project are under no obligation to use or incorporate your contributions +into the project. The decision to use or incorporate your contributions into the project will be made at the +sole discretion of the maintainers or their authorized delegates. \ No newline at end of file diff --git a/include/AprilTagNode.hpp b/include/AprilTagNode.hpp new file mode 100644 index 0000000..d565c8e --- /dev/null +++ b/include/AprilTagNode.hpp @@ -0,0 +1,40 @@ +/* + * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. + * + * NVIDIA CORPORATION and its licensors retain all intellectual property + * and proprietary rights in and to this software, related documentation + * and any modifications thereto. Any use, reproduction, disclosure or + * distribution of this software and related documentation without an express + * license agreement from NVIDIA CORPORATION is strictly prohibited. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +class AprilTagNode : public rclcpp::Node { +public: + AprilTagNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions()); + +private: + void onCameraFrame(const sensor_msgs::msg::Image::ConstSharedPtr& msg_img, const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci); + + const std::string tag_family_; + const double tag_edge_size_; + const int max_tags_; + + const image_transport::CameraSubscriber sub_cam_; + const rclcpp::Publisher::SharedPtr pub_tf_; + const rclcpp::Publisher::SharedPtr pub_detections_; + + struct AprilTagsImpl; + std::unique_ptr impl_; +}; diff --git a/launch/tag_36h11.launch.py b/launch/tag_36h11.launch.py new file mode 100644 index 0000000..32e8e77 --- /dev/null +++ b/launch/tag_36h11.launch.py @@ -0,0 +1,36 @@ +# Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +# detect all 36h11 tags +cfg_36h11 = { + "image_transport": "raw", + "family": "36h11", + "size": 0.162, +} + +def generate_launch_description(): + composable_node = ComposableNode( + name='apriltag', + package='nvapriltag_ros', plugin='AprilTagNode', + remappings=[("/apriltag/image", "/camera/image"), + ("/apriltag/camera_info", "/camera/camera_info")], + parameters=[cfg_36h11]) + container = ComposableNodeContainer( + name='tag_container', + namespace='apriltag', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[composable_node], + output='screen' + ) + + return launch.LaunchDescription([container]) diff --git a/nvapriltags/lib_aarch64_jetpack44/libapril_tagging.a b/nvapriltags/lib_aarch64_jetpack44/libapril_tagging.a new file mode 100644 index 0000000..8556d8e Binary files /dev/null and b/nvapriltags/lib_aarch64_jetpack44/libapril_tagging.a differ diff --git a/nvapriltags/lib_x86_64/libapril_tagging.a b/nvapriltags/lib_x86_64/libapril_tagging.a new file mode 100644 index 0000000..8ef5e7c Binary files /dev/null and b/nvapriltags/lib_x86_64/libapril_tagging.a differ diff --git a/nvapriltags/nvapriltags/nvAprilTags.h b/nvapriltags/nvapriltags/nvAprilTags.h new file mode 100644 index 0000000..ecce9c3 --- /dev/null +++ b/nvapriltags/nvapriltags/nvAprilTags.h @@ -0,0 +1,106 @@ +/* + * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. + * + * NVIDIA CORPORATION and its licensors retain all intellectual property + * and proprietary rights in and to this software, related documentation + * and any modifications thereto. Any use, reproduction, disclosure or + * distribution of this software and related documentation without an express + * license agreement from NVIDIA CORPORATION is strictly prohibited. + */ + +#ifndef __APRILTAGS__ +#define __APRILTAGS__ + +#include +#include +#include +#include + +// Forward declaration for CUDA API +// CUstream and cudaStream_t are CUstream_st* +struct CUstream_st; + +// Decoded AprilTag +typedef struct nvAprilTagsID_st +{ + float2 corners[4]; + uint16_t id; + uint8_t hamming_error; + float orientation[9]; //!< Rotation transform, when expressed as a 3x3 matrix acting on a column vector, is column major. + float translation[3]; //!< Translation vector from the camera, in the same units as used for the tag_size. +}nvAprilTagsID_t; + +// Input data type for image buffer +typedef struct nvAprilTagsImageInput_st +{ + uchar4* dev_ptr; //!< Device pointer to the buffer + size_t pitch; //!< Pitch in bytes + uint16_t width; //!< Width in pixels + uint16_t height; //!< Buffer height +}nvAprilTagsImageInput_t; + +typedef struct nvAprilTagsCameraIntrinsics_st { + float fx, fy, cx, cy; +}nvAprilTagsCameraIntrinsics_t; + +typedef enum +{ + NVAT_TAG36H11, // Default, currently the only tag family supported + NVAT_ENUM_SIZE = 0x7fffffff // Force int32_t +} +nvAprilTagsFamily; + +//! AprilTags Detector instance handle. Used to reference the detector after creation +typedef struct nvAprilTagsHandle_st* nvAprilTagsHandle; + +#ifdef __cplusplus +extern "C" { +#endif + // FUNCTION NAME: nvCreateAprilTagsDetector + // + //! DESCRIPTION: Creates and initializes an AprilTags detector instance that detects and decodes April tags + //! + //! \param [out] hApriltags Pointer to the handle of newly created AprilTags detector + //! \param [in] img_width Width of images to be fed in to AprilTags detector + //! \param [in] img_height Height of images to be fed in to AprilTags detector + //! \param [in] tag_family Enum representing the Tag Family to be detected; default NVAT_TAG36H11. + //! \param [in] cam Camera intrinsic parameters, or NULL, if the orientation and translation are not desired. + //! \param [in] tag_dim The linear dimension of the square tag. The translation will be expressed in the same units. + //! + //! \retval :: 0 - Success, else - Failure + int nvCreateAprilTagsDetector(nvAprilTagsHandle* hApriltags, //!< TODO: We usually return the result in the last parameter, not first. + const uint32_t img_width, const uint32_t img_height, + const nvAprilTagsFamily tag_family, + const nvAprilTagsCameraIntrinsics_t *cam, + float tag_dim); + + // FUNCTION NAME: nvAprilTagsDetect + // + //! DESCRIPTION: Runs the algorithms to detect potential April tags in the image and decodes valid April tags + //! + //! \param [in] hApriltags AprilTags detector handle + //! \param [in] img_input Input buffer containing the undistorted image on which to detect/decode April tags + //! \param [out] tags_out C-array containing detected Tags, after detection and decoding + //! \param [out] num_tags Number of tags detected + //! \param [in] max_tags Maximum number of tags that can be returned, based on allocated size of tags_out array. + //! \param [in] input_stream CUDA stream on which the computation is to occur, or 0 to use the default stream. + //! + //! \retval :: 0 - Success, else - Failure + int nvAprilTagsDetect(nvAprilTagsHandle hApriltags, const nvAprilTagsImageInput_t *img_input, + nvAprilTagsID_t *tags_out, uint32_t *num_tags, const uint32_t max_tags, + CUstream_st* input_stream); + + // FUNCTION NAME: nvAprilTagsDestroy + // + //! DESCRIPTION: Destroys an instance of AprilTags detector + //! + //! \param [in] hApriltags AprilTags detector handle to be destroyed + //! + //! \retval :: 0 - Success, else - Failure + int nvAprilTagsDestroy(nvAprilTagsHandle hApriltags); + +#ifdef __cplusplus +} +#endif + +#endif //__APRILTAGS__ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..538463f --- /dev/null +++ b/package.xml @@ -0,0 +1,34 @@ + + + + + nvapriltags_ros2 + 1.0.0 + AprilTag Detection + nvidia + MIT + + ament_cmake + + eigen + + rclcpp + rclcpp_components + sensor_msgs + tf2_msgs + apriltag_msgs + image_transport + cv_bridge + + + ament_cmake + + diff --git a/src/AprilTagNode.cpp b/src/AprilTagNode.cpp new file mode 100644 index 0000000..eca80f8 --- /dev/null +++ b/src/AprilTagNode.cpp @@ -0,0 +1,208 @@ +/* + * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. + * + * NVIDIA CORPORATION and its licensors retain all intellectual property + * and proprietary rights in and to this software, related documentation + * and any modifications thereto. Any use, reproduction, disclosure or + * distribution of this software and related documentation without an express + * license agreement from NVIDIA CORPORATION is strictly prohibited. + */ + +#include + +#include +#include +#include + +#include "cuda.h" // NOLINT +#include "cuda_runtime.h" // NOLINT +#include "nvAprilTags.h" + +namespace { +geometry_msgs::msg::Transform ToTransformMsg(const nvAprilTagsID_t &detection) { + geometry_msgs::msg::Transform t; + t.translation.x = detection.translation[0]; + t.translation.y = detection.translation[1]; + t.translation.z = detection.translation[2]; + + // Rotation matrix from nvAprilTags is column major + const Eigen::Map> + orientation(detection.orientation); + const Eigen::Quaternion q(orientation); + + t.rotation.w = q.w(); + t.rotation.x = q.x(); + t.rotation.y = q.y(); + t.rotation.z = q.z(); + + return t; +} +} // namespace + +struct AprilTagNode::AprilTagsImpl { + // Handle used to interface with the stereo library. + nvAprilTagsHandle april_tags_handle = nullptr; + + // Camera intrinsics + nvAprilTagsCameraIntrinsics_t cam_intrinsics; + + // Output vector of detected Tags + std::vector tags; + + // CUDA stream + cudaStream_t main_stream = {}; + + // CUDA buffers to store the input image. + nvAprilTagsImageInput_t input_image; + + // CUDA memory buffer container for RGBA images. + char *input_image_buffer = nullptr; + + // Size of image buffer + size_t input_image_buffer_size = 0; + + void initialize(const AprilTagNode &node, const uint32_t width, + const uint32_t height, const size_t image_buffer_size, + const size_t pitch_bytes, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr &msg_ci) { + assert(april_tags_handle != nullptr), "Already initialized."; + + // Get camera intrinsics + const double *k = msg_ci->k.data(); + const float fx = static_cast(k[0]); + const float fy = static_cast(k[4]); + const float cx = static_cast(k[2]); + const float cy = static_cast(k[5]); + cam_intrinsics = {fx, fy, cx, cy}; + + // Create AprilTags detector instance and get handle + const int error = nvCreateAprilTagsDetector( + &april_tags_handle, width, height, nvAprilTagsFamily::NVAT_TAG36H11, + &cam_intrinsics, node.tag_edge_size_); + if (error != 0) { + throw std::runtime_error( + "Failed to create NV April Tags detector (error code " + + std::to_string(error) + ")"); + } + + // Create stream for detection + cudaStreamCreate(&main_stream); + + // Allocate the output vector to contain detected AprilTags. + tags.resize(node.max_tags_); + + // Setup input image CUDA buffer. + const cudaError_t cuda_error = + cudaMalloc(&input_image_buffer, image_buffer_size); + if (cuda_error != cudaSuccess) { + throw std::runtime_error("Could not allocate CUDA memory (error code " + + std::to_string(cuda_error) + ")"); + } + + // Setup input image. + input_image_buffer_size = image_buffer_size; + input_image.width = width; + input_image.height = height; + input_image.dev_ptr = reinterpret_cast(input_image_buffer); + input_image.pitch = pitch_bytes; + } + + ~AprilTagsImpl() { + if (april_tags_handle != nullptr) { + cudaStreamDestroy(main_stream); + nvAprilTagsDestroy(april_tags_handle); + cudaFree(input_image_buffer); + } + } +}; + +AprilTagNode::AprilTagNode(rclcpp::NodeOptions options) + : Node("apriltag", "apriltag", options.use_intra_process_comms(true)), + // parameter + tag_family_(declare_parameter("family", "36h11")), + tag_edge_size_(declare_parameter("size", 2.0)), + max_tags_(declare_parameter("max_tags", 20)), + // topics + sub_cam_(image_transport::create_camera_subscription( + this, "image", + std::bind(&AprilTagNode::onCameraFrame, this, std::placeholders::_1, + std::placeholders::_2), + declare_parameter("image_transport", "raw"), + rmw_qos_profile_sensor_data)), + pub_tf_( + create_publisher("/tf", rclcpp::QoS(100))), + pub_detections_( + create_publisher( + "detections", rclcpp::QoS(1))), + impl_(std::make_unique()) {} + +void AprilTagNode::onCameraFrame( + const sensor_msgs::msg::Image::ConstSharedPtr &msg_img, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr &msg_ci) { + // Convert frame to 8-bit RGBA image + const cv::Mat img_rgba8 = cv_bridge::toCvShare(msg_img, "rgba8")->image; + + // Setup detector on first frame + if (impl_->april_tags_handle == nullptr) { + impl_->initialize(*this, img_rgba8.cols, img_rgba8.rows, + img_rgba8.total() * img_rgba8.elemSize(), img_rgba8.step, + msg_ci); + } + + // Copy frame into CUDA buffer + const cudaError_t cuda_error = + cudaMemcpy(impl_->input_image_buffer, img_rgba8.ptr(), + impl_->input_image_buffer_size, cudaMemcpyHostToDevice); + if (cuda_error != cudaSuccess) { + throw std::runtime_error( + "Could not memcpy to device CUDA memory (error code " + + std::to_string(cuda_error) + ")"); + } + + // Perform detection + uint32_t num_detections; + const int error = nvAprilTagsDetect( + impl_->april_tags_handle, &(impl_->input_image), impl_->tags.data(), + &num_detections, max_tags_, impl_->main_stream); + if (error != 0) { + throw std::runtime_error("Failed to run AprilTags detector (error code " + + std::to_string(error) + ")"); + } + + // Parse detections into published protos + apriltag_msgs::msg::AprilTagDetectionArray msg_detections; + msg_detections.header = msg_img->header; + tf2_msgs::msg::TFMessage tfs; + for (int i = 0; i < num_detections; i++) { + const nvAprilTagsID_t &detection = impl_->tags[i]; + + // detection + apriltag_msgs::msg::AprilTagDetection msg_detection; + msg_detection.family = tag_family_; + msg_detection.id = detection.id; + msg_detection.hamming = detection.hamming_error; + + // corners + for (int corner_idx = 0; corner_idx < 4; corner_idx++) { + msg_detection.corners.data()[corner_idx].x = + detection.corners[corner_idx].x; + msg_detection.corners.data()[corner_idx].y = + detection.corners[corner_idx].y; + } + msg_detections.detections.push_back(msg_detection); + + // Timestamped Pose3 transform + geometry_msgs::msg::TransformStamped tf; + tf.header = msg_img->header; + tf.child_frame_id = + std::string(tag_family_) + ":" + std::to_string(detection.id); + tf.transform = ToTransformMsg(detection); + tfs.transforms.push_back(tf); + } + + pub_detections_->publish(msg_detections); + pub_tf_->publish(tfs); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(AprilTagNode)