CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_pose: use Pose instead of PoseWithCovariance in Marker message (for now)

Browse Source
This commit is contained in:
Oleg Kalachev
2019-03-01 13:04:07 +03:00
parent af4321c530
commit f1c614d91a
3 changed files with 19 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
aruco_pose/msg/Marker.msg
View File

@@ -1,5 +1,5 @@
uint32 id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose
Point2D c1
Point2D c2
Point2D c3

8
aruco_pose/src/aruco_detect.cpp
View File

@@ -172,17 +172,17 @@ private:
fillCorners(marker, corners[i]);
if (estimate_poses_) {
fillPose(marker.pose.pose, rvecs[i], tvecs[i]);
fillPose(marker.pose, rvecs[i], tvecs[i]);
// snap orientation (if enabled and snap frame available)
if (!known_tilt_.empty() && !snap_to.header.frame_id.empty()) {
snapOrientation(marker.pose.pose.orientation, snap_to.transform.rotation);
snapOrientation(marker.pose.orientation, snap_to.transform.rotation);
}
// TODO: check IDs are unique
if (send_tf_) {
transform.child_frame_id = getChildFrameId(ids[i]);
transform.transform.rotation = marker.pose.pose.orientation;
transform.transform.rotation = marker.pose.orientation;
fillTranslation(transform.transform.translation, tvecs[i]);
br_.sendTransform(transform);
}
@@ -203,7 +203,7 @@ private:
vis_array_.markers.push_back(vis_marker);
for (unsigned int i = 0; i < ids.size(); i++)
pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose.pose,
pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose,
getMarkerLength(ids[i]), ids[i], i);
vis_markers_pub_.publish(vis_array_);

28
aruco_pose/test/basic.py
View File

@@ -22,13 +22,13 @@ class TestArucoPose(unittest.TestCase):
assert markers.header.frame_id == 'main_camera_optical'
assert markers.markers[0].id == 2
assert markers.markers[0].pose.pose.position.x == approx(0.36706567854)
assert markers.markers[0].pose.pose.position.y == approx(0.290484516644)
assert markers.markers[0].pose.pose.position.z == approx(2.18787602301)
assert markers.markers[0].pose.pose.orientation.x == approx(0.993997406299)
assert markers.markers[0].pose.pose.orientation.y == approx(-0.00532003481626)
assert markers.markers[0].pose.pose.orientation.z == approx(-0.107390951553)
assert markers.markers[0].pose.pose.orientation.w == approx(0.0201999263402)
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)
assert markers.markers[0].pose.orientation.x == approx(0.993997406299)
assert markers.markers[0].pose.orientation.y == approx(-0.00532003481626)
assert markers.markers[0].pose.orientation.z == approx(-0.107390951553)
assert markers.markers[0].pose.orientation.w == approx(0.0201999263402)
assert markers.markers[0].c1.x == approx(415.557739258)
assert markers.markers[0].c1.y == approx(335.557739258)
assert markers.markers[0].c2.x == approx(509.442260742)
@@ -39,13 +39,13 @@ class TestArucoPose(unittest.TestCase):
assert markers.markers[0].c4.y == approx(429.442260742)
assert markers.markers[3].id == 3
assert markers.markers[3].pose.pose.position.x == approx(-0.1805169666)
assert markers.markers[3].pose.pose.position.y == approx(-0.200697302327)
assert markers.markers[3].pose.pose.position.z == approx(0.585767514823)
assert markers.markers[3].pose.pose.orientation.x == approx(-0.961738074009)
assert markers.markers[3].pose.pose.orientation.y == approx(-0.0375180244707)
assert markers.markers[3].pose.pose.orientation.z == approx(-0.0115387773672)
assert markers.markers[3].pose.pose.orientation.w == approx(0.271144115664)
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)
assert markers.markers[3].pose.orientation.x == approx(-0.961738074009)
assert markers.markers[3].pose.orientation.y == approx(-0.0375180244707)
assert markers.markers[3].pose.orientation.z == approx(-0.0115387773672)
assert markers.markers[3].pose.orientation.w == approx(0.271144115664)
assert markers.markers[3].c1.x == approx(129.557723999)
assert markers.markers[3].c1.y == approx(49.557723999)
assert markers.markers[3].c2.x == approx(223.442276001)