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)