forked from Telos4/RoboRally
38 lines
1.0 KiB
Python
38 lines
1.0 KiB
Python
|
import sys
|
||
|
|
import rospy
|
||
|
|
|
||
|
|
from marker_pos_angle.msg import id_pos_angle
|
||
|
|
from fiducial_msgs.msg import FiducialTransformArray
|
||
|
|
from tf.transformations import euler_from_quaternion
|
||
|
|
|
||
|
|
pub = rospy.Publisher('marker_id_pos_angle', id_pos_angle, queue_size=10)
|
||
|
|
'''
|
||
|
|
This callback function listens to fiducial messages that describe the translation and rotation of markers w.r.t. to the
|
||
|
|
camera and transforms them to a position in the 2d plane and the angle of the robot in the plane
|
||
|
|
'''
|
||
|
|
def callback(data):
|
||
|
|
for d in data.transforms:
|
||
|
|
id = d.fiducial_id
|
||
|
|
|
||
|
|
q = d.transform.rotation
|
||
|
|
t = d.transform.translation
|
||
|
|
|
||
|
|
euler = euler_from_quaternion([q.x, q.y, q.z, q.w])
|
||
|
|
|
||
|
|
m = id_pos_angle()
|
||
|
|
m.id = id
|
||
|
|
m.x = t.x
|
||
|
|
m.y = -t.y
|
||
|
|
m.angle = -euler[2]
|
||
|
|
|
||
|
|
pub.publish(m)
|
||
|
|
|
||
|
|
def main(args):
|
||
|
|
rospy.init_node('euler_quaternion_node', anonymous=True)
|
||
|
|
|
||
|
|
quaternion_sub = rospy.Subscriber("/fiducial_transforms", FiducialTransformArray, callback)
|
||
|
|
|
||
|
|
rospy.spin()
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
main(sys.argv)
|