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