def run(self): """ Publishes the pot angles. """ global finished while True and not finished: self.langle_pub.publish(gmt.get_pot_angle('l')) self.rangle_pub.publish(gmt.get_pot_angle('r')) time.sleep(1 / self.rate) yellowprint("PublishPotAngles thread has finished running.")
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 = {} j = 0 thresh = n_avg*2 pot_avg = 0.0 while j < n_avg: blueprint('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1)) tfms = {} # F**k the frames bullshit ar_tfm = self.cameras.get_ar_markers(markers=self.ar_markers) hyd_tfm = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras = self.hydras) pot_angle = gmt.get_pot_angle() if not ar_tfm or (not hyd_tfm and self.hydras): yellowprint('Could not find all required transforms.') thresh -= 1 if thresh == 0: return False continue pot_avg += pot_angle j += 1 tfms.update(ar_tfm) tfms.update(hyd_tfm) # 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]) sleeper.sleep() 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]) update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle) return True
def get_specific_tooltip_tfm (self, m_type='AR'): """ Not used in any places Get the estimate of the tool tip from either ar_markers or hydras. """ if m_type not in ['AR', 'hydra']: redprint('Not sure what estimate %s gives.' % m_type) return if m_type == 'AR': if len(self.ar_markers) == 0: redprint('No AR markers to give you an estimate.') return marker_tfms = self.cameras.get_ar_markers(markers=self.ar_markers) else: if len(self.hydra_markers) == 0: redprint('No hydras to give you an estimate.') return marker_tfms = gmt.get_hydra_transforms(self.parent_frame, self.hydra_markers) theta = gmt.get_pot_angle(self.lr) return self.get_tooltip_transform(marker_tfms, theta)
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)) avg_tfms = {} for j in xrange(n_avg): print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True) tfms = {} tfms.update(gmt.get_ar_markers_from_cameras(self.cameras, parent_frame=self.parent_frame, markers=self.ar_markers)) tfms.update(gmt.get_hydra_transforms(parent_frame=parent_frame, hydras = self.hydras)) pot_angle = gmt.get_pot_angle() for marker in tfms: if marker not in avg_tfms: avg_tfms[marker] = [] avg_tfms[marker].append(tfms[marker]) for marker in avg_tfms: avg_tfms[marker] = utils.avg_transform(avg_tfms[marker]) update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
def get_obs_new_marker(self, marker, n_tfm=10, n_avg=10): """ Store a bunch of readings seen for new marker being added. Returns a list of dicts each of which has marker transforms found and potentiometer angles. """ all_obs = [] i = 0 thresh = n_avg * 2 sleeper = rospy.Rate(30) while i < n_tfm: raw_input(colorize("Getting transform %i out of %i. Hit return when ready." % (i, n_tfm), 'yellow', True)) j = 0 j_th = 0 pot_angle = 0 found = True avg_tfms = [] while j < n_avg: blueprint("Averaging transform %i out of %i." % (j, n_avg)) ar_tfms = self.cameras.get_ar_markers(); hyd_tfms = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras=None); curr_angle = gmt.get_pot_angle(self.lr) if not ar_tfms and not hyd_tfms: yellowprint("Could not find any transform.") j_th += 1 if j_th < thresh: continue else: found = False break tfms = ar_tfms pot_angle += curr_angle tfms.update(hyd_tfms) avg_tfms.append(tfms) j += 1 sleeper.sleep() if found is False: yellowprint("Something went wrong; try again.") continue tfms_found = {} for tfms in avg_tfms: for m in tfms: if m not in tfms_found: tfms_found[m] = [] tfms_found[m].append(tfms[m]) if marker not in tfms_found: yellowprint("Could not find marker to be added; try again.") continue for m in tfms_found: tfms_found[m] = utils.avg_transform(tfms_found[m]) pot_angle = pot_angle / n_avg all_obs.append({'tfms':tfms_found, 'pot_angle':pot_angle}) i += 1 return all_obs
def get_all_transforms(self, parent_frame, diff_cam=False): """ From marker transforms given in parent_frame, get all transforms of all markers on gripper. """ if self.cameras is None: redprint("Cameras not initialized. Could not get transforms.") return ar_tfms = self.cameras.get_ar_markers() if diff_cam: ar_tfms_cam = {} for i in range(self.cameras.num_cameras): ar_tfms_cam[i] = self.cameras.get_ar_markers(camera=i, parent_frame=True) hyd_tfms = gmt.get_hydra_transforms(self.parent_frame, None) theta = gmt.get_pot_angle(self.lr) marker_tfms = ar_tfms marker_tfms.update(hyd_tfms) ret_tfms = [] cor_avg_tfms = [] cor_hyd_avg = [] cor_ar_avg = [] ar_found = False hyd_found = False for m, tfm in marker_tfms.items(): if m in self.ar_markers: c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta)) cor_ar_avg.append(c_tfm) cor_avg_tfms.append(c_tfm) ar_found = True elif m in self.hydra_markers: c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta)) cor_hyd_avg.append(c_tfm) cor_avg_tfms.append(c_tfm) hyd_found = True if diff_cam: cor_ar_cams = {} cam_found = {i:False for i in ar_tfms_cam} for i in ar_tfms_cam: for m, tfm in ar_tfms_cam[i].items(): if m in self.ar_markers: c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta)) if i not in cor_ar_cams: cor_ar_cams[i] = [] cam_found[i] = True cor_ar_cams[i].append(c_tfm) if cam_found[i]: cor_ar_cams[i] = utils.avg_transform(cor_ar_cams[i]) if len(cor_avg_tfms) == 0: return ret_tfms cor_tfm = utils.avg_transform(cor_avg_tfms) cor_h_tfm = utils.avg_transform(cor_hyd_avg) cor_a_tfm = utils.avg_transform(cor_ar_avg) ret_tfms.append({'parent':parent_frame, 'child':'%sgripper_%s' % (self.lr, 'cor'), 'tfm':cor_tfm}) for m in self.allmarkers: if m != 'cor' and m != 'tool_tip': tfm = self.get_rel_transform('cor', m, theta) ret_tfms.append({'parent':parent_frame, 'child':'%sgripper_%s' % (self.lr, m), 'tfm':cor_tfm.dot(tfm)}) if self.tooltip_calculated: tfm = self.get_rel_transform('cor', 'tool_tip', theta) ret_tfms.append({'parent':parent_frame, 'child':'%sgripper_tooltip' % self.lr, 'tfm':cor_tfm.dot(tfm)}) if hyd_found: ret_tfms.append({'parent':parent_frame, 'child':'%sgripper_tooltip_hydra' % self.lr, 'tfm':cor_h_tfm.dot(tfm)}) if ar_found: ret_tfms.append({'parent':parent_frame, 'child':'%sgripper_tooltip_ar' % self.lr, 'tfm':cor_a_tfm.dot(tfm)}) if diff_cam: for i, cor_tfm in cor_ar_cams.items(): ret_tfms.append({'parent':parent_frame, 'child':'%sgripper_tooltip_camera%i' % (self.lr, i + 1), 'tfm':cor_tfm.dot(tfm)}) return ret_tfms