From 7a56a7b231ce43e85837cd246ddf4ed667a44aef Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 2 Mar 2019 23:51:42 +0300 Subject: [PATCH] aruco_pose: add length to Marker message --- aruco_pose/msg/Marker.msg | 1 + aruco_pose/src/aruco_detect.cpp | 1 + aruco_pose/test/basic.py | 4 ++++ 3 files changed, 6 insertions(+) diff --git a/aruco_pose/msg/Marker.msg b/aruco_pose/msg/Marker.msg index ee7d4046..44b1ec9c 100644 --- a/aruco_pose/msg/Marker.msg +++ b/aruco_pose/msg/Marker.msg @@ -1,4 +1,5 @@ uint32 id +float32 length geometry_msgs/Pose pose Point2D c1 Point2D c2 diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp index 9c2dc80b..7a385eb5 100644 --- a/aruco_pose/src/aruco_detect.cpp +++ b/aruco_pose/src/aruco_detect.cpp @@ -169,6 +169,7 @@ private: for (unsigned int i = 0; i < ids.size(); i++) { marker.id = ids[i]; + marker.length = getMarkerLength(marker.id); fillCorners(marker, corners[i]); if (estimate_poses_) { diff --git a/aruco_pose/test/basic.py b/aruco_pose/test/basic.py index bb44909a..c3d805a8 100755 --- a/aruco_pose/test/basic.py +++ b/aruco_pose/test/basic.py @@ -22,6 +22,7 @@ class TestArucoPose(unittest.TestCase): assert markers.header.frame_id == 'main_camera_optical' assert markers.markers[0].id == 2 + assert markers.markers[0].length == approx(0.33) assert markers.markers[0].pose.position.x == approx(0.36706567854) assert markers.markers[0].pose.position.y == approx(0.290484516644) assert markers.markers[0].pose.position.z == approx(2.18787602301) @@ -39,6 +40,7 @@ class TestArucoPose(unittest.TestCase): assert markers.markers[0].c4.y == approx(429.442260742) assert markers.markers[3].id == 3 + assert markers.markers[3].length == approx(0.1) assert markers.markers[3].pose.position.x == approx(-0.1805169666) assert markers.markers[3].pose.position.y == approx(-0.200697302327) assert markers.markers[3].pose.position.z == approx(0.585767514823) @@ -56,7 +58,9 @@ class TestArucoPose(unittest.TestCase): assert markers.markers[3].c4.y == approx(143.442276001) assert markers.markers[1].id == 1 + assert markers.markers[1].length == approx(0.33) assert markers.markers[2].id == 4 + assert markers.markers[2].length == approx(0.33) def test_visualization(self): vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)