def __init__(self, device_id, ils_cursor, ils_db):
self.sensorfusion = madgwick.Madgwick(0.0)
self.device_name = device_id
self.ils_cursor = ils_cursor
self.ils_db = ils_db
self.device_data = {
"s_1": [],
"s_2": [],
"s_3": [],
"location": [],
"pos": self.default_position,
"speed": [0.0, 0.0],
"var": self.default_variance
}
weights = np.array([1.0]*N)
# Create a black image, a window and bind the function to window
img = np.zeros((HEIGHT,WIDTH,3), np.uint8)
cv2.namedWindow(WINDOW_NAME)
# cv2.setMouseCallback(WINDOW_NAME,mouseCallback)
center = np.array([[-10,-10]])
# position = np.array([[0, 0]])
trajectory = np.zeros(shape=(0,2))
robot_pos = np.zeros(shape=(0,2))
DELAY_MSEC = 50
# creating the madgwick object for access IMU processing functions.
sensorfusion = madgwick.Madgwick(0.5)
# functions need for visualize results
def drawLines(img, points, r, g, b):
cv2.polylines(img, [np.int32(points)], isClosed=False, color=(r, g, b))
def drawCross(img, center, r, g, b):
d = 5
t = 2
LINE_AA = cv2.LINE_AA if cv2.__version__[0] >= '3' else cv2.CV_AA
color = (r, g, b)
ctrx = center[0,0]
ctry = center[0,1]
cv2.line(img, (ctrx - d, ctry - d), (ctrx + d, ctry + d), color, t, LINE_AA)
cv2.line(img, (ctrx + d, ctry - d), (ctrx - d, ctry + d), color, t, LINE_AA)