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 fiducial_msgs.msg import FiducialTransformArray
from tf.transformations import euler_from_quaternion 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) 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 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.y = -t.y
m.angle = -euler[2] 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) pub.publish(m)
def main(args): 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() rospy.spin()