def get_transformations(self, from_frame, to_tags, n=NUM_REDUNDANT_DETECTIONS): # define task def f(mapper, from_frame, to_tag, n): tfs = [] to_frame = 'Tag%s' % str(to_tag) listener = tf.TransformListener() listener.waitForTransform(from_frame, to_frame, rospy.Time(), rospy.Duration(4.0)) while (not mapper.is_shutdown) and (len(tfs) < n): try: t = listener.getLatestCommonTime(from_frame, to_frame) trans, rot = listener.lookupTransform(from_frame, to_frame, t) if self.is_valid_transformation(trans, rot): tfs.append(TF( trans=np.array(trans), rot=np.array(rot) )) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: mapper.log(e, 'warn') time.sleep(0.1) return from_frame, to_tag, tfs # create a pool of threads p = Pool(NUM_THREADS) for to_tag in to_tags: self.log('Looking up transformations [%s] -> [%s]' % (from_frame, to_tag)) p.enqueue(f, self, from_frame, to_tag, n) # spin workers p.run() # wait for results tfs = dict() for f0, f, ts in p.iterate_results(): self.log('Looked up %d transformations [%s] -> [%s]' % (len(ts), f0, f)) tfs[f] = extract_transformations( ts, self.parameters['~snap_position'], self.parameters['~snap_orientation'], self.parameters['~orientation_resolution_deg'] ) # --- return tfs
def get_transformations(self, from_frames, to_frame, n=NUM_REDUNDANT_DETECTIONS): # define task def f(mapper, from_frame, to_frame, n): tfs = [] listener = tf.TransformListener() while (not mapper.is_shutdown) and (len(tfs) < n): try: t = listener.getLatestCommonTime(from_frame, to_frame) trans, rot = listener.lookupTransform(from_frame, to_frame, t) tfs.append(TF( trans=np.array(trans), rot=np.array(rot) )) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue time.sleep(0.2) return from_frame, to_frame, tfs # create a pool of threads p = Pool(NUM_THREADS) for from_frame in from_frames: self.log('Looking up transformations [%s] -> [%s]' % (from_frame, to_frame)) p.enqueue(f, self, from_frame, to_frame, n) # spin workers p.run() # wait for results tfs = dict() for f, f0, ts in p.iterate_results(): self.log('Looked up %d transformations [%s] -> [%s]' % (len(ts), f, f0)) tfs[f] = extract_transformations( ts, self.parameters['~snap_position'], self.parameters['~snap_orientation'], self.parameters['~orientation_resolution_deg'] ) # --- return tfs