diff --git a/remote_control/fiducial_to_2d_pos_angle.py b/remote_control/fiducial_to_2d_pos_angle.py new file mode 100644 index 0000000..56717bf --- /dev/null +++ b/remote_control/fiducial_to_2d_pos_angle.py @@ -0,0 +1,38 @@ +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) \ No newline at end of file