def draw_fake_balloon(self, frame, veh_pos, balloon_pos, vehicle_roll,
vehicle_pitch, vehicle_yaw):
# calculate bearing to balloon
bearing_to_balloon = PositionVector.get_bearing(veh_pos, balloon_pos)
yaw_to_balloon = balloon_utils.wrap_PI(bearing_to_balloon -
vehicle_yaw)
# calculate earth frame pitch angle from vehicle to balloon
pitch_to_balloon = vehicle_pitch + PositionVector.get_elevation(
veh_pos, balloon_pos)
#print "Fake Balloon Bearing:%f Pitch:%f Dist:%f" % (math.degrees(bearing_to_balloon), math.degrees(pitch_to_balloon), dist_to_balloon_xy)
# calculate pixel position of balloon
balloon_x = balloon_video.angle_to_pixels_x(
yaw_to_balloon) + balloon_video.img_center_x
balloon_y = balloon_video.angle_to_pixels_y(
pitch_to_balloon) + balloon_video.img_center_y
# calculate size of balloon in pixels from distance and size
dist_to_balloon_xyz = PositionVector.get_distance_xyz(
veh_pos, balloon_pos)
balloon_radius = balloon_utils.get_pixels_from_distance(
dist_to_balloon_xyz, balloon_finder.balloon_radius_expected)
# store balloon radius
self.last_balloon_radius = balloon_radius
# draw balloon
cv2.circle(frame, (balloon_x, balloon_y), balloon_radius,
self.fake_balloon_colour_bgr_scalar, -1)
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 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 draw_fake_balloon(self, frame, veh_pos, balloon_pos, vehicle_roll, vehicle_pitch, vehicle_yaw):
# calculate bearing to balloon
bearing_to_balloon = PositionVector.get_bearing(veh_pos, balloon_pos)
yaw_to_balloon = balloon_utils.wrap_PI(bearing_to_balloon-vehicle_yaw)
# calculate earth frame pitch angle from vehicle to balloon
pitch_to_balloon = vehicle_pitch + PositionVector.get_elevation(veh_pos, balloon_pos)
#print "Fake Balloon Bearing:%f Pitch:%f Dist:%f" % (math.degrees(bearing_to_balloon), math.degrees(pitch_to_balloon), dist_to_balloon_xy)
# calculate pixel position of balloon
balloon_x = balloon_video.angle_to_pixels_x(yaw_to_balloon) + balloon_video.img_center_x
balloon_y = balloon_video.angle_to_pixels_y(pitch_to_balloon) + balloon_video.img_center_y
# calculate size of balloon in pixels from distance and size
dist_to_balloon_xyz = PositionVector.get_distance_xyz(veh_pos, balloon_pos)
balloon_radius = balloon_utils.get_pixels_from_distance(dist_to_balloon_xyz, balloon_finder.balloon_radius_expected)
# store balloon radius
self.last_balloon_radius = balloon_radius
# draw balloon
cv2.circle(frame,(balloon_x,balloon_y), balloon_radius, self.fake_balloon_colour_bgr_scalar, -1)
class VModule(object):
def __init__(self):
# the framerate
self.steps = 10
self.triggered = False
self.targetno = 1
def show_frame(self, no):
threshfile = "/home/ardupilot/droneapi-python/example/my_app/stereo/thresh" + str(no) + ".png"
circfile = "/home/ardupilot/droneapi-python/example/my_app/stereo/circled" + str(no) + ".png"
thresh = cv2.imread(threshfile)
circ = cv2.imread(circfile)
cv2.namedWindow("stereo")
cv2.startWindowThread()
cv2.imshow("stereo", np.hstack((thresh, circ)))
cv2.waitKey(1)
return
def location_report(self, tarloc, taralti):
# get target and vehicle locations
self.targetLoc = PositionVector()
self.vehicleLoc = PositionVector()
self.vehicleLoc.set_from_location(drn.get_location())
full_loc = Location(float(tarloc[0]), float(tarloc[1]), float(taralti), is_relative=True)
self.targetLoc.set_from_location(full_loc)
loc = (
"Vehicle location: "
+ "X: "
+ format(self.vehicleLoc.x)
+ "Y: "
+ format(self.vehicleLoc.y)
+ "Z: "
+ format(self.vehicleLoc.z)
)
alog.log.info(loc)
target_loc = (
"Target location: "
+ "X: "
+ format(self.targetLoc.x)
+ "Y: "
+ format(self.targetLoc.y)
+ "Z: "
+ format(self.targetLoc.z)
)
alog.log.info(target_loc)
dist = self.vehicleLoc.get_distance_xyz(self.vehicleLoc, self.targetLoc)
reld = "Relative distance: " + format(dist)
alog.log.info(reld)
return dist
# load_target- load an image to simulate the target.
def check_target1(self):
tarloc = [33.775497, -84.396860] # 33.775497 -84.396860 33.775632 -84.396806
taralti = 30
# wait for trigger over here
reld = self.location_report(tarloc, taralti)
if not (self.triggered):
while int(reld) > 10:
time.sleep(2)
reld = self.location_report(tarloc, taralti)
target = cv2.imread("/home/ardupilot/droneapi-python/example/my_app/stereo/1left.png")
self.load_target(target, 11, 0.0, 0.0)
# load_target- load an image to simulate the target.
def check_target2(self):
tarloc = [33.776888, -84.396367] # 33.775497 -84.396860 33.776888 -84.396367
taralti = 50
self.targetno = 2
# wait for trigger over here
reld = self.location_report(tarloc, taralti)
if not (self.triggered):
while int(reld) > 10:
time.sleep(2)
reld = self.location_report(tarloc, taralti)
target = cv2.imread("/home/ardupilot/droneapi-python/example/my_app/stereo/2left.png")
self.load_target(target, 11, 0.0, 0.0)
def check_target3(self):
tarloc = [33.777845, -84.396523] # 33.775497 -84.396860 33.776888 -84.396367 33.777845 -84.396523
taralti = 70
self.targetno = 3
# wait for trigger over here
reld = self.location_report(tarloc, taralti)
if not (self.triggered):
while int(reld) > 10:
time.sleep(2)
reld = self.location_report(tarloc, taralti)
target = cv2.imread("/home/ardupilot/droneapi-python/example/my_app/stereo/3left.png")
self.load_target(target, 11, 0.0, 0.0)
def load_target(self, target, zoom, camera_constX, camera_constY):
frame = CV.get_frame(target, camera_constX, camera_constY, zoom)
self.load_window(target, frame, zoom, camera_constX, camera_constY)
def load_window(self, target, frame, zoom, camera_constX, camera_constY):
self.steps = zoom
cv2.namedWindow("HUD")
cv2.startWindowThread()
while self.steps > 1.0:
if (int(self.steps * 10) == 91) and (self.targetno == 1): # ANALYZE THE VIDEO MODULE AND GET RESULTS
alog.log.debug("Obstacle detected.")
drn.right_drift(2)
drn.resume()
self.show_frame(self.targetno)
camera_constX = 0.02
camera_constY = 0.39
if (int(self.steps * 10) == 91) and (self.targetno == 2): # ANALYZE THE VIDEO MODULE AND GET RESULTS
alog.log.debug("Obstacle detected.")
drn.left_drift(6)
drn.resume()
self.show_frame(self.targetno)
camera_constX = 0.47
camera_constY = 0.4
if (int(self.steps * 10) == 91) and (self.targetno == 3): # ANALYZE THE VIDEO MODULE AND GET RESULTS
alog.log.debug("Obstacle detected.")
drn.left_drift(2)
drn.resume()
self.show_frame(self.targetno)
camera_constX = 0.05
camera_constY = 0.2
st = "Steps: " + format(self.steps)
alog.log.debug(st)
cv2.imshow("HUD", frame)
cv2.waitKey(1)
time.sleep(0.1)
camera_constX = camera_constX
camera_constY = camera_constY
self.steps = self.steps - 0.1
self.load_target(target, self.steps, camera_constX, camera_constY)
cv2.waitKey(3)
cv2.destroyAllWindows()
cv2.waitKey(1)
alog.log.info("Exiting vision threads")
