def pose_sub(pose_msg):
	poses.append(pose_quat_to_euler(pose_msg))
				 
	if len(poses) == poses.maxlen:
		filtered = np.median(np.array(poses), 0)
		pose_pub.publish(
			PoseStamped(
				header = pose_msg.header,
				pose   = pose_euler_to_quat(filtered)
			)
		)
Beispiel #2
0
 def pose_sub(self, pose_msg):
     pose = pose_quat_to_euler(pose_msg)
     if self.ready:
         obs = [self.xFun(tan(pose[5])), self.yFun(tan(pose[4]))]
         x, y = [int(n) for n in self.filter.observation(obs)]
         if self.do_move_mouse:
             move_mouse(x, y)
         mouse_msg = PointStamped()
         mouse_msg.header.stamp = rospy.Time.now()
         mouse_msg.point.x = x
         mouse_msg.point.y = y
         mouse_msg.point.z = 0
         self.mouse_pub.publish(mouse_msg)
     else:
         with self.pose_lock:
             self.last_pose = pose
Beispiel #3
0
	def pose_sub(self, pose_msg):
		pose = pose_quat_to_euler(pose_msg)
		if self.ready:
			obs = [self.xFun(tan(pose[5])), self.yFun(tan(pose[4]))]
			x, y = [int(n) for n in self.filter.observation(obs)]
			if self.do_move_mouse:
				move_mouse(x, y)
			mouse_msg = PointStamped()
			mouse_msg.header.stamp = rospy.Time.now()
			mouse_msg.point.x = x
			mouse_msg.point.y = y
			mouse_msg.point.z = 0
			self.mouse_pub.publish(mouse_msg)
		else:
			with self.pose_lock:
				self.last_pose = pose