def analyze_image(self):
# record time
now = time.time()
# capture vehicle position
self.vehicle_pos = PositionVector.get_from_location(
self.vehicle.location.global_relative_frame)
# capture vehicle attitude in buffer
self.att_hist.update()
# get delayed attitude from buffer
veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)
# get new image from camera
f = self.get_frame()
# look for balloon in image using blob detector
self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(f)
# add artificial horizon
balloon_finder.add_artificial_horizon(f, veh_att_delayed.roll,
veh_att_delayed.pitch)
if self.balloon_found:
# record time balloon was found
self.last_spotted_time = now
# convert x, y position to pitch and yaw direction (in radians)
self.balloon_pitch, self.balloon_heading = balloon_finder.pixels_to_direction(
xpos, ypos, veh_att_delayed.roll, veh_att_delayed.pitch,
veh_att_delayed.yaw)
self.balloon_pitch = math.radians(self.balloon_pitch)
self.balloon_pitch_top = self.balloon_pitch + balloon_video.pixels_to_angle_y(
size) # add balloon radius so we aim for top of balloon
self.balloon_heading = math.radians(self.balloon_heading)
# get distance
self.balloon_distance = get_distance_from_pixels(
size, balloon_finder.balloon_radius_expected)
# updated estimated balloon position
self.balloon_pos = balloon_finder.project_position(
self.vehicle_pos, self.balloon_pitch, self.balloon_heading,
self.balloon_distance)
# save image for debugging later
if not self.writer is None:
self.writer.write(f)
# increment frames analysed for stats
self.num_frames_analysed += 1
if self.stats_start_time == 0:
self.stats_start_time = time.time()
def main(self):
# set home to tridge's home field (absolute alt = 270)
PositionVector.set_home_location(
LocationGlobal(-35.362938, 149.165085, 0))
# calculate balloon position
fake_balloon_pos = PositionVector.get_from_location(
self.fake_balloon_location)
# vehicle attitude and position
veh_pos = PositionVector(0, 0, fake_balloon_pos.z) # at home location
veh_roll = math.radians(0) # leaned right 10 deg
veh_pitch = math.radians(0) # pitched back at 0 deg
veh_yaw = PositionVector.get_bearing(
veh_pos, fake_balloon_pos) # facing towards fake balloon
# display positions from home
print "Vehicle %s" % veh_pos
print "Balloon %s" % fake_balloon_pos
# generate simulated frame of balloon 10m north, 2m above vehicle
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
while (True):
# move vehicle towards balloon
veh_pos = veh_pos + (fake_balloon_pos - veh_pos) * 0.01
# regenerate frame
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch,
veh_yaw)
# look for balloon in image using blob detector
found_in_image, xpos, ypos, size = balloon_finder.analyse_frame(
img)
# display actual vs real distance
dist_actual = PositionVector.get_distance_xyz(
veh_pos, fake_balloon_pos)
dist_est = balloon_utils.get_distance_from_pixels(
size, balloon_finder.balloon_radius_expected)
print "Dist Est:%f Act:%f Size Est:%f Act:%f" % (
dist_est, dist_actual, size, self.last_balloon_radius)
# show image
cv2.imshow("fake balloon", img)
# wait for keypress
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
# destroy windows
cv2.destroyAllWindows()
def analyze_image(self):
# record time
now = time.time()
# capture vehicle position
self.vehicle_pos = PositionVector.get_from_location(self.vehicle.location.global_relative_frame)
# capture vehicle attitude in buffer
self.att_hist.update()
# get delayed attitude from buffer
veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)
# get new image from camera
f = self.get_frame()
# look for balloon in image using blob detector
self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(f)
# add artificial horizon
balloon_finder.add_artificial_horizon(f, veh_att_delayed.roll, veh_att_delayed.pitch)
if self.balloon_found:
# record time balloon was found
self.last_spotted_time = now
# convert x, y position to pitch and yaw direction (in radians)
self.balloon_pitch, self.balloon_heading = balloon_finder.pixels_to_direction(xpos, ypos, veh_att_delayed.roll, veh_att_delayed.pitch, veh_att_delayed.yaw)
self.balloon_pitch = math.radians(self.balloon_pitch)
self.balloon_pitch_top = self.balloon_pitch + balloon_video.pixels_to_angle_y(size) # add balloon radius so we aim for top of balloon
self.balloon_heading = math.radians(self.balloon_heading)
# get distance
self.balloon_distance = get_distance_from_pixels(size, balloon_finder.balloon_radius_expected)
# updated estimated balloon position
self.balloon_pos = balloon_finder.project_position(self.vehicle_pos, self.balloon_pitch, self.balloon_heading, self.balloon_distance)
# save image for debugging later
if not self.writer is None:
self.writer.write(f)
# increment frames analysed for stats
self.num_frames_analysed += 1
if self.stats_start_time == 0:
self.stats_start_time = time.time()
def main(self):
# set home to tridge's home field (absolute alt = 270)
PositionVector.set_home_location(Location(-35.362938,149.165085,0))
# calculate balloon position
fake_balloon_pos = PositionVector.get_from_location(self.fake_balloon_location)
# vehicle attitude and position
veh_pos = PositionVector(0,0,fake_balloon_pos.z) # at home location
veh_roll = math.radians(0) # leaned right 10 deg
veh_pitch = math.radians(0) # pitched back at 0 deg
veh_yaw = PositionVector.get_bearing(veh_pos,fake_balloon_pos) # facing towards fake balloon
# display positions from home
print "Vehicle %s" % veh_pos
print "Balloon %s" % fake_balloon_pos
# generate simulated frame of balloon 10m north, 2m above vehicle
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
while(True):
# move vehicle towards balloon
veh_pos = veh_pos + (fake_balloon_pos - veh_pos) * 0.01
# regenerate frame
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
# look for balloon in image using blob detector
found_in_image, xpos, ypos, size = balloon_finder.analyse_frame(img)
# display actual vs real distance
dist_actual = PositionVector.get_distance_xyz(veh_pos, fake_balloon_pos)
dist_est = balloon_utils.get_distance_from_pixels(size, balloon_finder.balloon_radius_expected)
print "Dist Est:%f Act:%f Size Est:%f Act:%f" % (dist_est, dist_actual, size, self.last_balloon_radius)
# show image
cv2.imshow("fake balloon", img)
# wait for keypress
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
# destroy windows
cv2.destroyAllWindows()
def run(self):
yawangle=0.0
last_mode=None
try:
#print "starting web server"
# initialise web server which will display final image
#web = Webserver(balloon_config.config.parser, (lambda : self.frame_filtered))
#web=Webserver(balloon_config.config.parser,(lambda:self.frame))
print "initialising camera"
# initialise video capture
camera = balloon_video.get_camera()
print "Ready to go!"
while(True):
#print(self.vehicle)
if self.vehicle is None:
#print('0.5')
self.vehicle=self.api.get_vehicles()[0]
#print('1')
#print('1.5')
# get a frame
_, frame = camera.read()
self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(frame)
self.frame=frame
#deltatime=time.time()-self.timelast
#self.timelast=time.time()
#print "time difference:",deltatime
#print "velocity x:",self.vehicle.velocity[0]
if self.vehicle.mode.name=="alt_hold":
last_mode="alt_hold"
print "Overriding a RC channel"
self.vehicle.channel_override = {"2" : 1400}
self.vehicle.flush()
print "Current overrides are:", self.vehicle.channel_override
print "Cancel override"
time.sleep(3)
#self.vehicle.channel_override={"2":0}
#self.vehicle.flush()
#time.sleep(1)
self.vehicle.channel_override = {"2" : 1600}
self.vehicle.flush()
time.sleep(3)
'''
self.vehicle.channel_override = {"1":1600,"2" : 1400}
self.vehicle.flush()
time.sleep(5)
print "Current overrides1 are right-forward:", self.vehicle.channel_override
self.vehicle.channel_override = {"1":1400,"2" : 1600}
self.vehicle.flush()
time.sleep(5)
print "Current overrides2 are left-afterward:", self.vehicle.channel_override
'''
else:
if last_mode=="alt_hold":
self.vehicle.channel_override={"1":0,"2":0}
self.vehicle.flush()
time.sleep(0.08)
last_mode=None
#if self.balloon_found and self.vehicle.mode.name=="GUIDED":
if self.vehicle.mode.name=="GUIDED":#only for testing speed command,response characteristics
last_mode="GUIDED"
self.vel_control(0,0)
time.sleep(3)
print"guided mode"
log_filename='posstep%s' % (time.time())
#print 'log filename',log_filename
f = open(log_filename, mode='a+')
f.write("STEP RESPONSE LOG DATA %s\n"%(time.localtime()))
f.write("time\tset_vx\tvx\tset_vy\tvy\tdeltax\tdeltay\tdeltaz\t\n")
'''
vel_setx=0.3
vel_sety=0
if self.vehicle.location is None:
print 'location is unknown in direction-x'
else:
#ref_home_location = Location(self.vehicle.location.lat,self.vehicle.location.lon,self.vehicle.location.alt)
home_lat=self.vehicle.location.lat
home_lon=self.vehicle.location.lon
home_alt=self.vehicle.location.alt
print'flying x-direction'
self.timelast=time.time()
for i in range(1, 301):
if self.vehicle.mode.name=="GUIDED":
print'index:',i
deltatime=time.time()-self.timelast
cur_lat=self.vehicle.location.lat
cur_lon=self.vehicle.location.lon
cur_alt=self.vehicle.location.alt
dlat=cur_lat-home_lat
dlon=cur_lon-home_lon
dalt=cur_alt-home_alt
deltax = dlat * pos_latlon_to_m
deltay = dlon* pos_latlon_to_m
deltaz = dalt
f.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n" % (deltatime,vel_setx,self.vehicle.velocity[0],vel_sety,self.vehicle.velocity[1],deltax,deltay,deltaz))
self.vel_control(vel_setx,vel_sety)
time.sleep(0.1)
'''
self.vel_control(0,0)
time.sleep(3)
if self.vehicle.location is None:
print 'location is unknown in direction-y'
else:
#ref_home_location = Location(self.vehicle.location.lat,self.vehicle.location.lon,self.vehicle.location.alt)
home_lat=self.vehicle.location.lat
home_lon=self.vehicle.location.lon
home_alt=self.vehicle.location.alt
print'flying y-direction'
self.timelast=time.time()
vel_setx=0
vel_sety=0.3
for i in range(1, 201):
if self.vehicle.mode.name=="GUIDED":
print'y-index:',i
deltatime=time.time()-self.timelast
cur_lat=self.vehicle.location.lat
cur_lon=self.vehicle.location.lon
cur_alt=self.vehicle.location.alt
dlat=cur_lat-home_lat
dlon=cur_lon-home_lon
dalt=cur_alt-home_alt
deltax = dlat * pos_latlon_to_m
deltay = dlon * pos_latlon_to_m
deltaz = dalt
f.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n" % (deltatime,vel_setx,self.vehicle.velocity[0],vel_sety,self.vehicle.velocity[1],deltax,deltay,deltaz))
self.vel_control(vel_setx,vel_sety)
time.sleep(0.1)
self.vel_control(0,0)
time.sleep(3)
print'ready to land...........'
while(self.vehicle.mode.name=="GUIDED"):
self.vehicle.mode.name="LAND"
self.vehicle.mode = VehicleMode("LAND")
self.vehicle.flush()
time.sleep(1)
while(self.vehicle.armed is True):
self.vehicle.armed=False
self.vehicle.flush()
time.sleep(1)
f.close()
#altitude_origin=self.get_mav_param('%s_MIN' % param, 0)
#print "altitude:",altitude_origin
#print "Location: %s" % self.vehicle.location.alt
else:
if last_mode=="GUIDED":
last_mode=None
self.vel_control(0,0)
time.sleep(0.1)
# handle interrupts
except:
print "interrupted, exiting"
# exit and close web server
print "exiting..."
