apo/RoboRally
1
0
Fork 0
forked from Telos4/RoboRally
Code Issues Pull Requests Releases Wiki Activity

removed fiducial transformation script duplicate

Browse Source
This commit is contained in:
Simon Pirkelmann 2019-08-12 16:48:59 +02:00
parent 2987da4373
commit 4830c2e252
1 changed files with 0 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
remote_control/fiducial_to_2d_pos_angle.py
View File

@ -1,38 +0,0 @@
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)