diff --git a/fiducial_transform/src/fiducial_transform/src/fiducial_transform/fiducial_to_2d_pos_angle.py b/fiducial_transform/src/fiducial_transform/src/fiducial_transform/fiducial_to_2d_pos_angle.py index 3a3c10c..a03f3e9 100644 --- a/fiducial_transform/src/fiducial_transform/src/fiducial_transform/fiducial_to_2d_pos_angle.py +++ b/fiducial_transform/src/fiducial_transform/src/fiducial_transform/fiducial_to_2d_pos_angle.py @@ -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()