Exemplo n.º 1
0
	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
Exemplo n.º 2
0
	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
Exemplo n.º 3
0
	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()
Exemplo n.º 4
0
	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)