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")