Merge pull request #13 from abhorkar-nv/abhorkar/minor-fixes

minor fixes

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

include/AprilTagNode.hpp

@@ -12,8 +12,8 @@
 
 #include <Eigen/Core>
 
-#include <apriltag_msgs/msg/april_tag_detection.hpp>
-#include <apriltag_msgs/msg/april_tag_detection_array.hpp>
+#include "nvapriltags_ros2/msg/april_tag_detection.hpp"
+#include "nvapriltags_ros2/msg/april_tag_detection_array.hpp"
 #include <image_transport/camera_subscriber.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
@@ -33,7 +33,7 @@ class AprilTagNode : public rclcpp::Node {
 
     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_;
+    const rclcpp::Publisher<nvapriltags_ros2::msg::AprilTagDetectionArray>::SharedPtr pub_detections_;
 
     struct AprilTagsImpl;
     std::unique_ptr<AprilTagsImpl> impl_;

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

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

msg/AprilTagDetectionArray.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+AprilTagDetection[] detections

package.xml

@@ -8,7 +8,7 @@
 # distribution of this software and related documentation without an express
 # license agreement from NVIDIA CORPORATION is strictly prohibited.
 -->
-<package format="2">
+<package format="3">
   <name>nvapriltags_ros2</name>
   <version>0.8.0</version>
   <description>AprilTag Detection</description>
@@ -19,11 +19,14 @@
 
   <build_depend>eigen</build_depend>
 
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
   <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>
 

src/AprilTagNode.cpp

@@ -132,7 +132,7 @@ AprilTagNode::AprilTagNode(rclcpp::NodeOptions options)
       pub_tf_(
           create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100))),
       pub_detections_(
-          create_publisher<apriltag_msgs::msg::AprilTagDetectionArray>(
+          create_publisher<nvapriltags_ros2::msg::AprilTagDetectionArray>(
               "detections", rclcpp::QoS(1))),
       impl_(std::make_unique<AprilTagsImpl>()) {}
 
@@ -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);