def __init__(self, name): self.name = name self.ref_frame = '/BASE' self.orig_frame = '/BUMBLEBEE_LEFT_REAL' self.corrected_frame = '/BUMBLEBEE_LEFT' self.listener = tf.TransformListener(True, rospy.Duration(100.0)) self.broadcaster = tf.TransformBroadcaster() self.cache_dir = roslib.packages.get_pkg_subdir('arm_fiducial_cal', 'cache', required=False) self.correcter = FCTransformCorrecter(self.cache_dir) self.prev_base_H_cam = None self.prev_base_H_ccam = None
class FCCorrectionNode: def __init__(self, name): self.name = name self.ref_frame = '/BASE' self.orig_frame = '/BUMBLEBEE_LEFT_REAL' self.corrected_frame = '/BUMBLEBEE_LEFT' self.listener = tf.TransformListener(True, rospy.Duration(100.0)) self.broadcaster = tf.TransformBroadcaster() self.cache_dir = roslib.packages.get_pkg_subdir('arm_fiducial_cal', 'cache', required=False) self.correcter = FCTransformCorrecter(self.cache_dir) self.prev_base_H_cam = None self.prev_base_H_ccam = None def publish_one_transform(self): t = rospy.Time.now() try: self.listener.waitForTransform( self.ref_frame, self.orig_frame, t, rospy.Duration(1.0)) (trans, rot) = self.listener.lookupTransform( self.ref_frame, self.orig_frame, t) base_H_cam = np.matrix(self.listener.fromTranslationRotation(trans, rot)) except tf.Exception as ex: rospy.logerr('%s: Unable to get transform' % self.name) return # cache the last corrected transform to save on computation if (not self.prev_base_H_cam == None) and np.sum(np.abs(self.prev_base_H_cam - base_H_cam)) < 1e-10: base_H_ccam = self.prev_base_H_ccam else: base_H_ccam = self.correcter.get_corrected_transform(base_H_cam) self.prev_base_H_cam = base_H_cam self.prev_base_H_ccam = base_H_ccam transform_msg = ros_util.matrix_to_transform(base_H_ccam) trans = transform_msg.translation rot = transform_msg.rotation # broadcast the corrected transform self.broadcaster.sendTransform( (trans.x, trans.y, trans.z), (rot.x, rot.y, rot.z, rot.w), t, self.corrected_frame, self.ref_frame) def run(self): # let a few tf messages come in rospy.sleep(0.1) # loop, publishing corrected transforms while not rospy.is_shutdown(): self.publish_one_transform() rospy.sleep(0.03)