From 0d65635c3f8cc6cb5d1abb3d4ea55552dd6e598c Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Wed, 7 Feb 2024 11:03:21 -0500 Subject: [PATCH] parameterize marker size --- image_proc/include/image_proc/track_marker.hpp | 1 + image_proc/src/track_marker.cpp | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/image_proc/include/image_proc/track_marker.hpp b/image_proc/include/image_proc/track_marker.hpp index e1076ac3c..7e1ffeba4 100644 --- a/image_proc/include/image_proc/track_marker.hpp +++ b/image_proc/include/image_proc/track_marker.hpp @@ -57,6 +57,7 @@ class TrackMarkerNode : public rclcpp::Node int queue_size_; int marker_id_; + double marker_size_; std::string image_topic_; rclcpp::Publisher::SharedPtr pub_; diff --git a/image_proc/src/track_marker.cpp b/image_proc/src/track_marker.cpp index 075be3194..470a84569 100644 --- a/image_proc/src/track_marker.cpp +++ b/image_proc/src/track_marker.cpp @@ -58,6 +58,7 @@ TrackMarkerNode::TrackMarkerNode(const rclcpp::NodeOptions & options) // Declare parameters before we setup any publishers or subscribers marker_id_ = this->declare_parameter("marker_id", 1); + marker_size_ = this->declare_parameter("marker_size", 0.05); detector_params_ = cv::aruco::DetectorParameters::create(); dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); @@ -119,7 +120,7 @@ void TrackMarkerNode::imageCb( // Estimate pose std::vector rvecs, tvecs; - cv::aruco::estimatePoseSingleMarkers(corners, 0.05, intrinsics, dist_coeffs, rvecs, tvecs); + cv::aruco::estimatePoseSingleMarkers(corners, marker_size_, intrinsics, dist_coeffs, rvecs, tvecs); // Publish pose of marker geometry_msgs::msg::PoseStamped pose;