diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..35e4b96 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,15 @@ +# Images +*.gif filter=lfs diff=lfs merge=lfs -text +*.jpg filter=lfs diff=lfs merge=lfs -text +*.png filter=lfs diff=lfs merge=lfs -text +*.psd filter=lfs diff=lfs merge=lfs -text + + +# Archives +*.gz filter=lfs diff=lfs merge=lfs -text +*.tar filter=lfs diff=lfs merge=lfs -text +*.zip filter=lfs diff=lfs merge=lfs -text + +# Documents +*.pdf filter=lfs diff=lfs merge=lfs -text + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b793570 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +# Ignore all pycache files +**/__pycache__/** diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 85cb32b..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,80 +0,0 @@ -# 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(nvapriltags_ros2 LANGUAGES C 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(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() - -# Msg -find_package(rosidl_default_generators REQUIRED) - -# AprilTagNode -add_library(AprilTagNode SHARED src/AprilTagNode.cpp) -add_dependencies(AprilTagNode nvapriltags) -ament_target_dependencies(AprilTagNode rclcpp rclcpp_components sensor_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}) - -set(msg_files - "msg/AprilTagDetection.msg" - "msg/AprilTagDetectionArray.msg" -) -rosidl_generate_interfaces(${PROJECT_NAME} - ${msg_files} - DEPENDENCIES std_msgs geometry_msgs -) - -rosidl_target_interfaces(AprilTagNode - ${PROJECT_NAME} "rosidl_typesupport_cpp") - -ament_package() diff --git a/README.md b/README.md index 60a033d..8507648 100644 --- a/README.md +++ b/README.md @@ -1,58 +1,96 @@ -# NVAprilTags ROS2 Node +# `isaac_ros_apriltag` -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 build and run on x86_64 and aarch64 (Jetson). It is modeled after and comparable to the ROS2 node for CPU AprilTags detection here: https://github.com/christianrauch/apriltag_ros.git +## Overview +This ROS2 node uses the NVIDIA GPU-accelerated AprilTags library to detect AprilTags in images and publishes their poses, IDs, and additional metadata. This has been tested on ROS2 (Foxy) and should build and run on x86_64 and aarch64 (Jetson). It is modeled after and comparable to the ROS2 node for [CPU AprilTags detection](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 the Isaac GEM that this node is based off of, see the latest Isaac SDK 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 +For more information on AprilTags themselves, including the paper and the reference CPU implementation, click [here](https://april.eecs.umich.edu/software/apriltag.html). -## Topics +## System Requirements +This Isaac ROS package is designed and tested to be compatible with ROS2 Foxy on Jetson hardware. +### Jetson +- AGX Xavier or Xavier NX +- JetPack 4.6 -### 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`) +### x86_64 +- CUDA 10.2+ supported discrete GPU +- VPI 1.1.11 +- Ubuntu 18.04+ -### Publisher: -- `/tf` (type: `tf2_msgs/TFMessage`) -- `/apriltag/detections` (type: `apriltag_msgs/AprilTagDetectionArray`) +### Docker +Precompiled ROS2 Foxy packages are not available for JetPack 4.6 (based on Ubuntu 18.04 Bionic). You can either manually compile ROS2 Foxy and required dependent packages from source or use the Isaac ROS development Docker image from [Isaac ROS Common](https://github.com/NVIDIA-AI-IOT/isaac_ros_common) based on images from [jetson-containers](https://github.com/dusty-nv/jetson-containers). -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. +Run the following script in `isaac_ros_common` to build the image and launch the container: -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. +`$ scripts/run_dev.sh ` -## Configuration +You can either provide an optional path to mirror in your host ROS workspace with Isaac ROS packages, which will be made available in the container as `/workspaces/isaac_ros-dev`, or you can setup a new workspace in the container. -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. +### Package Dependencies +- [isaac_ros_common](https://github.com/NVIDIA-AI-IOT/isaac_ros_common) +- [isaac_ros_image_pipeline](https://github.com/NVIDIA-AI-IOT/isaac_ros_image_pipeline) +- [image_common](https://github.com/ros-perception/image_common.git) +- [vision_cv](https://github.com/ros-perception/vision_opencv.git) +- [OpenCV 4.5+](https://opencv.org/) -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) -``` +**Note:** `isaac_ros_common' is used for running tests and/or creating a development container. It also contains VPI Debian packages that can be installed natively on a development machine without the container. -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. +## Quickstart +1. Create a ROS2 workspace if one is not already prepared: +`mkdir -p your_ws/src` +**Note:** The workspace can have any name; the quickstart assumes you name it `your_ws`. +2. Clone this package repository to `your_ws/src/isaac_ros_apriltag`. Check that you have [Git LFS](https://git-lfs.github.com/) installed before cloning to pull down all large files. +`cd your_ws/src && git clone https://github.com/NVIDIA-AI-IOT/isaac_ros_apriltag` +3. Build and source the workspace: +`cd your_ws && colcon build --symlink-install && source install/setup.bash` +4. (Optional) Run tests to verify complete and correct installation: +`colcon test` +5. Start `isaac_ros_apriltag` using the prebuilt executable: +`ros2 run isaac_ros_apriltag isaac_ros_apriltag` +6. In a separate terminal, spin up a **calibrated** camera publisher to `/image_rect` and `/camera_info` using any package (for example, `v4l2_camera`): +`ros2 run v4l2_camera v4l2_camera_node --ros-args -r /image_raw:=/image_rect` +7. Observe the AprilTag detection output `/tag_detections` on a separate terminal with the command: +`ros2 topic echo /tag_detections` -### Start -As any ROS2 package, check the repository out under `src/` in a ROS2 workspace and invoke `colcon build` appropriately to compile and prepare for use. +### Configuration +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) for more details. -The launch file can be used to start a component manager and load the composable node with configuration: -```bash -ros2 launch nvapriltags_ros2 tag_36h11.launch.py -``` -You need to run a camera node (e.g. from `v4l2_camera` package) to feed frames into the `nvapriltags_ros2` node for detection. For example, you can run the `v4l2_camera_node` and remap its output topics as follows: -```bash -ros2 run v4l2_camera v4l2_camera_node /camera_info:=/camera/camera_info /image_raw:=/camera/image -``` -Tools such as `rqt` or other components can then consume the AprilTag detection messages from the `nvapriltags_ros2` node. +### Replacing `apriltag_ros` with `isaac_ros_apriltag` +1. Add a dependency on `isaac_ros_apriltag` to `your_package/package.xml` and `your_package/CMakeLists.txt`. The original `apriltag_ros` dependency may be removed entirely. +2. Change the package and plugin names in any `*.launch.py` launch files to use `isaac_ros_apriltag` and `AprilTagNode`, respectively. -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 +## See Also +- `isaac_ros_image_pipeline`: Accelerated metapackage offering similar functionality to the standard CPU-based `image_pipeline` metapackage +- `isaac_ros_common`: Utilities for robust ROS2 testing, in conjunction with `launch_test` + +# Isaac ROS Apriltag Pipeline Tutorial +## Objective +This tutorial will help you quickly run and experiment with the full Isaac ROS Apriltag pipeline, from camera frames to tag detections. + +## Tutorial +1. Complete the Quickstart steps above. +2. Connect a compatible camera to your Jetson and set up the camera publisher stream. Your camera vendor may offer a specific ROS2-compatible camera driver package. Alternatively, many generic cameras are compatible with the `v4l2_camera` package. +**Important:** Ensure that the camera stream publishes `Image` and `CameraInfo` pairs to the topics `/image_raw` and `/camera_info`, respectively. +3. Ensure that your workspace has been built and sourced, if you have not done so already: +`cd your_ws && colcon build --symlink-install && source install/setup.bash` +4. Finally, launch the pre-composed pipeline launchfile: +`ros2 launch isaac_ros_apriltag isaac_ros_apriltag_pipeline.launch.py` + +Detections will show up at `/tag_detections`. + +**Note** For best performance on Jetson, ensure that power settings are configured appropriately ([Power Management for Jetson](https://docs.nvidia.com/jetson/l4t/index.html#page/Tegra%20Linux%20Driver%20Package%20Development%20Guide/power_management_jetson_xavier.html#wwpID0EUHA)). + +## Next Steps +Now that you have successfully launched the full Isaac ROS Apriltag pipeline, you can easily adapt the provided launchfile to integrate with your existing ROS2 environment. + +Alternatively, since the `AprilTagNode` is provided as a ROS2 Component, you can also compose the accelerated Apriltag processor directly into an existing executable. + +# Package Reference +## `isaac_ros_apriltag` +### Overview +The `isaac_ros_apriltag` package offers functionality for detecting poses from AprilTags in the frame. It largely replaces the `apriltag_ros` package, though an included dependency on the `ImageFormatConverterNode` plugin of the `isaac_ros_image_proc` package also functions as a way to replace the CPU-based image format conversion in `cv_bridge`. +### Available Components +| Component | Topics Subscribed | Topics Published | Parameters | +| -------------- | ------------------------------------------------------------------ | ---------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `AprilTagNode` | `camera/image_rect`, `camera/camera_info`: The input camera stream | `tag_detections`: The detection message array
`tf`: The tag poses | `family`: The tag family for the detector (this value can only be `36h11` at this time)
`size`: The tag edge size in meters, assuming square markers
`max_tags`: The maximum number of tags to be detected, which is 20 by default | diff --git a/include/AprilTagNode.hpp b/include/AprilTagNode.hpp deleted file mode 100644 index 0bb7a62..0000000 --- a/include/AprilTagNode.hpp +++ /dev/null @@ -1,40 +0,0 @@ -/* - * 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 "nvapriltags_ros2/msg/april_tag_detection.hpp" -#include "nvapriltags_ros2/msg/april_tag_detection_array.hpp" -#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/isaac_ros_apriltag/CMakeLists.txt b/isaac_ros_apriltag/CMakeLists.txt new file mode 100644 index 0000000..eb48343 --- /dev/null +++ b/isaac_ros_apriltag/CMakeLists.txt @@ -0,0 +1,86 @@ +# 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(isaac_ros_apriltag LANGUAGES C CXX CUDA) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CUDA_MIN_VERSION "10.2") + +execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE) +message( STATUS "Architecture: ${ARCHITECTURE}" ) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# VPI (workaround for inherited dependency from isaac_ros_image_proc) +find_package(vpi 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}) +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() + +# apriltag_node +ament_auto_add_library(apriltag_node SHARED src/apriltag_node.cpp) +target_compile_definitions(apriltag_node + PRIVATE "COMPOSITION_BUILDING_DLL" +) +target_link_libraries(apriltag_node nvapriltags ${CUDA_LIBRARIES}) +rclcpp_components_register_nodes(apriltag_node "isaac_ros::apriltag::AprilTagNode") +set(node_plugins "${node_plugins}isaac_ros::apriltag::AprilTagNode;$\n") + +# isaac_ros_apriltag executable +ament_auto_add_executable(${PROJECT_NAME} + src/apriltag_main.cpp +) +ament_target_dependencies(${PROJECT_NAME} isaac_ros_image_proc) +target_link_libraries(${PROJECT_NAME} apriltag_node vpi) # explicit link VPI to workaround inherited dependency + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # Ignore copyright notices since we use custom JetPack EULA + set(ament_cmake_copyright_FOUND TRUE) + + ament_lint_auto_find_test_dependencies() + + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test(test/isaac_ros_apriltag_pipeline_test.py) + +endif() + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/isaac_ros_apriltag/include/isaac_ros_apriltag/apriltag_node.hpp b/isaac_ros_apriltag/include/isaac_ros_apriltag/apriltag_node.hpp new file mode 100644 index 0000000..c4cc98d --- /dev/null +++ b/isaac_ros_apriltag/include/isaac_ros_apriltag/apriltag_node.hpp @@ -0,0 +1,65 @@ +/** + * 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 ISAAC_ROS_APRILTAG__APRILTAG_NODE_HPP_ +#define ISAAC_ROS_APRILTAG__APRILTAG_NODE_HPP_ + +#include +#include + +#include "eigen3/Eigen/Core" +#include "image_transport/camera_subscriber.hpp" +#include "isaac_ros_apriltag_interfaces/msg/april_tag_detection.hpp" +#include "isaac_ros_apriltag_interfaces/msg/april_tag_detection_array.hpp" +#include "message_filters/subscriber.h" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + + +namespace isaac_ros +{ +namespace apriltag +{ + +class AprilTagNode : public rclcpp::Node +{ +public: + explicit AprilTagNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions()); + + ~AprilTagNode(); + +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 int queue_size_; + const int32_t max_interval_duration_; // Unit in seconds + + message_filters::Subscriber image_sub_; + message_filters::Subscriber camera_info_sub_; + + const rclcpp::Publisher::SharedPtr pub_tf_; + const rclcpp::Publisher::SharedPtr + pub_detections_; + + struct AprilTagsImpl; + std::unique_ptr impl_; +}; + +} // namespace apriltag +} // namespace isaac_ros + +#endif // ISAAC_ROS_APRILTAG__APRILTAG_NODE_HPP_ diff --git a/isaac_ros_apriltag/launch/isaac_ros_apriltag_pipeline.launch.py b/isaac_ros_apriltag/launch/isaac_ros_apriltag_pipeline.launch.py new file mode 100644 index 0000000..9790f40 --- /dev/null +++ b/isaac_ros_apriltag/launch/isaac_ros_apriltag_pipeline.launch.py @@ -0,0 +1,50 @@ +# 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, Node +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + rectify_node = ComposableNode( + package='isaac_ros_image_proc', + plugin='isaac_ros::image_proc::RectifyNode', + name='rectify_node', + ) + + rectify_container = ComposableNodeContainer( + name='rectify_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[rectify_node], + output='screen' + ) + + apriltag_exe = Node( + package='isaac_ros_apriltag', + executable='isaac_ros_apriltag', + name='apriltag_exe', + ) + + realsense_camera_node = Node( + package='realsense2_camera', + node_executable='realsense2_camera_node', + parameters=[{ + 'color_height': 1080, + 'color_width': 1920, + 'enable_infra1': False, + 'enable_infra2': False, + 'enable_depth': False, + }], + remappings=[('/color/image_raw', '/image'), + ('/color/camera_info', '/camera_info')] + ) + + return launch.LaunchDescription([rectify_container, apriltag_exe, realsense_camera_node]) diff --git a/launch/tag_36h11.launch.py b/isaac_ros_apriltag/launch/tag_36h11.launch.py similarity index 79% rename from launch/tag_36h11.launch.py rename to isaac_ros_apriltag/launch/tag_36h11.launch.py index 717f584..c86fc01 100644 --- a/launch/tag_36h11.launch.py +++ b/isaac_ros_apriltag/launch/tag_36h11.launch.py @@ -12,17 +12,18 @@ # detect all 36h11 tags cfg_36h11 = { - "image_transport": "raw", - "family": "36h11", - "size": 0.162 + 'image_transport': 'raw', + 'family': '36h11', + 'size': 0.162 } + def generate_launch_description(): composable_node = ComposableNode( name='apriltag', - package='nvapriltags_ros2', plugin='AprilTagNode', - remappings=[("/apriltag/image", "/camera/image"), - ("/apriltag/camera_info", "/camera/camera_info")], + package='isaac_ros_apriltag', plugin='AprilTagNode', + remappings=[('/apriltag/image', '/camera/image'), + ('/apriltag/camera_info', '/camera/camera_info')], parameters=[cfg_36h11]) container = ComposableNodeContainer( name='tag_container', diff --git a/nvapriltags/lib_aarch64_jetpack44/libapril_tagging.a b/isaac_ros_apriltag/nvapriltags/lib_aarch64_jetpack44/libapril_tagging.a similarity index 100% rename from nvapriltags/lib_aarch64_jetpack44/libapril_tagging.a rename to isaac_ros_apriltag/nvapriltags/lib_aarch64_jetpack44/libapril_tagging.a diff --git a/nvapriltags/lib_x86_64/libapril_tagging.a b/isaac_ros_apriltag/nvapriltags/lib_x86_64/libapril_tagging.a similarity index 100% rename from nvapriltags/lib_x86_64/libapril_tagging.a rename to isaac_ros_apriltag/nvapriltags/lib_x86_64/libapril_tagging.a diff --git a/isaac_ros_apriltag/nvapriltags/nvapriltags/nvAprilTags.h b/isaac_ros_apriltag/nvapriltags/nvapriltags/nvAprilTags.h new file mode 100644 index 0000000..b0834d1 --- /dev/null +++ b/isaac_ros_apriltag/nvapriltags/nvapriltags/nvAprilTags.h @@ -0,0 +1,113 @@ +/** + * 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 ISAAC_ROS_APRILTAG__NVAPRILTAGS__NVAPRILTAGS__NVAPRILTAGS_H_ +#define ISAAC_ROS_APRILTAG__NVAPRILTAGS__NVAPRILTAGS__NVAPRILTAGS_H_ + +#include +#include + +#include + +// Forward declaration for CUDA API +// CUstream and cudaStream_t are CUstream_st* +struct CUstream_st; + +// Decoded AprilTag +typedef struct +{ + float2 corners[4]; + uint16_t id; + uint8_t hamming_error; + + // Rotation transform, when expressed as a 3x3 matrix acting on a column vector, is column major. + float orientation[9]; + + // Translation vector from the camera, in the same units as used for the tag_size. + float translation[3]; +} nvAprilTagsID_t; + +// Input data type for image buffer +typedef struct +{ + 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 + +/** + * @brief Creates and initializes an AprilTags detector instance that detects and decodes April tags + * + * @param hApriltags Pointer to the handle of newly created AprilTags detector + * @param img_width Width of images to be fed in to AprilTags detector + * @param img_height Height of images to be fed in to AprilTags detector + * @param tag_family Enum representing the Tag Family to be detected; default NVAT_TAG36H11 + * @param cam Camera intrinsic parameters, or NULL, if the orientation and translation are not desired + * @param tag_dim The linear dimension of the square tag. The translation will be expressed in the same units. + * @return int 0 - Success, else - Failure + */ +int nvCreateAprilTagsDetector( + // TODO(hemals): We usually return the result in the last parameter, not first. + nvAprilTagsHandle * hApriltags, + const uint32_t img_width, const uint32_t img_height, + const nvAprilTagsFamily tag_family, + const nvAprilTagsCameraIntrinsics_t * cam, + float tag_dim +); + +/** + * @brief Runs the algorithms to detect potential April tags in the image and decodes valid April tags + * + * @param hApriltags AprilTags detector handle + * @param img_input Input buffer containing the undistorted image on which to detect/decode April tags + * @param tags_out C-array containing detected Tags, after detection and decoding + * @param num_tags Number of tags detected + * @param max_tags Maximum number of tags that can be returned, based on allocated size of tags_out array + * @param input_stream CUDA stream on which the computation is to occur, or 0 to use the default stream + * @return int 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); + +/** + * @brief Destroys an instance of AprilTags detector + * + * @param hApriltags AprilTags detector handle to be destroyed + * @return int 0 - Success, else - Failure + */ +int nvAprilTagsDestroy(nvAprilTagsHandle hApriltags); + +#ifdef __cplusplus +} +#endif + +#endif // ISAAC_ROS_APRILTAG__NVAPRILTAGS__NVAPRILTAGS__NVAPRILTAGS_H_ diff --git a/isaac_ros_apriltag/package.xml b/isaac_ros_apriltag/package.xml new file mode 100644 index 0000000..02c667b --- /dev/null +++ b/isaac_ros_apriltag/package.xml @@ -0,0 +1,43 @@ + + + + + + isaac_ros_apriltag + 0.9.0 + AprilTag detection + + Hemal Shah + JetPack EULA + https://developer.nvidia.com/blog/accelerating-ai-modules-for-ros-and-ros-2-on-jetson/ + Arjun Bhorkar + Hemal Shah + + ament_cmake_auto + + rclcpp + rclcpp_components + sensor_msgs + tf2_msgs + image_transport + cv_bridge + eigen + isaac_ros_image_proc + isaac_ros_apriltag_interfaces + + ament_lint_auto + ament_lint_common + isaac_ros_test + + + ament_cmake + + diff --git a/isaac_ros_apriltag/src/apriltag_main.cpp b/isaac_ros_apriltag/src/apriltag_main.cpp new file mode 100644 index 0000000..d1385d1 --- /dev/null +++ b/isaac_ros_apriltag/src/apriltag_main.cpp @@ -0,0 +1,54 @@ +/** + * 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 "isaac_ros_apriltag/apriltag_node.hpp" +#include "isaac_ros_image_proc/image_format_converter_node.hpp" +#include "rclcpp/rclcpp.hpp" + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::executors::MultiThreadedExecutor exec; + + rclcpp::NodeOptions format_options; + format_options.arguments( + { + "--ros-args", + "-p", "encoding_desired:=rgba8", + "-p", "backends:=CUDA", + "-r", "image_raw:=image_rect", // Input topic: image_rect + "-r", "image:=apriltag/image_rect" // Output topic: apriltag/image_rect + }); + auto format_node = + std::make_shared(format_options); + exec.add_node(format_node); + + rclcpp::NodeOptions apriltag_options; + apriltag_options.arguments( + { + "--ros-args", + "-p", "image_transport:=raw", + "-p", "family:=36h11", + "-p", "size:=0.162", + "-r", "camera/image_rect:=apriltag/image_rect", // Input topic: apriltag/image_rect + "-r", "camera/camera_info:=camera_info", // Input topic: camera_info + }); + auto apriltag_node = std::make_shared(apriltag_options); + exec.add_node(apriltag_node); + + exec.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/AprilTagNode.cpp b/isaac_ros_apriltag/src/apriltag_node.cpp similarity index 50% rename from src/AprilTagNode.cpp rename to isaac_ros_apriltag/src/apriltag_node.cpp index 5fff680..25c2d91 100644 --- a/src/AprilTagNode.cpp +++ b/isaac_ros_apriltag/src/apriltag_node.cpp @@ -1,4 +1,4 @@ -/* +/** * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. * * NVIDIA CORPORATION and its licensors retain all intellectual property @@ -8,18 +8,27 @@ * license agreement from NVIDIA CORPORATION is strictly prohibited. */ -#include +#include "isaac_ros_apriltag/apriltag_node.hpp" -#include -#include -#include +#include +#include +#include -#include "cuda.h" // NOLINT -#include "cuda_runtime.h" // NOLINT +#include "cuda.h" // NOLINT - include .h without directory +#include "cuda_runtime.h" // NOLINT - include .h without directory +#include "cv_bridge/cv_bridge.h" +#include "eigen3/Eigen/Dense" +#include "image_transport/image_transport.hpp" +#include "message_filters/subscriber.h" +#include "message_filters/synchronizer.h" +#include "message_filters/sync_policies/approximate_time.h" #include "nvAprilTags.h" +#include "rclcpp/logger.hpp" -namespace { -geometry_msgs::msg::Transform ToTransformMsg(const nvAprilTagsID_t &detection) { +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]; @@ -27,7 +36,7 @@ geometry_msgs::msg::Transform ToTransformMsg(const nvAprilTagsID_t &detection) { // Rotation matrix from nvAprilTags is column major const Eigen::Map> - orientation(detection.orientation); + orientation(detection.orientation); const Eigen::Quaternion q(orientation); t.rotation.w = q.w(); @@ -37,9 +46,16 @@ geometry_msgs::msg::Transform ToTransformMsg(const nvAprilTagsID_t &detection) { return t; } + } // namespace -struct AprilTagNode::AprilTagsImpl { +namespace isaac_ros +{ +namespace apriltag +{ + +struct AprilTagNode::AprilTagsImpl +{ // Handle used to interface with the stereo library. nvAprilTagsHandle april_tags_handle = nullptr; @@ -56,19 +72,26 @@ struct AprilTagNode::AprilTagsImpl { nvAprilTagsImageInput_t input_image; // CUDA memory buffer container for RGBA images. - char *input_image_buffer = nullptr; + 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) { + typedef message_filters::sync_policies::ApproximateTime ApproximatePolicy; + typedef message_filters::Synchronizer Synchronizer; + std::shared_ptr sync; + + 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 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]); @@ -77,12 +100,12 @@ struct AprilTagNode::AprilTagsImpl { // 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_); + &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) + ")"); + "Failed to create NV April Tags detector (error code " + + std::to_string(error) + ")"); } // Create stream for detection @@ -93,10 +116,11 @@ struct AprilTagNode::AprilTagsImpl { // Setup input image CUDA buffer. const cudaError_t cuda_error = - cudaMalloc(&input_image_buffer, image_buffer_size); + 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) + ")"); + throw std::runtime_error( + "Could not allocate CUDA memory (error code " + + std::to_string(cuda_error) + ")"); } // Setup input image. @@ -107,7 +131,8 @@ struct AprilTagNode::AprilTagsImpl { input_image.pitch = pitch_bytes; } - ~AprilTagsImpl() { + ~AprilTagsImpl() + { if (april_tags_handle != nullptr) { cudaStreamDestroy(main_stream); nvAprilTagsDestroy(april_tags_handle); @@ -116,97 +141,116 @@ struct AprilTagNode::AprilTagsImpl { } }; + 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()) {} +: Node("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)), + queue_size_(declare_parameter("queue_size", 100)), + // Setting max interval to 100s to account for sync issues. + max_interval_duration_(declare_parameter("max_interval_duration", 100)), + // topics + image_sub_(message_filters::Subscriber( + this, + "camera/image_rect")), + camera_info_sub_(message_filters::Subscriber( + this, + "camera/camera_info")), + pub_tf_( + create_publisher("tf", rclcpp::QoS(100))), + pub_detections_( + create_publisher( + "tag_detections", rclcpp::QoS(1))), + impl_(std::make_unique()) +{ + impl_->sync.reset( + new AprilTagsImpl::Synchronizer( + AprilTagsImpl::ApproximatePolicy(queue_size_), + image_sub_, camera_info_sub_)); + impl_->sync->setMaxIntervalDuration(rclcpp::Duration(max_interval_duration_, 0)); + impl_->sync->registerCallback( + std::bind( + &AprilTagNode::onCameraFrame, this, std::placeholders::_1, + std::placeholders::_2)); +} void AprilTagNode::onCameraFrame( - const sensor_msgs::msg::Image::ConstSharedPtr &msg_img, - const sensor_msgs::msg::CameraInfo::ConstSharedPtr &msg_ci) { + 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); + 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); + 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) + ")"); + RCLCPP_ERROR( + this->get_logger(), + "Could not memcpy to device CUDA memory (error code %s)", std::to_string(cuda_error)); + return; } // 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); + 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) + ")"); + RCLCPP_ERROR(this->get_logger(), "Failed to run AprilTags detector (error code %d)", error); + return; } // Parse detections into published protos - nvapriltags_ros2::msg::AprilTagDetectionArray msg_detections; + isaac_ros_apriltag_interfaces::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]; + for (uint32_t i = 0; i < num_detections; i++) { + const nvAprilTagsID_t & detection = impl_->tags[i]; // detection - nvapriltags_ros2::msg::AprilTagDetection msg_detection; + isaac_ros_apriltag_interfaces::msg::AprilTagDetection msg_detection; msg_detection.family = tag_family_; msg_detection.id = detection.id; // corners for (int corner_idx = 0; corner_idx < 4; corner_idx++) { msg_detection.corners.data()[corner_idx].x = - detection.corners[corner_idx].x; + detection.corners[corner_idx].x; msg_detection.corners.data()[corner_idx].y = - detection.corners[corner_idx].y; + detection.corners[corner_idx].y; } // center const float slope_1 = (detection.corners[2].y - detection.corners[0].y) / - (detection.corners[2].x - detection.corners[0].x); + (detection.corners[2].x - detection.corners[0].x); const float slope_2 = (detection.corners[3].y - detection.corners[1].y) / - (detection.corners[3].x - detection.corners[1].x); + (detection.corners[3].x - detection.corners[1].x); const float intercept_1 = detection.corners[0].y - - (slope_1 * detection.corners[0].x); + (slope_1 * detection.corners[0].x); const float intercept_2 = detection.corners[3].y - - (slope_2 * detection.corners[3].x); + (slope_2 * detection.corners[3].x); msg_detection.center.x = (intercept_2 - intercept_1) / (slope_1 - slope_2); msg_detection.center.y = (slope_2 * intercept_1 - slope_1 * intercept_2) / - (slope_2 - slope_1); + (slope_2 - slope_1); // 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); + std::string(tag_family_) + ":" + std::to_string(detection.id); tf.transform = ToTransformMsg(detection); tfs.transforms.push_back(tf); @@ -222,5 +266,11 @@ void AprilTagNode::onCameraFrame( pub_tf_->publish(tfs); } -#include -RCLCPP_COMPONENTS_REGISTER_NODE(AprilTagNode) +AprilTagNode::~AprilTagNode() = default; + +} // namespace apriltag +} // namespace isaac_ros + +// Register as a component +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(isaac_ros::apriltag::AprilTagNode) diff --git a/isaac_ros_apriltag/test/isaac_ros_apriltag_pipeline_test.py b/isaac_ros_apriltag/test/isaac_ros_apriltag_pipeline_test.py new file mode 100644 index 0000000..eef31ed --- /dev/null +++ b/isaac_ros_apriltag/test/isaac_ros_apriltag_pipeline_test.py @@ -0,0 +1,108 @@ +# 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 os +import pathlib +import time + +from isaac_ros_apriltag_interfaces.msg import AprilTagDetectionArray +from isaac_ros_test import IsaacROSBaseTest, JSONConversion +import launch_ros +import pytest +import rclpy +from sensor_msgs.msg import CameraInfo, Image + + +@pytest.mark.rostest +def generate_test_description(): + """Generate launch description with all ROS2 nodes for testing.""" + rectify_node = launch_ros.descriptions.ComposableNode( + package='isaac_ros_image_proc', + plugin='isaac_ros::image_proc::RectifyNode', + name='rectify_node', + namespace=IsaacROSAprilTagPipelineTest.generate_namespace(), + ) + + rectify_container = launch_ros.actions.ComposableNodeContainer( + name='rectify_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[rectify_node], + output='screen' + ) + + apriltag_exe = launch_ros.actions.Node( + package='isaac_ros_apriltag', + executable='isaac_ros_apriltag', + name='apriltag_exe', + namespace=IsaacROSAprilTagPipelineTest.generate_namespace(), + ) + + return IsaacROSAprilTagPipelineTest.generate_test_description([ + rectify_container, + apriltag_exe + ]) + + +class IsaacROSAprilTagPipelineTest(IsaacROSBaseTest): + """Test for Isaac ROS Apriltag Pipeline.""" + + filepath = pathlib.Path(os.path.dirname(__file__)) + + @IsaacROSBaseTest.for_each_test_case() + def test_apriltag_pipeline(self, test_folder) -> None: + """Expect the pipeline to produce apriltag detections from images.""" + self.generate_namespace_lookup(['image', 'camera_info', 'tag_detections']) + + image_pub = self.node.create_publisher( + Image, self.namespaces['image'], self.DEFAULT_QOS) + camera_info_pub = self.node.create_publisher( + CameraInfo, self.namespaces['camera_info'], self.DEFAULT_QOS) + + received_messages = {} + tag_detections_sub, = self.create_logging_subscribers( + [('tag_detections', AprilTagDetectionArray)], received_messages) + + try: + image = JSONConversion.load_image_from_json(test_folder / 'image.json') + camera_info = JSONConversion.load_camera_info_from_json( + test_folder / 'camera_info.json') + + # Publish test case over both topics + image_pub.publish(image) + camera_info_pub.publish(camera_info) + + # Wait at most TIMEOUT seconds for subscriber to respond + TIMEOUT = 20 + end_time = time.time() + TIMEOUT + + done = False + while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=TIMEOUT) + + # If we have received exactly one message on the output topic, break + if 'tag_detections' in received_messages: + done = True + break + + self.assertTrue(done, "Didn't receive output on tag_detections topic!") + + # Collect received detections + tag_detections_actual = received_messages['tag_detections'] + + # Make sure that at least one detection was found + self.assertGreaterEqual(len(tag_detections_actual.detections), 1, + "Didn't find at least 1 detection in image!") + + # TODO(jaiveers): Store and check precise tag location? + + finally: + self.assertTrue(self.node.destroy_subscription(tag_detections_sub)) + self.assertTrue(self.node.destroy_publisher(image_pub)) + self.assertTrue(self.node.destroy_publisher(camera_info_pub)) diff --git a/isaac_ros_apriltag/test/test_cases/apriltag0/camera_info.json b/isaac_ros_apriltag/test/test_cases/apriltag0/camera_info.json new file mode 100644 index 0000000..6c6584e --- /dev/null +++ b/isaac_ros_apriltag/test/test_cases/apriltag0/camera_info.json @@ -0,0 +1,51 @@ +{ + "header": { + "frame_id": "tf_camera" + }, + "width": 1280, + "height": 800, + "distortion_model": "plumb_bob", + "D": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "K": [ + 434.943999, + 0.000000, + 651.073921, + 0.0, + 431.741273, + 441.878037, + 0.0, + 0.0, + 1.0 + ], + "R": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + "P": [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ] +} \ No newline at end of file diff --git a/isaac_ros_apriltag/test/test_cases/apriltag0/image.json b/isaac_ros_apriltag/test/test_cases/apriltag0/image.json new file mode 100644 index 0000000..3019a41 --- /dev/null +++ b/isaac_ros_apriltag/test/test_cases/apriltag0/image.json @@ -0,0 +1,4 @@ +{ + "image": "image.png", + "encoding": "bgr8" +} \ No newline at end of file diff --git a/isaac_ros_apriltag/test/test_cases/apriltag0/image.png b/isaac_ros_apriltag/test/test_cases/apriltag0/image.png new file mode 100644 index 0000000..f5cded7 --- /dev/null +++ b/isaac_ros_apriltag/test/test_cases/apriltag0/image.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6ea20e20dc8f25567276f0cf80f4a89ddd9d7b69707cd08e28946ca8915dbe3d +size 2407037 diff --git a/isaac_ros_apriltag_interfaces/CMakeLists.txt b/isaac_ros_apriltag_interfaces/CMakeLists.txt new file mode 100644 index 0000000..b6b46e0 --- /dev/null +++ b/isaac_ros_apriltag_interfaces/CMakeLists.txt @@ -0,0 +1,39 @@ +# 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(isaac_ros_apriltag_interfaces LANGUAGES C CXX CUDA) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Prepare custom AprilTag interfaces +find_package(rosidl_default_generators REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + msg/AprilTagDetection.msg + msg/AprilTagDetectionArray.msg + DEPENDENCIES geometry_msgs std_msgs +) +ament_export_dependencies(rosidl_default_runtime) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + +endif() + +ament_auto_package() diff --git a/msg/AprilTagDetection.msg b/isaac_ros_apriltag_interfaces/msg/AprilTagDetection.msg similarity index 100% rename from msg/AprilTagDetection.msg rename to isaac_ros_apriltag_interfaces/msg/AprilTagDetection.msg diff --git a/msg/AprilTagDetectionArray.msg b/isaac_ros_apriltag_interfaces/msg/AprilTagDetectionArray.msg similarity index 100% rename from msg/AprilTagDetectionArray.msg rename to isaac_ros_apriltag_interfaces/msg/AprilTagDetectionArray.msg diff --git a/package.xml b/isaac_ros_apriltag_interfaces/package.xml similarity index 50% rename from package.xml rename to isaac_ros_apriltag_interfaces/package.xml index e75a0fe..bb60740 100644 --- a/package.xml +++ b/isaac_ros_apriltag_interfaces/package.xml @@ -8,27 +8,28 @@ # distribution of this software and related documentation without an express # license agreement from NVIDIA CORPORATION is strictly prohibited. --> + - nvapriltags_ros2 - 0.8.0 - AprilTag Detection - nvidia - MIT + isaac_ros_apriltag_interfaces + 0.9.0 + Interfaces for performing Isaac ROS AprilTag detection - ament_cmake - - eigen + Hemal Shah + JetPack EULA + https://developer.nvidia.com/blog/accelerating-ai-modules-for-ros-and-ros-2-on-jetson/ + Arjun Bhorkar + Hemal Shah rosidl_default_generators rosidl_default_runtime - rosidl_interface_packages - rclcpp - rclcpp_components - sensor_msgs - tf2_msgs - image_transport - cv_bridge + geometry_msgs + std_msgs + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages ament_cmake diff --git a/nvapriltags/nvapriltags/nvAprilTags.h b/nvapriltags/nvapriltags/nvAprilTags.h deleted file mode 100644 index ecce9c3..0000000 --- a/nvapriltags/nvapriltags/nvAprilTags.h +++ /dev/null @@ -1,106 +0,0 @@ -/* - * 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__