def main(self):
veh_control.connect(local_connect())
self.set_target_location(veh_control.get_location())
while (veh_control.is_connected()):
location = veh_control.get_location()
attitude = veh_control.get_attitude()
self.refresh_simulator(location, attitude)
frame = self.get_frame()
cv2.imshow('frame', frame)
key = cv2.waitKey(1)
print key
if key == 1113938:
veh_control.set_velocity(2, 0, 0) #forward
elif key == 1113940:
veh_control.set_velocity(-2, 0, 0) #backward
elif key == 1113937:
veh_control.set_velocity(0, -2, 0) #left
elif key == 1113939:
veh_control.set_velocity(0, 2, 0) #right
elif (key == 1048690):
yaw = math.degrees(attitude.yaw) #yaw left
veh_control.set_yaw(yaw - 5)
elif (key == 1048692):
yaw = math.degrees(attitude.yaw) #yaw right
veh_control.set_yaw(yaw + 5)
elif (key == 1048677):
veh_control.set_velocity(0, 0, -2) #down
elif (key == 1048689):
veh_control.set_velocity(0, 0, 2) #up
else:
veh_control.set_velocity(0, 0, 0) #still
def main(self):
veh_control.connect(local_connect())
self.set_target_location(veh_control.get_location())
while(veh_control.is_connected()):
location = veh_control.get_location()
attitude = veh_control.get_attitude()
self.refresh_simulator(location,attitude)
frame = self.get_frame()
cv2.imshow('frame',frame)
key = cv2.waitKey(1)
print key
if key ==1113938:
veh_control.set_velocity(2,0,0) #forward
elif key == 1113940:
veh_control.set_velocity(-2,0,0) #backward
elif key == 1113937:
veh_control.set_velocity(0,-2,0) #left
elif key ==1113939:
veh_control.set_velocity(0,2,0) #right
elif(key == 1048690):
yaw = math.degrees(attitude.yaw) #yaw left
veh_control.set_yaw(yaw - 5)
elif(key == 1048692):
yaw = math.degrees(attitude.yaw) #yaw right
veh_control.set_yaw(yaw + 5)
elif(key == 1048677):
veh_control.set_velocity(0,0,-2) #down
elif(key == 1048689):
veh_control.set_velocity(0,0,2) #up
else:
veh_control.set_velocity(0,0,0) #still
def connect(self): while(veh_control.is_connected() == False): # connect to droneapi veh_control.connect(local_connect()) self.vehicle = veh_control.get_vehicle()
def run(self):
sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
#start a video capture
'''
if(self.simulator):
sc_logger.text(sc_logger.GENERAL, 'Using simulator')
sim.set_target_location(veh_control.get_home())
#sim.set_target_location(Location(0,0,0))
else:'''
sc_video.start_capture(self.camera_index)
#camera = balloon_video.get_camera()
video_writer = balloon_video.open_video_writer()
#create an image processor
detector = CircleDetector()
#create a queue for images
imageQueue = Queue.Queue()
#create a queue for vehicle info
vehicleQueue = Queue.Queue()
while veh_control.is_connected():
#get info from autopilot
location = veh_control.get_location()
attitude = veh_control.get_attitude()
print location
print attitude
# Take each frame
#_, frame = camera.read()
#update how often we dispatch a command
sc_dispatcher.calculate_dispatch_schedule()
# grab an image
capStart = current_milli_time()
frame = sc_video.get_image()
capStop = current_milli_time()
#frame = sc_video.undisort_image(frame)
#cv2.imshow('frame',frame)
# write the frame
video_writer.write(frame)
#update capture time
sc_dispatcher.update_capture_time(capStop-capStart)
#Process image
#We schedule the process as opposed to waiting for an available core
#This brings consistancy and prevents overwriting a dead process before
#information has been grabbed from the Pipe
if sc_dispatcher.is_ready():
#queue the image for later use: displaying image, overlays, recording
imageQueue.put(frame)
#queue vehicle info for later use: position processing
vehicleQueue.put((location,attitude))
#the function must be run directly from the class
#######
sc_dispatcher.dispatch(target=balloon_finder.analyse_frame, args=(frame,))
#retreive results
if sc_dispatcher.is_available():
sc_logger.text(sc_logger.GENERAL, 'Frame {0}'.format(self.frame_count))
self.frame_count += 1
#results of image processor
results = sc_dispatcher.retreive()
# get image that was passed with the image processor
img = imageQueue.get()
#get vehicle position that was passed with the image processor
location, attitude = vehicleQueue.get()
#overlay gui
#rend_Image = gui.add_target_highlights(img, results[3])
#show/record images
sc_logger.image(sc_logger.RAW, img)
#sc_logger.image(sc_logger.GUI, rend_Image)
#display/log data
sc_logger.text(sc_logger.ALGORITHM,'found: {0} x: {1} y: {2} radius: {3}'.format(results[0],results[1],results[2],results[3]))
def run(self):
sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
#start a video capture
if (self.simulator):
sc_logger.text(sc_logger.GENERAL, 'Using simulator')
sim.set_target_location(veh_control.get_home())
#sim.set_target_location(Location(0,0,0))
else:
sc_video.start_capture(self.camera_index)
#create an image processor
detector = CircleDetector()
#create a queue for images
imageQueue = Queue.Queue()
#create a queue for vehicle info
vehicleQueue = Queue.Queue()
while veh_control.is_connected():
'''
#kill camera for testing
if(cv2.waitKey(2) == 1113938):
self.kill_camera = not self.kill_camera
'''
#Reintialize the landing program when entering a landing mode
if veh_control.controlling_vehicle():
if not self.in_control:
if (self.allow_reset):
sc_logger.text(sc_logger.GENERAL,
'Program initialized to start state')
self.initialize_landing()
self.in_control = True
else:
self.in_control = False
#we are in the landing zone or in a landing mode and we are still running the landing program
#just because the program is running does not mean it controls the vehicle
#i.e. in the landing area but not in a landing mode
#FIXME add inside_landing_area() back to conditional
if (self.in_control or self.always_run) and self.pl_enabled:
#update how often we dispatch a command
sc_dispatcher.calculate_dispatch_schedule()
#get info from autopilot
location = veh_control.get_location()
attitude = veh_control.get_attitude()
'''
#get info from autopilot
location = Location(0.000009,0,location.alt)
attitude = Attitude(0,0,0)
'''
#update simulator
if (self.simulator):
sim.refresh_simulator(location, attitude)
# grab an image
capStart = current_milli_time()
frame = self.get_frame()
capStop = current_milli_time()
'''
if(self.kill_camera):
frame[:] = (0,255,0)
'''
#update capture time
sc_dispatcher.update_capture_time(capStop - capStart)
#Process image
#We schedule the process as opposed to waiting for an available core
#This brings consistancy and prevents overwriting a dead process before
#information has been grabbed from the Pipe
if sc_dispatcher.is_ready():
#queue the image for later use: displaying image, overlays, recording
imageQueue.put(frame)
#queue vehicle info for later use: position processing
vehicleQueue.put((location, attitude))
#the function must be run directly from the class
sc_dispatcher.dispatch(target=detector.analyze_frame,
args=(
frame,
attitude,
))
#retreive results
if sc_dispatcher.is_available():
sc_logger.text(sc_logger.GENERAL,
'Frame {0}'.format(self.frame_count))
self.frame_count += 1
#results of image processor
results = sc_dispatcher.retreive()
# get image that was passed with the image processor
img = imageQueue.get()
#get vehicle position that was passed with the image processor
location, attitude = vehicleQueue.get()
#overlay gui
rend_Image = gui.add_target_highlights(img, results[3])
#show/record images
sc_logger.image(sc_logger.RAW, img)
sc_logger.image(sc_logger.GUI, rend_Image)
#display/log data
sc_logger.text(
sc_logger.ALGORITHM,
'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'
.format(results[0], results[1], results[2],
results[3]))
sc_logger.text(sc_logger.AIRCRAFT, attitude)
sc_logger.text(sc_logger.AIRCRAFT, location)
#send commands to autopilot
self.control(results, attitude, location)
else:
if (self.pl_enabled == False):
sc_logger.text(sc_logger.GENERAL, 'Landing disabled')
else:
sc_logger.text(sc_logger.GENERAL,
'Not in landing mode or Landing Area')
#terminate program
sc_logger.text(sc_logger.GENERAL,
'Vehicle disconnected, Program Terminated')
if (self.simulator == False):
sc_video.stop_capture()
def connect(self):
while (veh_control.is_connected() == False):
# connect to droneapi
veh_control.connect(local_connect())
self.vehicle = veh_control.get_vehicle()
def run(self):
sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
#start a video capture
if(self.simulator):
sc_logger.text(sc_logger.GENERAL, 'Using simulator')
sim.set_target_location(veh_control.get_home())
#sim.set_target_location(Location(0,0,0))
else:
sc_video.start_capture(self.camera_index)
#create an image processor
detector = CircleDetector()
#create a queue for images
imageQueue = Queue.Queue()
#create a queue for vehicle info
vehicleQueue = Queue.Queue()
while veh_control.is_connected():
'''
#kill camera for testing
if(cv2.waitKey(2) == 1113938):
self.kill_camera = not self.kill_camera
'''
#Reintialize the landing program when entering a landing mode
if veh_control.controlling_vehicle():
if not self.in_control:
if(self.allow_reset):
sc_logger.text(sc_logger.GENERAL, 'Program initialized to start state')
self.initialize_landing()
self.in_control = True
else:
self.in_control = False
#we are in the landing zone or in a landing mode and we are still running the landing program
#just because the program is running does not mean it controls the vehicle
#i.e. in the landing area but not in a landing mode
#FIXME add inside_landing_area() back to conditional
if (self.in_control or self.always_run) and self.pl_enabled:
#update how often we dispatch a command
sc_dispatcher.calculate_dispatch_schedule()
#get info from autopilot
location = veh_control.get_location()
attitude = veh_control.get_attitude()
'''
#get info from autopilot
location = Location(0.000009,0,location.alt)
attitude = Attitude(0,0,0)
'''
#update simulator
if(self.simulator):
sim.refresh_simulator(location,attitude)
# grab an image
capStart = current_milli_time()
frame = self.get_frame()
capStop = current_milli_time()
'''
if(self.kill_camera):
frame[:] = (0,255,0)
'''
#update capture time
sc_dispatcher.update_capture_time(capStop-capStart)
#Process image
#We schedule the process as opposed to waiting for an available core
#This brings consistancy and prevents overwriting a dead process before
#information has been grabbed from the Pipe
if sc_dispatcher.is_ready():
#queue the image for later use: displaying image, overlays, recording
imageQueue.put(frame)
#queue vehicle info for later use: position processing
vehicleQueue.put((location,attitude))
#the function must be run directly from the class
sc_dispatcher.dispatch(target=detector.analyze_frame, args=(frame,attitude,))
#retreive results
if sc_dispatcher.is_available():
sc_logger.text(sc_logger.GENERAL, 'Frame {0}'.format(self.frame_count))
self.frame_count += 1
#results of image processor
results = sc_dispatcher.retreive()
# get image that was passed with the image processor
img = imageQueue.get()
#get vehicle position that was passed with the image processor
location, attitude = vehicleQueue.get()
'''
#overlay gui
rend_Image = gui.add_target_highlights(img, results[3])
#show/record images
sc_logger.image(sc_logger.RAW, img)
sc_logger.image(sc_logger.GUI, rend_Image)
'''
#display/log data
sc_logger.text(sc_logger.ALGORITHM,'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'.format(results[0],results[1],results[2],results[3]))
sc_logger.text(sc_logger.AIRCRAFT,attitude)
sc_logger.text(sc_logger.AIRCRAFT,location)
#send commands to autopilot
self.control(results,attitude,location)
else:
if(self.pl_enabled == False):
sc_logger.text(sc_logger.GENERAL, 'Landing disabled')
else:
sc_logger.text(sc_logger.GENERAL, 'Not in landing mode or Landing Area')
#terminate program
sc_logger.text(sc_logger.GENERAL, 'Vehicle disconnected, Program Terminated')
if(self.simulator == False):
sc_video.stop_capture()
def run(self):
sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
#start a video capture
if(self.simulator):
sc_logger.text(sc_logger.GENERAL, 'Using simulator')
#-35.362664, 149.166803 -35.362902 149.166249
sim.set_target_location(Location(-35.363134 ,149.165429,0))
#sim.set_target_location(Location(0,0,0))
else:
sc_video.start_capture(self.camera_index)
#camera = balloon_video.get_camera()
video_writer = balloon_video.open_video_writer()
#create an image processor
detector = CircleDetector()
#create a queue for images
imageQueue = Queue.Queue()
#create a queue for vehicle info
vehicleQueue = Queue.Queue()
while veh_control.is_connected():
'''
#kill camera for testing
if(cv2.waitKey(2) == 1113938):
self.kill_camera = not self.kill_camera
'''
#Reintialize the landing program when entering a landing mode
if veh_control.controlling_vehicle():
if not self.in_control:
if(self.allow_reset):
sc_logger.text(sc_logger.GENERAL, 'Program initialized to start state')
self.initialize_landing()
self.in_control = True
else:
self.in_control = False
# seach
if(self.flag==False):
# has issues
#search in area
if(self.point==1):
point1 = Location(self.point1_lat, self.point1_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point1)
self.vehicle.flush()
if(veh_control.get_location().lon>=self.point1_lon):
self.point=2
if(self.point==2):
point2 = Location(self.point2_lat, self.point2_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point2)
self.vehicle.flush()
if(veh_control.get_location().lat>=self.point1_lat):
self.point=3
if(self.point==3):
point3 = Location(self.point3_lat, self.point3_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point3)
self.vehicle.flush()
if(veh_control.get_location().lon>=self.point3_lon):
self.point=4
if(self.point==4):
point4 = Location(self.point4_lat, self.point4_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point4)
self.vehicle.flush()
if(veh_control.get_location().lat>=self.point4_lat):
self.point=1
'''
print "Going to first point..."
#30.264233, 120.118813 -35.362664, 149.166803
point1 = Location(30.264233, 120.118813, 4, is_relative=True)
self.vehicle.commands.goto(point1)
self.vehicle.flush()
'''
#we are in the landing zone or in a landing mode and we are still running the landing program
#just because the program is running does not mean it controls the vehicle
#i.e. in the landing area but not in a landing mode
#FIXME add inside_landing_area() back to conditional
if (self.in_control or self.always_run) and self.pl_enabled:
#update how often we dispatch a command
sc_dispatcher.calculate_dispatch_schedule()
#get info from autopilot
location = veh_control.get_location()
attitude = veh_control.get_attitude()
#update simulator
if(self.simulator):
sim.refresh_simulator(location,attitude)
# grab an image
capStart = current_milli_time()
frame = self.get_frame()
capStop = current_milli_time()
# write the frame
video_writer.write(frame)
#update capture time
sc_dispatcher.update_capture_time(capStop-capStart)
#Process image
#We schedule the process as opposed to waiting for an available core
#This brings consistancy and prevents overwriting a dead process before
#information has been grabbed from the Pipe
if sc_dispatcher.is_ready():
#queue the image for later use: displaying image, overlays, recording
imageQueue.put(frame)
#queue vehicle info for later use: position processing
vehicleQueue.put((location,attitude))
#the function must be run directly from the class
sc_dispatcher.dispatch(target=detector.analyze_frame, args=(frame,attitude,))
#retreive results
if sc_dispatcher.is_available():
sc_logger.text(sc_logger.GENERAL, 'Frame {0}'.format(self.frame_count))
self.frame_count += 1
#results of image processor
results = sc_dispatcher.retreive()
# get image that was passed with the image processor
img = imageQueue.get()
#get vehicle position that was passed with the image processor
location, attitude = vehicleQueue.get()
#overlay gui
rend_Image = gui.add_target_highlights(img, results[3])
#show/record images
sc_logger.image(sc_logger.RAW, img)
sc_logger.image(sc_logger.GUI, rend_Image)
#display/log data
sc_logger.text(sc_logger.ALGORITHM,'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'.format(results[0],results[1],results[2],results[3]))
sc_logger.text(sc_logger.AIRCRAFT,attitude)
sc_logger.text(sc_logger.AIRCRAFT,location)
#send commands to autopilot
if(results[2]!=-1):
self.flag = True
if(self.flag==True):
self.control(results,attitude,location)
else:
if(self.pl_enabled == False):
sc_logger.text(sc_logger.GENERAL, 'Landing disabled')
else:
sc_logger.text(sc_logger.GENERAL, 'Not in landing mode or Landing Area')
#terminate program
sc_logger.text(sc_logger.GENERAL, 'Vehicle disconnected, Program Terminated')
if(self.simulator == False):
sc_video.stop_capture()