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)