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

added parameter options to transformation script

simple_control
Simon Pirkelmann 2019-08-08 19:13:08 +02:00
parent 3b364fd9b3
commit afdd3dbc15
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
fiducial_transform/src/fiducial_transform/src/fiducial_transform/fiducial_to_2d_pos_angle.py
View File

@ -5,6 +5,9 @@ from marker_pos_angle.msg import id_pos_angle
from fiducial_msgs.msg import FiducialTransformArray
from tf.transformations import euler_from_quaternion
transform_topic = rospy.get_param("transform_topic", '/fiducial_transforms')
output = rospy.get_param("output", 'True')
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
@ -24,12 +27,16 @@ def callback(data):
m.y = -t.y
m.angle = -euler[2]
if output == 'True':
print("(marker_id, pos_x, pos_y, angle) = ({}, {}, {}, {})".format(m.id, m.x, m.y, m.angle))
pub.publish(m)
def main(args):
rospy.init_node('euler_quaternion_node', anonymous=True)
rospy.init_node('fiducial_transform_node', anonymous=True)
quaternion_sub = rospy.Subscriber("/fiducial_transforms", FiducialTransformArray, callback)
print("Starting subscriber listening for {} topic".format(transform_topic))
quaternion_sub = rospy.Subscriber(transform_topic, FiducialTransformArray, callback)
rospy.spin()