From 2bbd2810edd762836edd123068cc21887101d589 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Sat, 24 Sep 2022 14:01:07 -0700 Subject: [PATCH 1/9] =?UTF-8?q?use=20default=20C++=20instead=20of=20C++11?= =?UTF-8?q?=20which=20avoids=20'=E2=80=98std::shared=5Fmutex=E2=80=99=20is?= =?UTF-8?q?=20only=20available=20from=20C++17=20onwards'=20error=20on=2022?= =?UTF-8?q?.04?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- stag_detect/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/stag_detect/CMakeLists.txt b/stag_detect/CMakeLists.txt index fd293a0..61bfaa5 100644 --- a/stag_detect/CMakeLists.txt +++ b/stag_detect/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs fiducial_msgs dynamic_reconfigure + vision_msgs ) find_package(OpenCV REQUIRED) @@ -36,8 +37,6 @@ catkin_package( ## Build ## ########### -add_definitions(-std=c++11) - include_directories( include ${catkin_INCLUDE_DIRS} From 55136ce3e91b5beab845c1f3eecab0ad778d9704 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Mon, 26 Dec 2022 08:26:37 -0800 Subject: [PATCH 2/9] use hpp instead of deprecated .h pluginlib headers --- stag_detect/include/stag_ros/stag_nodelet.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stag_detect/include/stag_ros/stag_nodelet.h b/stag_detect/include/stag_ros/stag_nodelet.h index 389758d..51f84d3 100644 --- a/stag_detect/include/stag_ros/stag_nodelet.h +++ b/stag_detect/include/stag_ros/stag_nodelet.h @@ -23,7 +23,7 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #pragma once -#include +#include #include From 501b83967a56b54d83e80e8cadc015cd846d8490 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Sat, 20 Apr 2024 07:17:16 -0700 Subject: [PATCH 3/9] use cv::drawFrameAxes instead of aruco::drawAxis --- aruco_detect/src/aruco_detect.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aruco_detect/src/aruco_detect.cpp b/aruco_detect/src/aruco_detect.cpp index d9e1199..09da06e 100644 --- a/aruco_detect/src/aruco_detect.cpp +++ b/aruco_detect/src/aruco_detect.cpp @@ -428,8 +428,8 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) reprojectionError); for (size_t i=0; iimage, cameraMatrix, distortionCoeffs, - rvecs[i], tvecs[i], (float)fiducial_len); + cv::drawFrameAxes(cv_ptr->image, cameraMatrix, distortionCoeffs, + rvecs[i], tvecs[i], (float)fiducial_len); if(verbose){ ROS_INFO("Detected id %d T %.2f %.2f %.2f R %.2f %.2f %.2f", ids[i], tvecs[i][0], tvecs[i][1], tvecs[i][2], From 8c4b4ff892af299bd02fca5dd13612c5a60e760f Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Thu, 30 May 2024 15:52:14 -0700 Subject: [PATCH 4/9] allow overriding of the tf child frame ids so multiple instances of aruco_detect won't conflict --- aruco_detect/src/aruco_detect.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/aruco_detect/src/aruco_detect.cpp b/aruco_detect/src/aruco_detect.cpp index 09da06e..14b74fe 100644 --- a/aruco_detect/src/aruco_detect.cpp +++ b/aruco_detect/src/aruco_detect.cpp @@ -95,6 +95,8 @@ class FiducialsNode { double fiducial_len; + std::string tf_prefix; + bool doPoseEstimation; bool haveCamInfo; bool publishFiducialTf; @@ -510,7 +512,7 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) ts.transform.rotation.z = q.z(); ts.header.frame_id = frameId; ts.header.stamp = msg->header.stamp; - ts.child_frame_id = "fiducial_" + std::to_string(ids[i]); + ts.child_frame_id = tf_prefix + std::to_string(ids[i]); broadcaster.sendTransform(ts); } else { @@ -518,7 +520,7 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) ts.transform = ft.transform; ts.header.frame_id = frameId; ts.header.stamp = msg->header.stamp; - ts.child_frame_id = "fiducial_" + std::to_string(ft.fiducial_id); + ts.child_frame_id = tf_prefix + std::to_string(ft.fiducial_id); broadcaster.sendTransform(ts); } } @@ -614,6 +616,8 @@ FiducialsNode::FiducialsNode() : nh(), pnh("~"), it(nh) pnh.param("vis_msgs", vis_msgs, false); pnh.param("verbose", verbose, false); + pnh.param("tf_prefix", tf_prefix, "fiducial_"); + std::string str; std::vector strs; From 7b3ae5286a5769c448a642a30b4f6c04e72227e5 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Thu, 30 May 2024 15:53:16 -0700 Subject: [PATCH 5/9] put fiducial image in relative namespace instead root so multiple instance of aruco detect can be run --- aruco_detect/src/aruco_detect.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aruco_detect/src/aruco_detect.cpp b/aruco_detect/src/aruco_detect.cpp index 14b74fe..bd82cea 100644 --- a/aruco_detect/src/aruco_detect.cpp +++ b/aruco_detect/src/aruco_detect.cpp @@ -663,7 +663,7 @@ FiducialsNode::FiducialsNode() : nh(), pnh("~"), it(nh) } } - image_pub = it.advertise("/fiducial_images", 1); + image_pub = it.advertise("fiducial_images", 1); vertices_pub = nh.advertise("fiducial_vertices", 1); From f5b80090cd778d2a49d13e1ccc8167ca1c8746d0 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Fri, 31 May 2024 06:44:02 -0700 Subject: [PATCH 6/9] use a regular image publisher instead of image_transport for the debug fiducial image --- aruco_detect/src/aruco_detect.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/aruco_detect/src/aruco_detect.cpp b/aruco_detect/src/aruco_detect.cpp index bd82cea..819d74f 100644 --- a/aruco_detect/src/aruco_detect.cpp +++ b/aruco_detect/src/aruco_detect.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -113,7 +114,7 @@ class FiducialsNode { ros::NodeHandle nh; ros::NodeHandle pnh; - image_transport::Publisher image_pub; + ros::Publisher image_pub; // log spam prevention int prev_detected_count; @@ -663,7 +664,7 @@ FiducialsNode::FiducialsNode() : nh(), pnh("~"), it(nh) } } - image_pub = it.advertise("fiducial_images", 1); + image_pub = nh.advertise("fiducial_images", 1); vertices_pub = nh.advertise("fiducial_vertices", 1); From ec23167b47f7b2a6a2563068e3b7202c96358cf6 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Fri, 31 May 2024 06:51:58 -0700 Subject: [PATCH 7/9] copy header into Detection2D items --- aruco_detect/src/aruco_detect.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/aruco_detect/src/aruco_detect.cpp b/aruco_detect/src/aruco_detect.cpp index 819d74f..b74f378 100644 --- a/aruco_detect/src/aruco_detect.cpp +++ b/aruco_detect/src/aruco_detect.cpp @@ -464,6 +464,7 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) tf2::Quaternion q; if (vis_msgs) { vision_msgs::Detection2D vm; + vm.header = vma.header; vision_msgs::ObjectHypothesisWithPose vmh; vmh.id = ids[i]; vmh.score = exp(-2 * object_error); // [0, infinity] -> [1,0] From e1520df06e7f306f2f8a8837bfae297ea757b83b Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Fri, 31 May 2024 07:24:30 -0700 Subject: [PATCH 8/9] set the camera frame id as a prefix so no chance of conflicts with other aruco_detect isntances --- aruco_detect/src/aruco_detect.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/aruco_detect/src/aruco_detect.cpp b/aruco_detect/src/aruco_detect.cpp index b74f378..367b0af 100644 --- a/aruco_detect/src/aruco_detect.cpp +++ b/aruco_detect/src/aruco_detect.cpp @@ -503,6 +503,7 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) // Publish tf for the fiducial relative to the camera if (publishFiducialTf) { + const auto frame = msg->header.frame_id + "_" + tf_prefix + std::to_string(ids[i]); if (vis_msgs) { geometry_msgs::TransformStamped ts; ts.transform.translation.x = tvecs[i][0]; @@ -514,7 +515,7 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) ts.transform.rotation.z = q.z(); ts.header.frame_id = frameId; ts.header.stamp = msg->header.stamp; - ts.child_frame_id = tf_prefix + std::to_string(ids[i]); + ts.child_frame_id = frame; broadcaster.sendTransform(ts); } else { @@ -522,7 +523,7 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) ts.transform = ft.transform; ts.header.frame_id = frameId; ts.header.stamp = msg->header.stamp; - ts.child_frame_id = tf_prefix + std::to_string(ft.fiducial_id); + ts.child_frame_id = frame; broadcaster.sendTransform(ts); } } From e4ea5f5478bcb603affa236276cf7fd0bfe18c30 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Fri, 31 May 2024 08:33:45 -0700 Subject: [PATCH 9/9] factor out duplicate code with setting transforms/poses --- aruco_detect/src/aruco_detect.cpp | 63 ++++++++++++------------------- 1 file changed, 24 insertions(+), 39 deletions(-) diff --git a/aruco_detect/src/aruco_detect.cpp b/aruco_detect/src/aruco_detect.cpp index 367b0af..e73f9e1 100644 --- a/aruco_detect/src/aruco_detect.cpp +++ b/aruco_detect/src/aruco_detect.cpp @@ -459,23 +459,31 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) (reprojectionError[i] / dist(corners[i][0], corners[i][2])) * (norm(tvecs[i]) / fiducial_len); + tf2::Quaternion q; + geometry_msgs::Transform transform; + { + transform.translation.x = tvecs[i][0]; + transform.translation.y = tvecs[i][1]; + transform.translation.z = tvecs[i][2]; + q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle); + transform.rotation.w = q.w(); + transform.rotation.x = q.x(); + transform.rotation.y = q.y(); + transform.rotation.z = q.z(); + } + // Standard ROS vision_msgs fiducial_msgs::FiducialTransform ft; - tf2::Quaternion q; if (vis_msgs) { vision_msgs::Detection2D vm; vm.header = vma.header; vision_msgs::ObjectHypothesisWithPose vmh; vmh.id = ids[i]; vmh.score = exp(-2 * object_error); // [0, infinity] -> [1,0] - vmh.pose.pose.position.x = tvecs[i][0]; - vmh.pose.pose.position.y = tvecs[i][1]; - vmh.pose.pose.position.z = tvecs[i][2]; - q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle); - vmh.pose.pose.orientation.w = q.w(); - vmh.pose.pose.orientation.x = q.x(); - vmh.pose.pose.orientation.y = q.y(); - vmh.pose.pose.orientation.z = q.z(); + vmh.pose.pose.position.x = transform.translation.x; + vmh.pose.pose.position.y = transform.translation.y; + vmh.pose.pose.position.z = transform.translation.z; + vmh.pose.pose.orientation = transform.rotation; vm.results.push_back(vmh); vma.detections.push_back(vm); @@ -483,14 +491,7 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) else { ft.fiducial_id = ids[i]; - ft.transform.translation.x = tvecs[i][0]; - ft.transform.translation.y = tvecs[i][1]; - ft.transform.translation.z = tvecs[i][2]; - q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle); - ft.transform.rotation.w = q.w(); - ft.transform.rotation.x = q.x(); - ft.transform.rotation.y = q.y(); - ft.transform.rotation.z = q.z(); + ft.transform = transform; ft.fiducial_area = calcFiducialArea(corners[i]); ft.image_error = reprojectionError[i]; // Convert image_error (in pixels) to object_error (in meters) @@ -504,28 +505,12 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) // Publish tf for the fiducial relative to the camera if (publishFiducialTf) { const auto frame = msg->header.frame_id + "_" + tf_prefix + std::to_string(ids[i]); - if (vis_msgs) { - geometry_msgs::TransformStamped ts; - ts.transform.translation.x = tvecs[i][0]; - ts.transform.translation.y = tvecs[i][1]; - ts.transform.translation.z = tvecs[i][2]; - ts.transform.rotation.w = q.w(); - ts.transform.rotation.x = q.x(); - ts.transform.rotation.y = q.y(); - ts.transform.rotation.z = q.z(); - ts.header.frame_id = frameId; - ts.header.stamp = msg->header.stamp; - ts.child_frame_id = frame; - broadcaster.sendTransform(ts); - } - else { - geometry_msgs::TransformStamped ts; - ts.transform = ft.transform; - ts.header.frame_id = frameId; - ts.header.stamp = msg->header.stamp; - ts.child_frame_id = frame; - broadcaster.sendTransform(ts); - } + geometry_msgs::TransformStamped ts; + ts.transform = transform; + ts.header.frame_id = frameId; + ts.header.stamp = msg->header.stamp; + ts.child_frame_id = frame; + broadcaster.sendTransform(ts); } } }