def run(self):
targets = cycle(self.search_pattern)
self.target = next(targets)
self.vehicle.simple_goto(self.target, groundspeed=GROUNDSPEED)
while self.keep_running():
print "Lateral distance to target: ", "%.2f" % get_distance_metres(
self.vehicle.location.global_relative_frame, self.target), "m"
if get_distance_metres(self.vehicle.location.global_relative_frame,
self.target) <= TOLERANCE:
print ">>> Target reached!"
self.target = next(targets)
self.vehicle.simple_goto(self.target, groundspeed=GROUNDSPEED)
time.sleep(1) # Soften busy-waiting
def continue_to_last_target(self):
# Look for next key_point, the point of contact-loss continuing in the same direction
cont = True
while cont:
signal_out = None
try:
signal = self.signal_queue.get(block=False)
signal_out = (signal[2] > 6) # (not signal[0])
except Queue.Empty:
signal_out = False
print(
get_distance_metres(self.vehicle_location(), self.last_target))
if signal_out or (get_distance_metres(
self.vehicle_location(), self.last_target) < TOLERANCE):
self.key_points.append(self.vehicle_location())
cont = False
time.sleep(1) # Soften busy-waiting
def __init__(self, vehicle, area_coords, signal_queue, last_target):
super(NearSearch, self).__init__()
self.vehicle = vehicle
self.signal_queue = signal_queue
self.last_target = last_target
self.vehicle.airspeed = AIRSPEED
A, _, _, D = generate_area(area_coords, SEARCH_ALTITUDE)
length = get_distance_metres(A, D)
self.y_lat_step = (D.lat - A.lat) / length
self.y_lon_step = (D.lon - A.lon) / length
# Create signal key point list, add the first point of contact
self.key_points = [self.vehicle_location()]
def center_on_line(self):
print "Center on line"
print(self.key_points)
point_A = self.key_points[0]
point_B = self.key_points[1]
lat = (point_A.lat + point_B.lat) / 2.0
lon = (point_A.lon + point_B.lon) / 2.0
center = Pos(lat, lon, SEARCH_ALTITUDE)
self.vehicle.simple_goto(center, groundspeed=GROUNDSPEED)
while get_distance_metres(self.vehicle_location(), center) > TOLERANCE:
time.sleep(1) # Soften busy-waiting
pass
print ">> Centered on line"
self.key_points.append(self.vehicle_location())
self.go_perpendicular(center)
def __init__(self, vehicle, area_coords):
super(CoarseSearch, self).__init__()
self._stop_event = threading.Event()
self.vehicle = vehicle
self.target = None
self.vehicle.airspeed = AIRSPEED
dt = GRANULARITY
#Generate search pattern
A, B, C, D = generate_area(area_coords, SEARCH_ALTITUDE)
length = get_distance_metres(A, D)
num_steps = int(numpy.ceil(length / dt + 1))
steps = numpy.linspace(0, length, num=num_steps)
y_lat_step = (D.lat - A.lat) / length
y_lon_step = (D.lon - A.lon) / length
left = [
Pos(A.lat + t * y_lat_step, A.lon + t * y_lon_step,
SEARCH_ALTITUDE) for t in steps
]
right = [
Pos(B.lat + t * y_lat_step, B.lon + t * y_lon_step,
SEARCH_ALTITUDE) for t in steps
]
self.search_pattern = []
# Re-arrange to right-angle zig-zag pattern
is_left = True
for l, r in zip(left, right):
if is_left:
self.search_pattern.extend([l, r])
else:
self.search_pattern.extend([r, l])
is_left = not is_left
def go_perpendicular(self, center):
point_C = self.key_points[2]
bound = PERP_BOUND
upper_lat = point_C.lat + self.y_lat_step * bound
upper_lon = point_C.lon + self.y_lon_step * bound
lower_lat = point_C.lat + self.y_lat_step * (-bound)
lower_lon = point_C.lon + self.y_lon_step * (-bound)
t1, t2 = (Pos(upper_lat, upper_lon, SEARCH_ALTITUDE),
Pos(lower_lat, lower_lon, SEARCH_ALTITUDE))
print("Go to top")
self.vehicle.simple_goto(t1, groundspeed=GROUNDSPEED)
self.continue_straight()
print("Reached top")
print("Go back to center")
self.vehicle.simple_goto(center, groundspeed=GROUNDSPEED)
while get_distance_metres(self.vehicle_location(), center) > TOLERANCE:
time.sleep(1) # Soften busy-waiting
pass
print("Reached center")
print("Go to bottom")
self.vehicle.simple_goto(t2, groundspeed=GROUNDSPEED)
self.continue_straight()
point_A, point_B = self.key_points[-2:]
lat = (point_A.lat + point_B.lat) / 2.0
lon = (point_A.lon + point_B.lon) / 2.0
print ">>>> PERSON LOCATED:"
print ">>>> lat:", lat, " lon:", lon
self.vehicle.mode = VehicleMode("LAND")
seqnum = seqnum + 1
# print ('Ground Control Station sended seqnum={} messages'.format(seqnum))
time.sleep(.5)
elif mavlink_sysid == 2:
while True:
lat = vehicle.location.global_relative_frame.lat
lon = vehicle.location.global_relative_frame.lon
startMissionPoint = LocationGlobalRelative(lat, lon, target_altitude)
MissionPoint = LocationGlobalRelative(-27.604122, -48.518019,
target_altitude)
currentPosition = vehicle.location.global_relative_frame
distanceOfBegin = helper.get_distance_metres(startMissionPoint,
currentPosition)
while (distanceOfBegin > ShortestDistance):
currentPosition = vehicle.location.global_relative_frame
distanceOfBegin = helper.get_distance_metres(
startMissionPoint, currentPosition)
time.sleep(.1)
distanceOfEnd = helper.get_distance_metres(MissionPoint,
currentPosition)
while (distanceOfEnd > ShortestDistance):
distanceOfEnd = helper.get_distance_metres(MissionPoint,
currentPosition)
currentPosition = vehicle.location.global_relative_frame
vehicle.simple_goto(MissionPoint)
ver_x = ver_x + math.cos(heading * math.pi / 180)
ver_y = ver_y + math.sin(heading * math.pi / 180)
if abs(ver_x) < .001 and abs(ver_y) < .001:
heading_avg = vehicle.heading
else:
# Calculate heading using atan2 and convert it to [0, 359]
heading_avg = int(math.atan2(ver_y, ver_x) * 180 / math.pi + 360) % 360
# Find the nearest agent's heading
nearest_uav_distance = None
nearest_uav_location = None
for uav_id, (lat, lon, alt, heading, timestamp) in uav_positions.items():
if uav_id != ns3interface.local_id():
# Calculate distance between us and the other UAV
distance = helper.get_distance_metres(
vehicle.location.global_relative_frame,
LocationGlobalRelative(lat, lon, alt)
)
# Find minimum
if (nearest_uav_distance is None) or nearest_uav_distance > distance:
nearest_uav_distance = distance
nearest_uav_location = LocationGlobalRelative(lat, lon, alt)
# Apply flocking rules
if (nearest_uav_distance is None) or nearest_uav_distance > 3:
# Alignment and cohesion
alignment_speed = helper.heading_diff(
heading_avg,
vehicle.heading
)
cohesion_speed = helper.heading_diff(
helper.get_bearing( # direction towards flock center