Initial release.

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()

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<family>:<id>` (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>    # tag family name: 36h11 [only one family supported]
+            size: <tag edge size>   # tag edge size in meter (default: 2.0)
+
+            # (optional) list of tags
+            max_tags: <maximum tag count>    # 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

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.

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 <Eigen/Core>
+
+#include <apriltag_msgs/msg/april_tag_detection.hpp>
+#include <apriltag_msgs/msg/april_tag_detection_array.hpp>
+#include <image_transport/camera_subscriber.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+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<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
+    const rclcpp::Publisher<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr pub_detections_;
+
+    struct AprilTagsImpl;
+    std::unique_ptr<AprilTagsImpl> impl_;
+};

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])

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 <stdint.h>
+#include <stddef.h>
+#include <vector>
+#include <vector_types.h>
+
+// 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__

package.xml

@@ -0,0 +1,34 @@
+<!--
+# 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.
+-->
+
+<?xml version="1.0"?>
+<package format="2">
+  <name>nvapriltags_ros2</name>
+  <version>1.0.0</version>
+  <description>AprilTag Detection</description>
+  <maintainer email="[email protected]">nvidia</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>eigen</build_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2_msgs</depend>
+  <depend>apriltag_msgs</depend>  
+  <depend>image_transport</depend>
+  <depend>cv_bridge</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>