def retreive(self):
if self.is_available():
#grab results
results = self.parent_conn.recv()
#update processTime. All processes must return a runtime through the pipe
self.processTime, self.processTimeSet = self.rolling_average(
self.processTimeSet, results[1])
#Calculate real runtime. Diagnostic purposes only
#In an ideal system the dispatch rate would equal the retreival rate. That is not the case here
actualRunTime = current_milli_time() - self.lastRetreival
self.lastRetreival = current_milli_time()
VN_logger.text(
VN_logger.PERFORMANCE,
"DispatchHz: {0} runHz: {1} ProcessHz: {2} CaptureHz: {3} Processes: {4} "
.format(round(1000.0 / (self.runTime)),
round(1000.0 / actualRunTime),
round((1000.0 / results[1])),
round(1000.0 / self.captureTime),
len(multiprocessing.active_children())))
return results
return None
def set_velocity(self, velocity_x, velocity_y, velocity_z):
#only let commands through at 10hz
if (time.time() - self.last_set_velocity) > self.vel_update_rate:
self.last_set_velocity = time.time()
# create the SET_POSITION_TARGET_LOCAL_NED command
msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0,
0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0x01C7, # type_mask (ignore pos | ignore acc)
0,
0,
0, # x, y, z positions (not used)
velocity_x,
velocity_y,
velocity_z, # x, y, z velocity in m/s
0,
0,
0, # x, y, z acceleration (not used)
0,
0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
VN_logger.text(
VN_logger.AIRCRAFT, 'Sent Vx: {0}, Vy: {1}, Vz: {2}'.format(
velocity_x, velocity_y, velocity_z))
def image_capture_background(self, imgcap_connection):
# exit immediately if imgcap_connection is invalid
if imgcap_connection is None:
VN_logger.text(VN_logger.GENERAL, "image_capture failed because pipe is uninitialised")
return
# clear latest image
latest_image = None
while True:
# constantly get the image from the webcam
success_flag, image=self.camera.read()
# if successful overwrite our latest image
if success_flag:
latest_image = image
# check if the parent wants the image
if imgcap_connection.poll():
recv_obj = imgcap_connection.recv()
# if -1 is received we exit
if recv_obj == -1:
break
# otherwise we return the latest image
imgcap_connection.send(latest_image)
# release camera when exiting
self.camera.release()
def get_home(self, wait_for_arm=False):
#wait unitl we are armed to grab the INITIAL home position. This will lock up the program.
if (wait_for_arm and self.last_home is None):
VN_logger.text(
VN_logger.GENERAL,
'Waiting for intial home lock: Requires armed status....')
while (self.vehicle.armed == False):
time.sleep(0.3)
VN_logger.text(VN_logger.GENERAL, 'Got valid intial home location')
if (time.time() - self.last_home_call > self.home_update_rate):
self.last_home_call = time.time()
# download the vehicle waypoints
mission_cmds = self.vehicle.commands
mission_cmds.download()
mission_cmds.wait_valid()
# get the home lat and lon
home_lat = mission_cmds[0].x
home_lon = mission_cmds[0].y
self.last_home = Location(home_lat, home_lon, 0)
return self.last_home
def image_capture_background(self, imgcap_connection):
# exit immediately if imgcap_connection is invalid
if imgcap_connection is None:
VN_logger.text(
VN_logger.GENERAL,
"image_capture failed because pipe is uninitialised")
return
# clear latest image
latest_image = None
while True:
# constantly get the image from the webcam
success_flag, image = self.camera.read()
# if successful overwrite our latest image
if success_flag:
latest_image = image
# check if the parent wants the image
if imgcap_connection.poll():
recv_obj = imgcap_connection.recv()
# if -1 is received we exit
if recv_obj == -1:
break
# otherwise we return the latest image
imgcap_connection.send(latest_image)
# release camera when exiting
self.camera.release()
def report_landing_target(self, angle_x, angle_y, distance):
#only let commands through at 33hz
if(time.time() - self.last_report_landing_target) > self.landing_update_rate:
self.last_report_landing_target_ = time.time()
# create the LANDING TARGET message
msg = self.vehicle.message_factory.landing_target_encode(
0, # landing target number (not used)
8, # frame
angle_x, # Angular offset x axis
angle_y, # Angular offset y axis
distance) #Distance to target
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
VN_logger.text(VN_logger.AIRCRAFT, 'Sent AngX: {0}, AngY: {1}, Dist: {2}'.format(angle_x,angle_y,distance))
def retreive(self):
if self.is_available():
#grab results
results = self.parent_conn.recv()
#update processTime. All processes must return a runtime through the pipe
self.processTime, self.processTimeSet = self.rolling_average(self.processTimeSet, results[0])
#Calculate real runtime. Diagnostic purposes only
#In an ideal system the dispatch rate would equal the retreival rate. That is not the case here
actualRunTime = current_milli_time() - self.lastRetreival
self.lastRetreival = current_milli_time()
VN_logger.text(VN_logger.PERFORMANCE, "DispatchHz: {0} runHz: {1} ProcessHz: {2} CaptureHz: {3} Processes: {4} ".format(round(1000.0/(self.runTime)), round(1000.0/actualRunTime), round((1000.0/results[0])),round(1000.0/self.captureTime), len(multiprocessing.active_children())))
return results
return None
def set_velocity(self, velocity_x, velocity_y, velocity_z):
#only let commands through at 10hz
if(time.time() - self.last_set_velocity) > self.vel_update_rate:
self.last_set_velocity = time.time()
# create the SET_POSITION_TARGET_LOCAL_NED command
msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0x01C7, # type_mask (ignore pos | ignore acc)
0, 0, 0, # x, y, z positions (not used)
velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
VN_logger.text(VN_logger.AIRCRAFT, 'Sent Vx: {0}, Vy: {1}, Vz: {2}'.format(velocity_x,velocity_y,velocity_z))
def report_landing_target(self, angle_x, angle_y, distance):
#only let commands through at 33hz
if (time.time() -
self.last_report_landing_target) > self.landing_update_rate:
self.last_report_landing_target_ = time.time()
# create the LANDING TARGET message
msg = self.vehicle.message_factory.landing_target_encode(
0, # landing target number (not used)
8, # frame
angle_x, # Angular offset x axis
angle_y, # Angular offset y axis
distance) #Distance to target
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
VN_logger.text(
VN_logger.AIRCRAFT,
'Sent AngX: {0}, AngY: {1}, Dist: {2}'.format(
angle_x, angle_y, distance))
def get_home(self, wait_for_arm = False):
#wait unitl we are armed to grab the INITIAL home position. This will lock up the program.
if(wait_for_arm and self.last_home is None):
VN_logger.text(VN_logger.GENERAL, 'Waiting for intial home lock: Requires armed status....')
while(self.vehicle.armed == False):
time.sleep(0.3)
VN_logger.text(VN_logger.GENERAL, 'Got valid intial home location')
if(time.time() - self.last_home_call > self.home_update_rate):
self.last_home_call = time.time()
# download the vehicle waypoints
mission_cmds = self.vehicle.commands
mission_cmds.download()
mission_cmds.wait_valid()
# get the home lat and lon
home_lat = mission_cmds[0].x
home_lon = mission_cmds[0].y
self.last_home = Location(home_lat,home_lon,0)
return self.last_home
def get_camera(self, index):
VN_logger.text(VN_logger.GENERAL, 'Starting Camera....')
# setup video capture
self.camera = cv2.VideoCapture(index)
self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, self.img_width)
self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, self.img_height)
# check we can connect to camera
if not self.camera.isOpened():
VN_logger.text(VN_logger.GENERAL,
"failed to open camera, exiting!")
sys.exit(0)
VN_logger.text(VN_logger.GENERAL, 'Camera Open!')
return self.camera
def get_camera(self,index):
VN_logger.text(VN_logger.GENERAL, 'Starting Camera....')
# setup video capture
self.camera = cv2.VideoCapture(index)
self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,self.img_width)
self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,self.img_height)
# check we can connect to camera
if not self.camera.isOpened():
VN_logger.text(VN_logger.GENERAL,"failed to open camera, exiting!")
sys.exit(0)
VN_logger.text(VN_logger.GENERAL, 'Camera Open!')
return self.camera
#terminate program VN_logger.text(VN_logger.GENERAL, 'Vehicle disconnected, Program Terminated') if(self.simulator == False): VN_video.stop_capture() # if starting from mavproxy if __name__ == "__builtin__": # start precision landing strat = PrecisionLand() # connect to droneapi VN_logger.text(VN_logger.GENERAL, 'Connecting to vehicle...') strat.connect() VN_logger.text(VN_logger.GENERAL, 'Vehicle connected!') # run strategy strat.run()
def run(self):
VN_logger.text(VN_logger.GENERAL, "Running {0}".format(self.name()))
# start a video capture
if self.simulator:
VN_logger.text(VN_logger.GENERAL, "Using simulator")
sim.load_target(self.target_file, self.target_size)
sim.set_target_location(veh_control.get_home(True))
# sim.set_target_location(Location(0,0,0))
else:
VN_video.start_capture(self.camera_index)
# create an image processor
# detector = CircleDetector()
detector = TopCode()
# create a queue for images
imageQueue = []
# initilize autopilot variables
location = Location(0, 0, 0)
attitude = Attitude(0, 0, 0)
while veh_control.is_connected():
"""
#kill camera for testing
if(cv2.waitKey(2) == 1113938):
self.kill_camera = not self.kill_camera
"""
# 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.always_run) or (veh_control.get_mode() == "LAND") or (veh_control.get_mode() == "RTL"):
# update how often we dispatch a command
VN_dispatcher.calculate_dispatch_schedule()
# update simulator
if self.simulator:
# get info from autopilot
location = veh_control.get_location()
attitude = veh_control.get_attitude()
sim.refresh_simulator(location, attitude)
# grab an image
frame = None
capStart = current_milli_time()
if self.simulator:
frame = sim.get_frame()
else:
frame = VN_video.get_image()
capStop = current_milli_time()
"""
if(self.kill_camera):
frame[:] = (0,255,0)
"""
# update capture time
VN_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 VN_dispatcher.is_ready():
# queue the image for later use: displaying image, overlays, recording
imageQueue.append((frame, self.frames_captured))
# the function must be run directly from the class
VN_dispatcher.dispatch(target=detector.analyze_frame_async, args=(frame, self.frames_captured))
self.frames_captured += 1
# retreive results
if VN_dispatcher.is_available():
# results of image processor
results = VN_dispatcher.retreive()
# get image that was passed with the image processor
for f in imageQueue:
img, frame_id = f
if results[0] == frame_id:
imageQueue.remove(f)
break
VN_logger.text(VN_logger.GENERAL, "Frame {0}".format(frame_id))
# overlay gui
# rend_Image = gui.add_target_highlights(img, results[3])
rend_Image = gui.add_ring_highlights(img, results[4])
# show/record images
VN_logger.image(VN_logger.RAW, img)
VN_logger.image(VN_logger.GUI, rend_Image)
# display/log data
VN_logger.text(
VN_logger.ALGORITHM,
"RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}".format(
results[1], results[2], results[3], results[4]
),
)
VN_logger.text(VN_logger.DEBUG, "Rings detected: {0}".format(len(results[4])))
# send results if we found the landing pad
if results[2] is not None:
# shift origin to center of the image
x_pixel = results[2][0] - (self.camera_width / 2.0)
y_pixel = results[2][1] - (
self.camera_height / 2.0
) # y-axis is inverted??? Works with arducopter
# convert target location to angular radians
x_angle = x_pixel * (self.camera_hfov / self.camera_width) * (math.pi / 180.0)
y_angle = y_pixel * (self.camera_vfov / self.camera_height) * (math.pi / 180.0)
# send commands to autopilot
veh_control.report_landing_target(x_angle, y_angle, results[3], 0, 0)
else:
VN_logger.text(VN_logger.GENERAL, "Not in landing mode")
# terminate program
VN_logger.text(VN_logger.GENERAL, "Vehicle disconnected, Program Terminated")
if self.simulator == False:
VN_video.stop_capture()
) # y-axis is inverted??? Works with arducopter
# convert target location to angular radians
x_angle = x_pixel * (self.camera_hfov / self.camera_width) * (math.pi / 180.0)
y_angle = y_pixel * (self.camera_vfov / self.camera_height) * (math.pi / 180.0)
# send commands to autopilot
veh_control.report_landing_target(x_angle, y_angle, results[3], 0, 0)
else:
VN_logger.text(VN_logger.GENERAL, "Not in landing mode")
# terminate program
VN_logger.text(VN_logger.GENERAL, "Vehicle disconnected, Program Terminated")
if self.simulator == False:
VN_video.stop_capture()
# if starting from mavproxy
if __name__ == "__builtin__":
# start precision landing
strat = PrecisionLand()
# connect to droneapi
VN_logger.text(VN_logger.GENERAL, "Connecting to vehicle...")
strat.connect()
VN_logger.text(VN_logger.GENERAL, "Vehicle connected!")
# run strategy
strat.run()
y_angle = y_pixel * (self.camera_vfov /
self.camera_height) * (math.pi /
180.0)
#send commands to autopilot
veh_control.report_landing_target(
x_angle, y_angle, results[3], 0, 0)
else:
VN_logger.text(VN_logger.GENERAL, 'Not in landing mode')
#terminate program
VN_logger.text(VN_logger.GENERAL,
'Vehicle disconnected, Program Terminated')
if (self.simulator == False):
VN_video.stop_capture()
# if starting from mavproxy
if __name__ == "__builtin__":
# start precision landing
strat = PrecisionLand()
# connect to droneapi
VN_logger.text(VN_logger.GENERAL, 'Connecting to vehicle...')
strat.connect()
VN_logger.text(VN_logger.GENERAL, 'Vehicle connected!')
# run strategy
strat.run()
def run(self):
VN_logger.text(VN_logger.GENERAL, 'Running {0}'.format(self.name()))
#start a video capture
if (self.simulator):
VN_logger.text(VN_logger.GENERAL, 'Using simulator')
sim.load_target(self.target_file, self.target_size)
sim.set_target_location(veh_control.get_home(True))
#sim.set_target_location(Location(0,0,0))
else:
VN_video.start_capture(self.camera_index)
#create an image processor
#detector = CircleDetector()
detector = TopCode()
#create a queue for images
imageQueue = []
#initilize autopilot variables
location = Location(0, 0, 0)
attitude = Attitude(0, 0, 0)
while veh_control.is_connected():
'''
#kill camera for testing
if(cv2.waitKey(2) == 1113938):
self.kill_camera = not self.kill_camera
'''
#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.always_run) or (veh_control.get_mode()
== "LAND") or (veh_control.get_mode()
== "RTL"):
#update how often we dispatch a command
VN_dispatcher.calculate_dispatch_schedule()
#update simulator
if (self.simulator):
#get info from autopilot
location = veh_control.get_location()
attitude = veh_control.get_attitude()
sim.refresh_simulator(location, attitude)
# grab an image
frame = None
capStart = current_milli_time()
if (self.simulator):
frame = sim.get_frame()
else:
frame = VN_video.get_image()
capStop = current_milli_time()
'''
if(self.kill_camera):
frame[:] = (0,255,0)
'''
#update capture time
VN_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 VN_dispatcher.is_ready():
#queue the image for later use: displaying image, overlays, recording
imageQueue.append((frame, self.frames_captured))
#the function must be run directly from the class
VN_dispatcher.dispatch(target=detector.analyze_frame_async,
args=(frame, self.frames_captured))
self.frames_captured += 1
#retreive results
if VN_dispatcher.is_available():
#results of image processor
results = VN_dispatcher.retreive()
# get image that was passed with the image processor
for f in imageQueue:
img, frame_id = f
if results[0] == frame_id:
imageQueue.remove(f)
break
VN_logger.text(VN_logger.GENERAL,
'Frame {0}'.format(frame_id))
#overlay gui
#rend_Image = gui.add_target_highlights(img, results[3])
rend_Image = gui.add_ring_highlights(img, results[4])
#show/record images
VN_logger.image(VN_logger.RAW, img)
VN_logger.image(VN_logger.GUI, rend_Image)
#display/log data
VN_logger.text(
VN_logger.ALGORITHM,
'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'
.format(results[1], results[2], results[3],
results[4]))
VN_logger.text(
VN_logger.DEBUG,
'Rings detected: {0}'.format(len(results[4])))
#send results if we found the landing pad
if (results[2] is not None):
#shift origin to center of the image
x_pixel = results[2][0] - (self.camera_width / 2.0)
y_pixel = results[2][1] - (
self.camera_height / 2.0
) #y-axis is inverted??? Works with arducopter
#convert target location to angular radians
x_angle = x_pixel * (self.camera_hfov /
self.camera_width) * (math.pi /
180.0)
y_angle = y_pixel * (self.camera_vfov /
self.camera_height) * (math.pi /
180.0)
#send commands to autopilot
veh_control.report_landing_target(
x_angle, y_angle, results[3], 0, 0)
else:
VN_logger.text(VN_logger.GENERAL, 'Not in landing mode')
#terminate program
VN_logger.text(VN_logger.GENERAL,
'Vehicle disconnected, Program Terminated')
if (self.simulator == False):
VN_video.stop_capture()