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