def determine_obj_frame(self): if self.detected_artags != None: eng_poses = [] ar_tfs = [] for da in self.detected_artags.markers: ar_pose = th.aruco_to_pose_stamp(da) eng_pose = self.get_engine_pose_from_ps(ar_pose) eng_poses.append(eng_pose) ar_tfs.append(th.pose_stamp_to_tf(eng_pose)) ar_tf_sum = np.zeros((4,4)) for tf in ar_tfs: ar_tf_sum = ar_tf_sum + tf/len(ar_tfs) engine_frame_combined = ar_tf_sum tran = transformations.translation_from_matrix(engine_frame_combined), eul = transformations.euler_from_matrix(engine_frame_combined) self.updated_engine_pose = str(tran[0][0]) + ' ' + str(tran[0][1]) + ' ' + str(tran[0][2]) + ' ' + str(eul[0]) + ' ' + str(eul[1]) + ' ' + str(eul[2]) ps = th.tf_to_pose_stamp(engine_frame_combined, 'engine_frame_combined') th.broadcast_transform(ps, global_frame) return engine_frame_combined else: return None
def get_engine_pose_from_ps(self, pose_stamp): idTw = th.pose_stamp_to_tf(pose_stamp) id = str(pose_stamp.header.frame_id) if id in self.engTid: tf = np.dot(self.engTid[id], idTw) ps = th.tf_to_pose_stamp(tf, 'propose_engine_'+id) return ps th.broadcast_transform(ps, global_frame) else: print 'The following id was not found in model: ', id
def runOnce(self): self.build_transforms() #broadcast the transforms idTw from model for m in self.idTw: ps = th.tf_to_pose_stamp(self.idTw[m], m) th.broadcast_transform(ps, global_frame) #self.validate_sanity() self.determine_obj_frame() self.runPublisher()
def validate_sanity(self): for m in self.engTid: if m != target_frame: test = np.dot(self.engTid[m], self.idTw[m]) ps = th.tf_to_pose_stamp(test, target_frame+m) th.broadcast_transform(ps, global_frame)
