From aba76f58672dc6d2a8aef73fa69c5969d75693f0 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 4 Mar 2025 19:36:43 +0100 Subject: [PATCH 1/5] add 'image_proc' dependency for 'RectifyNode' in launch file --- package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/package.xml b/package.xml index dcc4df5..a7e881d 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,8 @@ image_transport cv_bridge + image_proc + ament_lint_auto ament_cmake_clang_format ament_cmake_cppcheck From 4fec9d2177d85aaf82cb532fde2c91c0c2512137 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 4 Mar 2025 22:12:02 +0100 Subject: [PATCH 2/5] use camera_ros for full libcamera support --- README.md | 4 ++-- ...h11.launch.yml => camera_36h11.launch.yml} | 22 ++++++++++--------- package.xml | 1 + 3 files changed, 15 insertions(+), 12 deletions(-) rename launch/{v4l2_36h11.launch.yml => camera_36h11.launch.yml} (68%) diff --git a/README.md b/README.md index c06a9d7..0b54c42 100644 --- a/README.md +++ b/README.md @@ -81,7 +81,7 @@ apriltag_ros AprilTagNode ``` -This `AprilTagNode` component can be loaded with other nodes into a "container node" process where they used shared-memory communication to prevent unnecessary data copies. The example launch file [v4l2_36h11.launch.yml](launch/v4l2_36h11.launch.yml) loads the `AprilTagNode` component together with the `v4l2_camera::V4L2Camera` component from the [`v4l2_camera` package](https://gitlab.com/boldhearts/ros2_v4l2_camera) (`sudo apt install ros-$ROS_DISTRO-v4l2-camera`) into one container and enables `use_intra_process_comms` for both: +This `AprilTagNode` component can be loaded with other nodes into a "container node" process where they used shared-memory communication to prevent unnecessary data copies. The example launch file [camera_36h11.launch.yml](launch/camera_36h11.launch.yml) loads the `AprilTagNode` component together with the `camera::CameraNode` component from the [`camera_ros` package](https://index.ros.org/p/camera_ros/) (`sudo apt install ros-$ROS_DISTRO-camera-ros`) into one container and enables `use_intra_process_comms` for both: ```sh -ros2 launch apriltag_ros v4l2_36h11.launch.yml +ros2 launch apriltag_ros camera_36h11.launch.yml ``` diff --git a/launch/v4l2_36h11.launch.yml b/launch/camera_36h11.launch.yml similarity index 68% rename from launch/v4l2_36h11.launch.yml rename to launch/camera_36h11.launch.yml index 7605e91..1f33d89 100644 --- a/launch/v4l2_36h11.launch.yml +++ b/launch/camera_36h11.launch.yml @@ -9,13 +9,13 @@ launch: name: apriltag_container namespace: "" composable_node: - - pkg: v4l2_camera - plugin: v4l2_camera::V4L2Camera + - pkg: camera_ros + plugin: camera::CameraNode name: camera - namespace: v4l2 + namespace: camera param: - - name: video_device - value: /dev/video$(var device) + - name: camera + value: $(var device) extra_arg: - name: use_intra_process_comms value: "True" @@ -23,10 +23,12 @@ launch: - pkg: image_proc plugin: image_proc::RectifyNode name: rectify - namespace: v4l2 + namespace: camera remap: - from: image - to: image_raw + to: /camera/camera/image_raw + - from: camera_info + to: /camera/camera/camera_info extra_arg: - name: use_intra_process_comms value: "True" @@ -37,9 +39,9 @@ launch: namespace: apriltag remap: - from: /apriltag/image_rect - to: /v4l2/image_rect - - from: /apriltag/camera_info - to: /v4l2/camera_info + to: /camera/image_rect + - from: /camera/camera_info + to: /camera/camera/camera_info param: - from: $(find-pkg-share apriltag_ros)/cfg/tags_36h11.yaml extra_arg: diff --git a/package.xml b/package.xml index a7e881d..366220b 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ image_transport cv_bridge + camera_ros image_proc ament_lint_auto From dfd8a6a2e3cfc3dcdd73022d36432c050e37f303 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 4 Mar 2025 22:31:23 +0100 Subject: [PATCH 3/5] its "ROS 2" with a space :-) https://www.ros.org/imgs/TrademarkRulesAndGuidelines2022.pdf --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 0b54c42..fb4bcbe 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ -# AprilTag ROS2 Node +# AprilTag ROS 2 Node -This ROS2 node uses the AprilTag library to detect AprilTags in images and publish their pose, id and additional metadata. +This ROS 2 node uses the AprilTag library to detect AprilTags in images and publish their pose, id and additional metadata. For more information on AprilTag, the paper and the reference implementation: https://april.eecs.umich.edu/software/apriltag.html From 93ae5f3792ba3abf4b329aa2d300a049c5fc8020 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 4 Mar 2025 23:29:41 +0100 Subject: [PATCH 4/5] check camera intrinsics and notify user if pose estimation is enabled --- src/AprilTagNode.cpp | 44 +++++++++++++++++++++++++++++++++----------- 1 file changed, 33 insertions(+), 11 deletions(-) diff --git a/src/AprilTagNode.cpp b/src/AprilTagNode.cpp index 5f646a3..c13bf26 100644 --- a/src/AprilTagNode.cpp +++ b/src/AprilTagNode.cpp @@ -117,7 +117,20 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options) const auto sizes = declare_parameter("tag.sizes", std::vector{}, descr("tag sizes per id", true)); // get method for estimating tag pose - estimate_pose = pose_estimation_methods.at(declare_parameter("pose_estimation_method", "pnp", descr("pose estimation method: \"pnp\" (more accurate) or \"homography\" (faster)", true))); + const std::string& pose_estimation_method = + declare_parameter("pose_estimation_method", "pnp", + descr("pose estimation method: \"pnp\" (more accurate) or \"homography\" (faster), " + "set to \"\" (empty) to disable pose estimation", + true)); + + if(!pose_estimation_method.empty()) { + if(pose_estimation_methods.count(pose_estimation_method)) { + estimate_pose = pose_estimation_methods.at(pose_estimation_method); + } + else { + RCLCPP_ERROR_STREAM(get_logger(), "Unknown pose estimation method '" << pose_estimation_method << "'."); + } + } // detector parameters in "detector" namespace declare_parameter("detector.threads", td->nthreads, descr("number of threads")); @@ -165,7 +178,15 @@ void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_i const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci) { // camera intrinsics for rectified images - const std::array intrinsics = {msg_ci->p.data()[0], msg_ci->p.data()[5], msg_ci->p.data()[2], msg_ci->p.data()[6]}; + const std::array intrinsics = {msg_ci->p[0], msg_ci->p[5], msg_ci->p[2], msg_ci->p[6]}; + + // check for valid intrinsics + const bool calibrated = msg_ci->width && msg_ci->height && + intrinsics[0] && intrinsics[1] && intrinsics[2] && intrinsics[3]; + + if(estimate_pose != nullptr && !calibrated) { + RCLCPP_WARN_STREAM(get_logger(), "The camera is not calibrated! Set 'pose_estimation_method' to \"\" (empty) to disable pose estimation and this warning."); + } // convert to 8bit monochrome image const cv::Mat img_uint8 = cv_bridge::toCvShare(msg_img, "mono8")->image; @@ -213,20 +234,21 @@ void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_i msg_detections.detections.push_back(msg_detection); // 3D orientation and position - geometry_msgs::msg::TransformStamped tf; - tf.header = msg_img->header; - // set child frame name by generic tag name or configured tag name - tf.child_frame_id = tag_frames.count(det->id) ? tag_frames.at(det->id) : std::string(det->family->name) + ":" + std::to_string(det->id); - const double size = tag_sizes.count(det->id) ? tag_sizes.at(det->id) : tag_edge_size; - if(estimate_pose != nullptr) { + if(estimate_pose != nullptr && calibrated) { + geometry_msgs::msg::TransformStamped tf; + tf.header = msg_img->header; + // set child frame name by generic tag name or configured tag name + tf.child_frame_id = tag_frames.count(det->id) ? tag_frames.at(det->id) : std::string(det->family->name) + ":" + std::to_string(det->id); + const double size = tag_sizes.count(det->id) ? tag_sizes.at(det->id) : tag_edge_size; tf.transform = estimate_pose(det, intrinsics, size); + tfs.push_back(tf); } - - tfs.push_back(tf); } pub_detections->publish(msg_detections); - tf_broadcaster.sendTransform(tfs); + + if(estimate_pose != nullptr) + tf_broadcaster.sendTransform(tfs); apriltag_detections_destroy(detections); } From aa2a269847b1eb8e2928ff8a9357b000586f85bc Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 4 Mar 2025 23:33:35 +0100 Subject: [PATCH 5/5] set 'pose_estimation_method' in documentation and config --- README.md | 2 ++ cfg/tags_36h11.yaml | 3 +++ 2 files changed, 5 insertions(+) diff --git a/README.md b/README.md index fb4bcbe..e91e714 100644 --- a/README.md +++ b/README.md @@ -44,6 +44,8 @@ apriltag: # node name sharpening: 0.25 # sharpening of decoded images debug: 0 # write additional debugging images to current working directory + pose_estimation_method: "pnp" # method for estimating the tag pose + # (optional) list of tags # If defined, 'frames' and 'sizes' must have the same length as 'ids'. tag: diff --git a/cfg/tags_36h11.yaml b/cfg/tags_36h11.yaml index 9c63520..b7b669d 100644 --- a/cfg/tags_36h11.yaml +++ b/cfg/tags_36h11.yaml @@ -14,6 +14,9 @@ sharpening: 0.25 # sharpening of decoded images debug: False # write additional debugging images to current working directory + + pose_estimation_method: "pnp" # method for estimating the tag pose + # optional list of tags tag: ids: [9, 14] # tag ID