def __init__(self, mc): self.mc = mc self.calculator = DistanceCalculator() myMotionCommander = MotionCommander(self.mc) self.cap = cv2.VideoCapture("video_localizacao.webm") CenterLimitH = [305, 335] self.savedX = 50 self.savedY = 0 self.savedAlpha = 180 self.savedBeta = 90 self.map_view = MapView()
def __init__(self, X, P): ''' self.X = posição do robo e dos marcadores self.P = covariancia dos robos e marcadores self.markers = marcadores descobertos e salvos ''' self.X = X #Estado inicial self.X_pred = self.X.copy() self.P = P self.calculator = DistanceCalculator() self.markers = [] #lista de marcadores salvos self.mapView = MapView()
alpha_rad = self.calculator.alpha * math.pi / 180 # mudança de coordenadas para o drone frente = delta_x * math.cos(alpha_rad) + delta_y * math.sin(alpha_rad) esquerda = -delta_x * math.sin(alpha_rad) + delta_y * math.cos( alpha_rad) print(frente, esquerda, delta_alpha) mc.move_distance(frente / 100, esquerda / 100, 0, velocity=0.3) time.sleep(1) if (delta_alpha > 0): mc.turn_left(delta_alpha) if (delta_alpha < 0): mc.turn_right(-delta_alpha) time.sleep(1) if __name__ == '__main__': trajetoria = Trajetoria(None) mapView = MapView() thread = threading.Thread(target=trajetoria.video) thread.setDaemon(True) thread.start() time.sleep(3) while (True): cv2.imshow("frame", trajetoria.frame) cv2.imshow("mapa", trajetoria.map_view.map) if cv2.waitKey(100) & 0xFF == ord('q'): break