docs/3_ROBOT_SETUP.md

@@ -113,7 +113,7 @@ $ sudo pip install pygame
 ```
 The program is located in the `remote_control/`. Run it using
 ```
-$ python keyboard_controller.py 192.168.1.101
+$ python keyboard_controller.py '192.168.1.101'
 ```
 When running the program you should see an output in the WebREPL that a connection was established.
 You can use the arrow keys for sending commands to the microcontroller which it then passes on to the motors.

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)