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 build_transforms(self):
self.idTw = {} # id->tf_w -- transforms from world/odom to tag, aka poses from aruco detector
self.engTw = None
self.engTid = {} # id->tf_eng -- defines the interfeature transforms from id->engine
for m in self.engine_model.markers:
self.idTw[str(m.id)] = th.pose_stamp_to_tf(m.pose)
self.engTw = self.idTw[target_frame]
for m in self.engine_model.markers:
self.engTid[str(m.id)] = np.dot(self.engTw, transformations.inverse_matrix(self.idTw[str(m.id)]))
