From 1d8d2d80432b2c367225e238321d5b53ffe83122 Mon Sep 17 00:00:00 2001
From: spirkelmann <simon.pirkelmann@gmail.com>
Date: Fri, 7 Jun 2019 14:20:05 +0200
Subject: [PATCH] added ros node for transformation of translation and
 quaternion rotation to position and angle in 2d plane

---
 remote_control/fiducial_to_2d_pos_angle.py | 38 ++++++++++++++++++++++
 1 file changed, 38 insertions(+)
 create mode 100644 remote_control/fiducial_to_2d_pos_angle.py

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