def get_avg_hydra_tfm(lr, n_avg, sleeper):
"""
Returns averaged hydra tfms.
"""
if not isinstance(lr, list): lr = [lr]
avg_tfms = {h:[] for h in lr}
j = 0
while j < n_avg:
hydra_tfms = gmt.get_hydra_transforms('hydra_base', lr)
if not hydra_tfms:
continue
for h in hydra_tfms:
avg_tfms[h].append(hydra_tfms[h])
j += 1
sleeper.sleep()
for h in lr: avg_tfms[h] = avg_transform(avg_tfms[h])
return avg_tfms
def process_observation (self, n_avg=5):
self.iterations += 1
raw_input(colorize("Iteration %d: Press return when ready to capture transforms."%self.iterations, "red", True))
sleeper = rospy.Rate(30)
avg_tfms = {}
pot_avg = 0.0
j = 0
n_attempts_max = n_avg*2
n_attempts_effective = 0;
while j < n_attempts_max:
j += 1
tfms = {}
# F**k the frames bullshit
ar_tfm = self.cameras.get_ar_markers(markers=self.ar_markers, camera=1)
hyd_tfm = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras = self.hydras)
pot_angle = gmt.get_pot_angle(self.lr)
if not ar_tfm or (not hyd_tfm and self.hydras):
if not ar_tfm:
yellowprint('Could not find required ar markers from '+str(self.ar_markers))
else:
yellowprint('Could not find required hydra transforms from '+str(self.hydras))
continue
pot_avg += pot_angle
tfms.update(ar_tfm)
tfms.update(hyd_tfm)
blueprint('\tGetting averaging transform: %d of %d ...'%(n_attempts_effective, n_avg-1))
n_attempts_effective += 1
# The angle relevant for each finger is only half the angle.
for marker in tfms:
if marker not in avg_tfms:
avg_tfms[marker] = []
avg_tfms[marker].append(tfms[marker])
if n_attempts_effective == n_avg:
break
sleeper.sleep()
if n_attempts_effective < n_avg:
return False;
pot_avg /= n_avg
greenprint("Average angle found: %f"%pot_avg)
for marker in avg_tfms:
avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])
## only one marker on gripper now
#if len(avg_tfms) == 1:
# yellowprint('Found %i marker only. Not enough to update.'%avg_tfms.keys()[0])
update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
return True
