78 lines
2.2 KiB
Python
78 lines
2.2 KiB
Python
import math
|
|
|
|
from Constant import position_accuracy
|
|
class Position:
|
|
def __init__(self):
|
|
self.X = 0.0
|
|
self.Y = 0.0
|
|
self.Z = 0.0
|
|
self.U = 0.0
|
|
self.V = 0.0
|
|
self.W = 0.0
|
|
|
|
def compare(self,position):
|
|
distance = math.sqrt((self.X-position.X)**2+
|
|
(self.Y-position.Y)**2+
|
|
(self.Z - position.Z)**2+
|
|
(self.U - position.U)**2+
|
|
(self.V - position.V)**2+
|
|
(self.W - position.W) ** 2)
|
|
if distance<=position_accuracy:
|
|
return True
|
|
else:
|
|
return False
|
|
|
|
# def compare(self,position):
|
|
# if self.X-position.X<position_accuracy and \
|
|
# self.Y-position.Y<position_accuracy and \
|
|
# self.Z - position.Z < position_accuracy and \
|
|
# self.U - position.U < position_accuracy and \
|
|
# self.V - position.V < position_accuracy and \
|
|
# self.W - position.W < position_accuracy:
|
|
# return True
|
|
# else:
|
|
# return False
|
|
# pass
|
|
|
|
def is_error_angel_move(self,position,interval):
|
|
if self.X - position.X >= interval or \
|
|
self.Y - position.Y >= interval or \
|
|
self.Z - position.Z >= interval or \
|
|
self.U - position.U >= interval or \
|
|
self.V - position.V >= interval or \
|
|
self.W - position.W >= interval:
|
|
return True
|
|
else:
|
|
return False
|
|
pass
|
|
"""
|
|
摄像头识别位置和角度
|
|
"""
|
|
class Detection_Position(Position):
|
|
def __init__(self):
|
|
super().__init__()
|
|
|
|
def init_position(self,X,Y,Z,U,V,W):
|
|
self.X = X
|
|
self.Y = Y
|
|
self.Z = Z
|
|
self.U = U
|
|
self.V = V
|
|
self.W = W
|
|
|
|
|
|
class Real_Position(Position):
|
|
def __init__(self):
|
|
super().__init__()
|
|
|
|
def init_position(self, X, Y, Z, U, V, W):
|
|
self.X = X
|
|
self.Y = Y
|
|
self.Z = Z
|
|
self.U = U
|
|
self.V = V
|
|
self.W = W
|
|
return self
|
|
|
|
def to_string(self):
|
|
return "X:{:.3f},Y:{:.3f},Z:{:.3f},U:{:.3f},V:{:.3f},W:{:.3f}".format(self.X,self.Y,self.Z,self.U,self.V,self.W) |