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

Use aruco_map_ prefix for markers in the map

Browse Source
This commit is contained in:
Oleg Kalachev
2019-07-20 17:30:06 +03:00
parent 57c22fccf7
commit bc0e45740f
5 changed files with 50 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
aruco_pose/src/aruco_detect.cpp
View File

@@ -185,9 +185,13 @@ private:
// TODO: check IDs are unique
if (send_tf_) {
transform.child_frame_id = getChildFrameId(ids[i]);
transform.transform.rotation = marker.pose.orientation;
fillTranslation(transform.transform.translation, tvecs[i]);
br_.sendTransform(transform);
// check if such static transform exists
if (!tf_buffer_.canTransform(transform.header.frame_id, transform.child_frame_id, transform.header.stamp)) {
transform.transform.rotation = marker.pose.orientation;
fillTranslation(transform.transform.translation, tvecs[i]);
br_.sendTransform(transform);
}
}
}
array_.markers.push_back(marker);

64
aruco_pose/test/basic.py
View File

@@ -24,7 +24,7 @@ def approx(expected):
def test_markers(node):
markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
assert len(markers.markers) == 4
assert len(markers.markers) == 5
assert markers.header.frame_id == 'main_camera_optical'
assert markers.markers[0].id == 2
@@ -45,28 +45,46 @@ def test_markers(node):
assert markers.markers[0].c4.x == approx(415.557739258)
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)
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)
assert markers.markers[3].c2.y == approx(49.557723999)
assert markers.markers[3].c3.x == approx(223.442276001)
assert markers.markers[3].c3.y == approx(143.442276001)
assert markers.markers[3].c4.x == approx(129.557723999)
assert markers.markers[3].c4.y == approx(143.442276001)
assert markers.markers[4].id == 3
assert markers.markers[4].length == approx(0.1)
assert markers.markers[4].pose.position.x == approx(-0.1805169666)
assert markers.markers[4].pose.position.y == approx(-0.200697302327)
assert markers.markers[4].pose.position.z == approx(0.585767514823)
assert markers.markers[4].pose.orientation.x == approx(-0.961738074009)
assert markers.markers[4].pose.orientation.y == approx(-0.0375180244707)
assert markers.markers[4].pose.orientation.z == approx(-0.0115387773672)
assert markers.markers[4].pose.orientation.w == approx(0.271144115664)
assert markers.markers[4].c1.x == approx(129.557723999)
assert markers.markers[4].c1.y == approx(49.557723999)
assert markers.markers[4].c2.x == approx(223.442276001)
assert markers.markers[4].c2.y == approx(49.557723999)
assert markers.markers[4].c3.x == approx(223.442276001)
assert markers.markers[4].c3.y == approx(143.442276001)
assert markers.markers[4].c4.x == approx(129.557723999)
assert markers.markers[4].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[3].id == 4
assert markers.markers[3].length == approx(0.33)
assert markers.markers[2].id == 100
assert markers.markers[2].length == approx(0.33)
assert markers.markers[2].pose.position.x == approx(-1.37600105389)
assert markers.markers[2].pose.position.y == approx(-0.323028530991)
assert markers.markers[2].pose.position.z == approx(2.94611272668)
assert markers.markers[2].pose.orientation.x == approx(-0.955543925678)
assert markers.markers[2].pose.orientation.y == approx(0.0458801909197)
assert markers.markers[2].pose.orientation.z == approx(-0.249604946264)
assert markers.markers[2].pose.orientation.w == approx(-0.150093920537)
assert markers.markers[2].c1.x == approx(52.557723999)
assert markers.markers[2].c1.y == approx(205.557723999)
assert markers.markers[2].c2.x == approx(113.442276001)
assert markers.markers[2].c2.y == approx(205.557723999)
assert markers.markers[2].c3.x == approx(113.442276001)
assert markers.markers[2].c3.y == approx(265.442260742)
assert markers.markers[2].c4.x == approx(52.557723999)
assert markers.markers[2].c4.y == approx(265.442260742)
def test_markers_frames(node, tf_buffer):
marker_2 = tf_buffer.lookup_transform('main_camera_optical', 'aruco_2', rospy.Time(), rospy.Duration(5))
@@ -82,24 +100,24 @@ def test_map_markers_frames(node, tf_buffer):
stamp = rospy.get_rostime()
timeout = rospy.Duration(5)
marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_1', stamp, timeout)
marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_1', stamp, timeout)
assert marker_1.transform.translation.x == approx(0)
assert marker_1.transform.translation.y == approx(0)
assert marker_1.transform.translation.z == approx(0)
marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_4', stamp, timeout)
marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_4', stamp, timeout)
assert marker_4.transform.translation.x == approx(1)
assert marker_4.transform.translation.y == approx(1)
assert marker_4.transform.translation.z == approx(0)
marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_12', stamp, timeout)
marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_12', stamp, timeout)
assert marker_12.transform.translation.x == approx(0.2)
assert marker_12.transform.translation.y == approx(0.5)
assert marker_12.transform.translation.z == approx(0)
def test_visualization(node):
vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
assert len(vis.markers) == 9
assert len(vis.markers) == 11
def test_debug(node):
img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)

2
aruco_pose/test/basic.test
View File

@@ -23,7 +23,7 @@
<param name="type" value="map"/>
<param name="map" value="$(find aruco_pose)/test/basic.txt"/>
<param name="markers/frame_id" value="aruco_map"/>
<param name="markers/child_frame_id_prefix" value="aruco_map_"/>
<param name="markers/child_frame_id_prefix" value="aruco_in_map_"/>
</node>
<param name="test_module" value="$(find aruco_pose)/test/basic.py"/>

BIN
aruco_pose/test/map.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.1 KiB

After

Width:  |  Height:  |  Size: 2.1 KiB

2
clever/launch/aruco.launch
View File

@@ -25,7 +25,7 @@
<param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
<param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
<param name="markers/frame_id" value="aruco_map"/>
<param name="markers/child_frame_id_prefix" value="aruco_map_"/>
<param name="markers/child_frame_id_prefix" value="aruco_"/>
</node>
<!-- vpe publisher from aruco markers -->