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.
@@ -137,4 +137,4 @@ In order to charge the battery of the robot just plug a micro USB cable into the
 ![Battery charging](https://imaginaerraum.de/git/Telos4/RoboRally/raw/branch/master/docs/images/charging.jpeg)
 
 ### Next steps
-The next step is to setup the position detection using ROS. For this see the [4_ROS_SETUP.md](https://imaginaerraum.de/git/Telos4/RoboRally/src/branch/master/docs/4_ROS_SETUP.md)
+The next step is to setup the position detection using ROS. For this see the [4_ROS_SETUP.md](https://imaginaerraum.de/git/Telos4/RoboRally/src/branch/master/docs/4_ROS_SETUP.md)

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)