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)
#navigate waypoints in manual mode
DIST_THRESH = 4 #distance to waypoint before moving onto next waypoint
P_TURN = 1 #p term of navigation controller
SPEED = 100 #throttle level
wp_cnt = 1
for wp in waypoints:
#convert global lat/lon local meters(position vectors)
wp_posvec = PositionVector.get_from_location(wp)
veh_posvec = PositionVector.get_from_location(vehicle.location.global_relative_frame)
dist = PositionVector.get_distance_xy(veh_posvec,wp_posvec)
while dist > DIST_THRESH:
#calculate target_heading and heading error
target_heading = math.degrees(PositionVector.get_bearing(veh_posvec,wp_posvec))
error = target_heading - vehicle.heading
#remap error from 0 to 360 -> -180 to 180
if error > 180:
error = error - 360
if error < -180:
error = 360 + error
print "going to wp {}: {} meters, {} error".format(wp_cnt,dist,error)
#calculate RC overrides(P controller)
steering = max(min(int(P_TURN * error * min(1,dist/12.0) + 1500),1550),1450)
throttle = int(500 * (SPEED/100.0) + 1500)
#send RC overrides
vehicle.channels.overrides['1'] = steering
DIST_THRESH = 4 #distance to waypoint before moving onto next waypoint
P_TURN = 1 #p term of navigation controller
SPEED = 100 #throttle level
wp_cnt = 1
for wp in waypoints:
#convert global lat/lon local meters(position vectors)
wp_posvec = PositionVector.get_from_location(wp)
veh_posvec = PositionVector.get_from_location(
vehicle.location.global_relative_frame)
dist = PositionVector.get_distance_xy(veh_posvec, wp_posvec)
while dist > DIST_THRESH:
#calculate target_heading and heading error
target_heading = math.degrees(
PositionVector.get_bearing(veh_posvec, wp_posvec))
error = target_heading - vehicle.heading
#remap error from 0 to 360 -> -180 to 180
if error > 180:
error = error - 360
if error < -180:
error = 360 + error
print "going to wp {}: {} meters, {} error".format(wp_cnt, dist, error)
#calculate RC overrides(P controller)
steering = max(
min(int(P_TURN * error * min(1, dist / 12.0) + 1500), 1550), 1450)
throttle = int(500 * (SPEED / 100.0) + 1500)
#send RC overrides
