From 2c9486f9610441f46697dfea2c4ed083b186a37f Mon Sep 17 00:00:00 2001 From: Arjun Date: Fri, 11 Jun 2021 18:04:06 -0700 Subject: [PATCH] minor fixes --- CMakeLists.txt | 20 +++++++++++++++++--- include/AprilTagNode.hpp | 6 +++--- launch/tag_36h11.launch.py | 2 +- msg/AprilTagDetection.msg | 5 +++++ msg/AprilTagDetectionArray.msg | 2 ++ package.xml | 7 +++++-- src/AprilTagNode.cpp | 28 +++++++++++++++++++++++----- 7 files changed, 56 insertions(+), 14 deletions(-) create mode 100644 msg/AprilTagDetection.msg create mode 100644 msg/AprilTagDetectionArray.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index bfc18b4..85cb32b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,7 @@ cmake_minimum_required(VERSION 3.5) -project(nvapriltags_ros2 LANGUAGES CXX CUDA) +project(nvapriltags_ros2 LANGUAGES C CXX CUDA) set(CMAKE_CXX_STANDARD 14) set(CUDA_MIN_VERSION "10.2") @@ -19,7 +19,6 @@ 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) @@ -46,10 +45,13 @@ 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 apriltag_msgs tf2_msgs image_transport cv_bridge) +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") @@ -63,4 +65,16 @@ install(TARGETS AprilTagNode 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/include/AprilTagNode.hpp b/include/AprilTagNode.hpp index d565c8e..0bb7a62 100644 --- a/include/AprilTagNode.hpp +++ b/include/AprilTagNode.hpp @@ -12,8 +12,8 @@ #include -#include -#include +#include "nvapriltags_ros2/msg/april_tag_detection.hpp" +#include "nvapriltags_ros2/msg/april_tag_detection_array.hpp" #include #include #include @@ -33,7 +33,7 @@ class AprilTagNode : public rclcpp::Node { const image_transport::CameraSubscriber sub_cam_; const rclcpp::Publisher::SharedPtr pub_tf_; - const rclcpp::Publisher::SharedPtr pub_detections_; + 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 index e4020f3..717f584 100644 --- a/launch/tag_36h11.launch.py +++ b/launch/tag_36h11.launch.py @@ -14,7 +14,7 @@ cfg_36h11 = { "image_transport": "raw", "family": "36h11", - "size": 0.162, + "size": 0.162 } def generate_launch_description(): diff --git a/msg/AprilTagDetection.msg b/msg/AprilTagDetection.msg new file mode 100644 index 0000000..d4254a7 --- /dev/null +++ b/msg/AprilTagDetection.msg @@ -0,0 +1,5 @@ +string family +int32 id +geometry_msgs/Point center # centre in (x,y) pixel coordinates +geometry_msgs/Point[4] corners # corners of tag ((x1,y1),(x2,y2),...) +geometry_msgs/PoseWithCovarianceStamped pose diff --git a/msg/AprilTagDetectionArray.msg b/msg/AprilTagDetectionArray.msg new file mode 100644 index 0000000..7b45385 --- /dev/null +++ b/msg/AprilTagDetectionArray.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +AprilTagDetection[] detections diff --git a/package.xml b/package.xml index c7c1019..e75a0fe 100644 --- a/package.xml +++ b/package.xml @@ -8,7 +8,7 @@ # 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 @@ -19,11 +19,14 @@ eigen + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + rclcpp rclcpp_components sensor_msgs tf2_msgs - apriltag_msgs image_transport cv_bridge diff --git a/src/AprilTagNode.cpp b/src/AprilTagNode.cpp index 048ac9e..5fff680 100644 --- a/src/AprilTagNode.cpp +++ b/src/AprilTagNode.cpp @@ -132,7 +132,7 @@ AprilTagNode::AprilTagNode(rclcpp::NodeOptions options) pub_tf_( create_publisher("/tf", rclcpp::QoS(100))), pub_detections_( - create_publisher( + create_publisher( "detections", rclcpp::QoS(1))), impl_(std::make_unique()) {} @@ -170,17 +170,16 @@ void AprilTagNode::onCameraFrame( } // Parse detections into published protos - apriltag_msgs::msg::AprilTagDetectionArray msg_detections; + nvapriltags_ros2::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; + nvapriltags_ros2::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++) { @@ -189,7 +188,19 @@ void AprilTagNode::onCameraFrame( msg_detection.corners.data()[corner_idx].y = detection.corners[corner_idx].y; } - msg_detections.detections.push_back(msg_detection); + + // center + const float slope_1 = (detection.corners[2].y - detection.corners[0].y) / + (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); + const float intercept_1 = detection.corners[0].y - + (slope_1 * detection.corners[0].x); + const float intercept_2 = detection.corners[3].y - + (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); // Timestamped Pose3 transform geometry_msgs::msg::TransformStamped tf; @@ -198,6 +209,13 @@ void AprilTagNode::onCameraFrame( std::string(tag_family_) + ":" + std::to_string(detection.id); tf.transform = ToTransformMsg(detection); tfs.transforms.push_back(tf); + + // Pose + msg_detection.pose.pose.pose.position.x = tf.transform.translation.x; + msg_detection.pose.pose.pose.position.y = tf.transform.translation.y; + msg_detection.pose.pose.pose.position.z = tf.transform.translation.z; + msg_detection.pose.pose.pose.orientation = tf.transform.rotation; + msg_detections.detections.push_back(msg_detection); } pub_detections_->publish(msg_detections);