def test_avg_tag_localization(self):
self.setup()
# Publish a matching apriltag observation (from the static publisher in the test file)
msg_tag = AprilTags()
tag_100 = TagDetection()
tag_100.id = 100
trans = tag_100.transform.translation
rot = tag_100.transform.rotation
(trans.x, trans.y, trans.z) = (1,-1,0)
(rot.x, rot.y, rot.z, rot.w) = (0,0,1,0)
msg_tag.detections.append(tag_100)
tag_101 = TagDetection()
tag_101.id = 101
trans = tag_101.transform.translation
rot = tag_101.transform.rotation
(trans.x, trans.y, trans.z) = (1,0,0)
(rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_euler(0,0,np.pi/2)
msg_tag.detections.append(tag_101)
self.pub_tag.publish(msg_tag)
# Wait for the node to publish a robot transform
tfbuf = tf2_ros.Buffer()
tfl = tf2_ros.TransformListener(tfbuf)
try:
Tr_w = tfbuf.lookup_transform("world", "duckiebot", rospy.Time(), rospy.Duration(5))
except(tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
self.assertFalse(True, "Test timed out waiting for the transform to be broadcast.")
trans = (Tr_w.transform.translation.x, Tr_w.transform.translation.y, Tr_w.transform.translation.z)
rot = (Tr_w.transform.rotation.x, Tr_w.transform.rotation.y, Tr_w.transform.rotation.z, Tr_w.transform.rotation.w)
numpy.testing.assert_array_almost_equal(trans, (2, 0.5, 0))
numpy.testing.assert_array_almost_equal(rot, tr.quaternion_from_euler(0,0,3*np.pi/4))
def test_single_tag_localization(self):
self.setup()
# Publish two matching apriltag observations
msg_tag = AprilTags()
tag = TagDetection()
tag.id = 100
trans = tag.transform.translation
rot = tag.transform.rotation
(trans.x, trans.y, trans.z) = (1, -1, 0)
(rot.x, rot.y, rot.z, rot.w) = (0, 0, 1, 0)
msg_tag.detections.append(tag)
self.pub_tag.publish(msg_tag)
# Wait for the node to publish a robot transform
tfbuf = tf2_ros.Buffer()
tfl = tf2_ros.TransformListener(tfbuf)
try:
Tr_w = tfbuf.lookup_transform("world", "duckiebot", rospy.Time(),
rospy.Duration(5))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
self.assertFalse(
True,
"Test timed out waiting for the transform to be broadcast.")
trans = (Tr_w.transform.translation.x, Tr_w.transform.translation.y,
Tr_w.transform.translation.z)
rot = (Tr_w.transform.rotation.x, Tr_w.transform.rotation.y,
Tr_w.transform.rotation.z, Tr_w.transform.rotation.w)
numpy.testing.assert_array_almost_equal(trans, (2, 0, 0))
numpy.testing.assert_array_almost_equal(rot, (0, 0, 1, 0))
def test_april_tags_single_interval(self):
#Setup the publisher and subscribers
self.pub_april_tags = rospy.Publisher(
"visual_odometry_april_tags_node/april_tags",
AprilTags,
queue_size=1,
latch=True)
self.pub_wheels_cmd = rospy.Publisher(
"visual_odometry_april_tags_node/wheels_cmd",
WheelsCmdStamped,
queue_size=1,
latch=True)
# Wait for the forward_kinematics_node to finish starting up
timeout = time.time() + 5.0
while (self.pub_wheels_cmd.get_num_connections() < 1 or self.pub_april_tags.get_num_connections() < 1) and \
not rospy.is_shutdown() and not time.time()>timeout:
rospy.sleep(0.1)
# Publish a single wheels cmd, and two simple april tag messages
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.vel_left = 1
msg_wheels_cmd.vel_right = 1
self.pub_wheels_cmd.publish(msg_wheels_cmd)
rospy.sleep(0.2) #Wait so the tags come in the right order
T1 = Transform()
T2 = Transform()
T1.translation.y = 2
T2.translation.y = 2
T2.rotation.x, T2.rotation.y, T2.rotation.z, T2.rotation.w = tr.quaternion_about_axis(
-np.pi / 2, (0, 0, 1))
msg_tag1 = AprilTags()
tag = TagDetection()
tag.transform = T1
msg_tag1.detections.append(tag)
msg_tag1.header.stamp = rospy.Duration(0)
self.pub_april_tags.publish(msg_tag1)
rospy.sleep(0.2) #Wait so the tags come in the right order
msg_tag2 = AprilTags()
msg_tag1.header.stamp = rospy.Duration(1)
tag.transform = T2
msg_tag1.detections.append(tag)
self.pub_april_tags.publish(msg_tag1)
# Wait 1 second for the file to be output
rospy.sleep(3)
res = np.genfromtxt(
rospy.get_param("visual_odometry_april_tags_node/filename", ''))
assert_almost_equal(res, np.array([1, 1, 1, np.pi / 2, 2, 2]))
def test_april_tags_single_interval(self):
#Setup the publisher and subscribers
self.pub_april_tags = rospy.Publisher("visual_odometry_april_tags_node/april_tags", AprilTags, queue_size=1, latch=True)
self.pub_wheels_cmd = rospy.Publisher("visual_odometry_april_tags_node/wheels_cmd", WheelsCmdStamped, queue_size=1, latch=True)
# Wait for the forward_kinematics_node to finish starting up
timeout = time.time()+5.0
while (self.pub_wheels_cmd.get_num_connections() < 1 or self.pub_april_tags.get_num_connections() < 1) and \
not rospy.is_shutdown() and not time.time()>timeout:
rospy.sleep(0.1)
# Publish a single wheels cmd, and two simple april tag messages
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.vel_left = 1
msg_wheels_cmd.vel_right = 1
self.pub_wheels_cmd.publish(msg_wheels_cmd)
rospy.sleep(0.2) #Wait so the tags come in the right order
T1 = Transform()
T2 = Transform()
T1.translation.y = 2
T2.translation.y = 2
T2.rotation.x, T2.rotation.y, T2.rotation.z, T2.rotation.w = tr.quaternion_about_axis(-np.pi/2, (0,0,1))
msg_tag1 = AprilTags()
tag = TagDetection()
tag.transform = T1
msg_tag1.detections.append(tag)
msg_tag1.header.stamp = rospy.Duration(0)
self.pub_april_tags.publish(msg_tag1)
rospy.sleep(0.2) #Wait so the tags come in the right order
msg_tag2 = AprilTags()
msg_tag1.header.stamp = rospy.Duration(1)
tag.transform = T2
msg_tag1.detections.append(tag)
self.pub_april_tags.publish(msg_tag1)
# Wait 1 second for the file to be output
rospy.sleep(3)
res = np.genfromtxt(rospy.get_param("visual_odometry_april_tags_node/filename", ''))
assert_almost_equal(res, np.array([1,1,1,np.pi/2, 2, 2]))