def __init__(self): self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/drone/camera1/image_raw",Image,self.callback) self.train_descriptor=[] self.score = [0,0,0,0,0,0,0,0,0,0] self.alpha = 0 self._pid = PID(0.001,0,0) self.whichone = 'none' self.gorev = 1.11 self.pist_1=[4,4] self.pist_2=[-4,4] self.pist_3=[-4,-4] self.pist_4=[4,-4] self.pist_5=[0,0] self.stm_pist = None self.odtu_pist=None self.ort_pist=None self.landing_pist=None for each in range(5): self.thresh, self.dedet, self.isim, self.sift, train_descriptor_a, self.surf, self.bf = baslat(each) self.train_descriptor.append(train_descriptor_a)
pist_2 = [-1, 1] pist_3 = [-1, -1] pist_4 = [1, -1] pist_5 = [0, 0] pid = PID(0.001, 0, 0) train_descriptor = [] score = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0] alpha = 0 stm_pist = None odtu_pist = None ort_pist = None landing_pist = None for each in range(5): thresh, dedet, isim, sift, train_descriptor_a, surf, bf = baslat(each) train_descriptor.append(train_descriptor_a) parser = argparse.ArgumentParser( description='Control Copter and send commands in GUIDED mode ') parser.add_argument( '--connect', help= "Vehicle connection target string. If not specified, SITL automatically started and used." ) args = parser.parse_args() connection_string = '/dev/ttyACM0' sitl = None if not connection_string:
from cv_bridge import CvBridge, CvBridgeError import matplotlib matplotlib.use('Agg') import matplotlib.pyplot as plt import numpy as np import time from ip import baslat,islet, hangisi from goruntu_isle1 import baslat,hangisi from dronekit import connect, VehicleMode, LocationGlobal, LocationGlobalRelative from pymavlink import mavutil # Needed for command message definitions import math import argparse a=5 gorev=1.1 thresh, dedet, isim, sift, train_descriptor, surf, bf = baslat(0) parser = argparse.ArgumentParser(description='Control Copter and send commands in GUIDED mode ') parser.add_argument('--connect',help="Vehicle connection target string. If not specified, SITL automatically started and used.") args = parser.parse_args() connection_string = '127.0.0.1:14550' sitl = None #Start SITL if no connection string specified if not connection_string: import dronekit_sitl sitl = dronekit_sitl.start_default() connection_string = sitl.connection_string() # Connect to the Vehicle print('Connecting to vehicle on: %s' % connection_string) vehicle = connect(connection_string, wait_ready=True)