def callback(data):
    global objFramePrefix_
    global distanceMax_
    if (len(data.objects.data) > 0):
        output = AprilTagDetectionArray()
        output.header = data.header
        for i in range(0, len(data.objects.data), 12):
            try:
                objId = data.objects.data[i]
                (trans, quat) = listener.lookupTransform(data.header.frame_id, ((objFramePrefix_ + '_') + str(int(objId))), data.header.stamp)
                tag = AprilTagDetection()
                tag.id.append(objId)
                tag.pose.pose.pose.position.x = trans[0]
                tag.pose.pose.pose.position.y = trans[1]
                tag.pose.pose.pose.position.z = trans[2]
                tag.pose.pose.pose.orientation.x = quat[0]
                tag.pose.pose.pose.orientation.y = quat[1]
                tag.pose.pose.pose.orientation.z = quat[2]
                tag.pose.pose.pose.orientation.w = quat[3]
                tag.pose.header = output.header
                if ((distanceMax_ <= 0.0) or (trans[2] < distanceMax_)):
                    output.detections.append(tag)
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
        if (len(output.detections) > 0):
            pub.publish(output)
def callback(data):
    global objFramePrefix_
    global distanceMax_
    if (len(data.objects.data) > 0):
        output = AprilTagDetectionArray()
        output.header = data.header
        for i in range(0, len(data.objects.data), 12):
            try:
                objId = data.objects.data[i]
                (trans, quat) = listener.lookupTransform(data.header.frame_id, ((objFramePrefix_ + '_') + str(int(objId))), data.header.stamp)
                tag = AprilTagDetection()
                tag.id.append(objId)
                tag.pose.pose.pose.position.x = trans[0]
                tag.pose.pose.pose.position.y = trans[1]
                tag.pose.pose.pose.position.z = trans[2]
                tag.pose.pose.pose.orientation.x = quat[0]
                tag.pose.pose.pose.orientation.y = quat[1]
                tag.pose.pose.pose.orientation.z = quat[2]
                tag.pose.pose.pose.orientation.w = quat[3]
                tag.pose.header = output.header
                if ((distanceMax_ <= 0.0) or (trans[2] < distanceMax_)):
                    output.detections.append(tag)
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
        if (len(output.detections) > 0):
            pub.publish(output)