def step_autonomy(self, t, dt):
self._target_id = -1
self._action = ["None", 0]
self._pass += 1
if self._pass <= 400:
self._wp = np.array([self._tlat, self._tlon, self._last_ap_wp[2]])
elif self._pass < 600 and self._pass > 400:
self._wp = np.array(
[self._desired_lat, self._desired_lon, self._last_ap_wp[2]])
else:
self._pass = 0
# Fire at any reds that are within firing parameters
for target_id, red in self._reds.iteritems():
if (red.game_status == enums.GAME_ACTIVE_DEFENSE
or red.game_status == enums.GAME_ACTIVE_OFFENSE):
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
red.state.pose.pose.position.lat,
red.state.pose.pose.position.lon,
red.state.pose.pose.position.rel_alt):
self._target_id = target_id
self._action = ["Fire", self._target_id]
return True
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target every frame
min_dist = np.inf
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
# print uav.vehicle_id,
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
#print self._blues.values()
# If a target has been identified, move towards it
if self._target_id != None:
#print self._reds[self._target_id]
target_pos = self._reds[self._target_id].state.pose.pose.position
# s = ", ".join(str(e) for e in self._shot)
# print "Reds Shot: "
# print s
#print "ID: %d Target ID: %d Dist: %d" % (self._id, self._target_id, min_dist)
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
# Set the waypoint past the current target, so we don't go into a
# loiter mode
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
#print "=====>ID: %d shot at Target %d" % (self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target every frame
min_dist = np.inf
for uav in self._reds.values():
if uav.vehicle_id not in self._shot and (uav.game_status == enums.GAME_ACTIVE_DEFENSE or uav.game_status == enums.GAME_ACTIVE_OFFENSE):
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
# If a target has been identified, move towards it
if self._target_id != None:
target_pos = self._reds[self._target_id].state.pose.pose.position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
lat = tgt_lat
lon = tgt_lon
if self._vehicle_type == enums.AC_FIXED_WING:
# When using fixed wing, Set the waypoint past the current
# target, so we don't go into a loiter mode
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
# Maintain altitude
self._wp[2] = self._last_ap_wp[2]
return True
def target_hitable(self, target_id):
sp = self._own_pose.pose.pose.position
so = self._own_pose.pose.pose.orientation
quat = (so.x, so.y, so.z, so.w)
if target_id in self._reds:
target_pos = self._reds[target_id].state.pose.pose.position
return gps.hitable(sp.lat, sp.lon, sp.alt, quat, self._max_range,
self._fov_width, self._fov_height,
target_pos.lat, target_pos.lon, target_pos.alt)
else:
return False
def hitable(self, target_id):
'''Returns True if the target_id is hitable
'''
own_lla = self.state.lla()
try:
tgt_lla = self.local_reds[target_id].lla()
except KeyError:
print('ID not in local_reds: %d' % target_id)
return false
return gps.hitable(own_lla[0], own_lla[1], own_lla[2], \
(self.state.quat_gps.vector[0], \
self.state.quat_gps.vector[1], \
self.state.quat_gps.vector[2], \
self.state.quat_gps.scalar ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lla[0], tgt_lla[1], tgt_lla[2])
def step_autonomy(self, t, dt):
status = super(GotoDefend, self).step_autonomy(t, dt)
# Fire at any reds that are within firing parameters
for target_id, red in self._reds.iteritems():
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
red.state.pose.pose.position.lat,
red.state.pose.pose.position.lon,
red.state.pose.pose.position.rel_alt):
self._action = ["Fire", target_id]
return status
def step_autonomy(self, t, dt):
status = super(GotoAttack, self).step_autonomy(t, dt)
self._action = ["None", 0]
# Fire at any reds that are within firing parameters
for target_id, red in self._reds.iteritems():
if (red.game_status == enums.GAME_ACTIVE_DEFENSE
or red.game_status == enums.GAME_ACTIVE_OFFENSE):
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
red.state.pose.pose.position.lat,
red.state.pose.pose.position.lon,
red.state.pose.pose.position.rel_alt):
self._action = ["Fire", target_id]
return status
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = -1
############################################################
# If self._target_id is not None and in the shot group
# then reset to None, otherwise set to last_target
############################################################
min_dist = 500 #self._target_dist # np.inf
#print 'ID: {} min_dist {}'.format(self._id, min_dist)
for uav in self._reds.values():
if uav.vehicle_id not in self._shot and (
uav.game_status == enums.GAME_ACTIVE_DEFENSE
or uav.game_status == enums.GAME_ACTIVE_OFFENSE):
uav_lat = uav.state.pose.pose.position.lat
uav_lon = uav.state.pose.pose.position.lon
uav_alt = uav.state.pose.pose.position.rel_alt
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav_lat, uav_lon)
if d < min_dist:
min_dist = d
self._target_dist = d
self._target_id = uav.vehicle_id
if self._target_id in self._shot:
self._target_id = -1
self._target_last_id = -1
self._target_dist = np.inf
self._target_lat = None
self._target_lon = None
self._target_heading = None
self._target_speed = None
# If a target has been identified, move towards it
if self._target_id > 0:
#print "ID: %d Target: %d Last Target: %d" % (self._id, self._target_id, self._target_last_id)
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt # added to keep from changing alt
if self._last_lat is not None:
self._speed = gps.gps_distance(own_lat, own_lon,\
self._last_lat, self._last_lon) / 0.1
self._heading = gps.normalize_angle(
gps.gps_bearing(self._last_lat, self._last_lon, own_lat,
own_lon)) * RAD2DEG
self._last_lat = own_lat
self._last_lon = own_lon
target_pos = self._reds[self._target_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
#print 'UAV: {} tgt_lat: {} tgt_lon: {} last_lat: {} last_lon: {}'.format(self._id, tgt_lat, tgt_lon,\
#self._target_last_lat, self._target_last_lon)
if (self._target_id
== self._target_last_id) and (self._target_last_lat
is not None):
self._target_speed = gps.gps_distance(tgt_lat, tgt_lon,\
self._target_last_lat, self._target_last_lon) / 0.1
self._target_heading = gps.normalize_angle(
gps.gps_bearing(self._target_last_lat,
self._target_last_lon, tgt_lat,
tgt_lon)) * RAD2DEG
#print "ID: %d Speed: %f Head: %f Target: %d Dist: %f Speed: %f Head: %f" % (self._id, self._speed, self._heading, self._target_id, self._target_dist, self._target_speed, self._target_heading)
#print "Lat: %f Lon: %f Head: %f T_lat: %f T_lon %f T_head: %f" % (own_lat, own_lon, self._heading, \
#tgt_lat, tgt_lon, self._target_heading)
else:
self._target_dist = np.inf
self._target_lat = None
self._target_lon = None
self._target_heading = None
self._target_speed = None
self._target_last_id = self._target_id # set last target id to current id
self._target_last_lat = tgt_lat
self._target_last_lon = tgt_lon
#############################################################################
# Calc intercept point
# Reference: http://jaran.de/goodbits/2011/07/17/calculating-an-intercept-course-to-a-target-with-constant-direction-and-velocity-in-a-2-dimensional-plane/
# Translated from Java to Python
#############################################################################
t = None
if self._target_heading is not None:
s = self._speed
ox = tgt_lat - own_lat
oy = tgt_lon - own_lon
vx = self._target_speed * math.cos(self._target_heading) # sin
vy = self._target_speed * math.sin(self._target_heading) # cos
h1 = vx**2 + vy**2 - s**2
h2 = ox * vx + oy * vy
if h1 == 0:
t = -(ox**2 + oy**2) / (2 * h2)
else:
minusPHalf = -h2 / h1
disc = minusPHalf**2 - (ox**2 + oy**2) / h1
if disc < 0:
t = None
else:
root = math.sqrt(disc)
t1 = minusPHalf + root
t2 = minusPHalf - root
tmin = t1 if t1 < t2 else t2
tmax = t1 if t1 > t2 else t2
t = tmin if tmin > 0 else tmax
if t < 0: # or d < self._max_range: # close enough so go right to him (switch to greedy shooter)
t = None
else:
t *= 1.0 #1.53
if t is not None:
int_lat = tgt_lat + vx * t
int_lon = tgt_lon + vy * t
int_dist = gps.gps_distance(own_lat, own_lon, int_lat, int_lon)
bearing = gps.gps_bearing(own_lat, own_lon, int_lat, int_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing,
int_dist + 100)
#print "ID: %d using intercept bearing %f to Lat: %f Lon: %f Dist: %f to UAV: %d" % (self._id, bearing, lat, lon, int_dist, self._target_id) # I_lat: %f I_lon: %f t: %f
else:
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing,
dist + 100) # range was 1000
#print "ID: %d using direct bearing %f to Lat: %f Lon: %f to UAV: %d" % (self._id, bearing, lat, lon, self._target_id)
# Set the waypoint past the current target, so we don't go into a loiter mode
self._wp = np.array([lat, lon, own_alt]) # don't use tgt_alt
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", self._target_id]
print "GS Int=============================>ID: %d shot at Target %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._target_id = None
self._target_last_id = -1
self._target_dist = np.inf
self._target_lat = None
self._target_lon = None
self._target_heading = None
self._target_speed = None
self._wp = self._safe_waypoint
#print 'ID: {} has no target!'.format(self._id)
return True
def step_autonomy(self, t, dt):
# Reset the action every loop
self._action = ["None", 0]
self._target_id = None
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.alt
# Search for the closest target
min_dist = np.inf
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
d = gps.gps_distance(own_lat, \
own_lon, \
uav.state.pose.pose.position.lat, \
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
# If a target has been identified, move towards it
if self._target_id != None:
target_pos = self._reds[self._target_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
distance_to_target = gps.gps_distance(own_lat, own_lon, tgt_lat,
tgt_lon)
# Set bearing to look at the enemy vehicle
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
# Set waypoint to move forward and strafe sideways once the enemy is within range
self._evasion_range = self._weapons_range * 2
self._engagement_range = self._weapons_range * 2 / 3
if distance_to_target <= self._engagement_range:
lat, lon = gps.gps_newpos(
own_lat, own_lon,
gps.normalize(bearing - self._fov_width / 2), 0.1)
elif distance_to_target <= self._evasion_range:
# Check to see if sidestep right waypoint is outside the battlebox
# (only need to check distance_to_target/2 since enemy is closing)
lat_right, lon_right = gps.gps_newpos(own_lat, own_lon, \
gps.normalize(bearing + (math.pi / 3)), distance_to_target/2)
if self._bboxes[0].contains(lat_right, lon_right, own_alt):
lat, lon = gps.gps_newpos(own_lat, own_lon, \
gps.normalize(bearing + (math.pi / 3)), distance_to_target)
else:
# If right sidestep is outside the battlebox, sidestep to the left
lat, lon = gps.gps_newpos(
own_lat, own_lon,
gps.normalize(bearing - (math.pi / 3)),
distance_to_target)
else:
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w), \
self._weapons_range, self._fov_width, self._fov_height, \
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
#self._change = False
self._target_range = np.inf
self._target_score = np.inf
del self._tmp_list[:] # clear the search lists
del self._target_list[:]
# Search for the closest target every frame
min_dist = MIN_DIST # range to start looking for enemy
min_score = MIN_SCORE
for uav in self._reds.values():
if (uav.vehicle_id not in self._shot):
target_pos = self._reds[
uav.vehicle_id].state.pose.pose.position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
# get distance and bearing to potential enemy
d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
# Convert absolute angles to relative (shooter to target)
pose_quat = (own_orientation.x, own_orientation.y,
own_orientation.z, own_orientation.w)
rpy = qmath.quat_to_euler(pose_quat)
rel_bearing = math.fabs(
gps.normalize_angle(bearing - rpy[2])) * RAD2DEG
#print 'UAV {} to enemy {}: Dist: {} Bearing {}'.format(self._id, uav.vehicle_id), d, rel_bearing)
# is the target UAV in front of me?
score = d * rel_bearing # lower is better
if uav.vehicle_id not in self._tmp_list:
self._tmp_list.append((uav.vehicle_id, d, rel_bearing))
#print 'UAV {} to enemy {}: Dist: {} Bearing {} Score {}'.format(self._id, uav.vehicle_id, d, rel_bearing, score)
self._target_list = sorted(self._tmp_list,
key=lambda tgts: tgts[1],
reverse=True)
target_list_len = len(self._target_list) - 1
for uav in self._target_list:
if uav[0] in self._shot:
self._target_list.remove(uav)
if target_list_len < 0:
self._target_id = None
else:
self._target_id = self._target_list[target_list_len][0]
self._target_dist = self._target_list[target_list_len][1]
self._target_bearing = self._target_list[target_list_len][2]
#print 'UAV {} target list = {}'.format(self._id, self._target_list)
# If a target has been identified, move towards it
if self._target_id != None:
target_pos = self._reds[self._target_id].state.pose.pose.position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
msg = apmsg.MsgStat()
msg.id = self._target_id
msg.count = int(self._target_dist)
msg.latency = self._target_bearing
self.pubs['score'].publish(msg)
#print '{} targeting UAV {} at distance {} bearing {}'.format(self._id, self._target_id, self._target_dist, self._target_bearing)
# Set the waypoint past the current target, so we don't go into a
# loiter mode
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 10)
self._wp = np.array([
lat, lon, own_alt
]) # don't climb or dive to target (was target_pos.rel_alt)
# Determine if the target is within firing parameters
if gps.hitable(own_lat, own_lon, own_alt, \
(own_orientation.x, \
own_orientation.y, \
own_orientation.z, \
own_orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", self._target_id]
print "=============> Wing %d shot at Target %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
print 'UAV {} has no target ID!'.format(self._id)
self._wp = self._safe_waypoint
self._change = False
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target every frame
min_dist = self._min_dist # np.inf
own_pos = self._own_pose.pose.pose.position
own_orientation = self._own_pose.pose.pose.orientation
own_lat = own_pos.lat
own_lon = own_pos.lon
own_alt = own_pos.rel_alt
for uav in self._reds.values():
if uav.vehicle_id not in self._shot and (
uav.game_status == enums.GAME_ACTIVE_DEFENSE
or uav.game_status == enums.GAME_ACTIVE_OFFENSE):
target_pos = self._reds[
uav.vehicle_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
if d <= self._max_range:
if gps.hitable(own_lat, own_lon, own_alt, \
(own_orientation.x, own_orientation.y, \
own_orientation.z, own_orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", uav.vehicle_id]
print 'GSA========(in loop)===========> ID: {} shot at UAV: {}'.format(
self._id, uav.vehicle_id)
#self._action = ["None", 0]
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
if self._target_id in self._shot:
self._target_id = None
if self._last_target_id == self._target_id:
total_count = self._max_count * (
1 + self._chase_safe) / self._chase_safe
if d < self._target_dist: # how close do we have to be before I call chasing
self._target_count += 1
if self._target_count < self._max_count:
pass
#print 'ID: {} chasing UAV: {} for {} iterations'.format(self._id, self._target_id, self._target_count)
elif self._target_count < total_count:
pass
#print 'ID: {} going to safe wp for {} iterations'.format(self._id, total_count - self._target_count)
else:
self._target_count = 0
self._target_id = None
# If a target has been identified, move towards it
if self._target_id != None and self._target_count < self._max_count:
self._last_target_id = self._target_id
target_pos = self._reds[self._target_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
lat = tgt_lat
lon = tgt_lon
if self._vehicle_type == enums.AC_FIXED_WING:
# When using fixed wing, Set the waypoint past the current
# target, so we don't go into a loiter mode
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(own_lat, own_lon, own_alt, \
(own_orientation.x, own_orientation.y, \
own_orientation.z, own_orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", self._target_id]
print 'GSA============================> ID: {} shot at UAV: {}'.format(
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
# Maintain altitude
self._wp[2] = self._last_ap_wp[2]
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = self._last_target
# Search for the closest target every frame if you don't already have one
min_dist = np.inf # self._min_dist
if self._got_target is False:
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
self._last_target = self._target_id
if d < MIN_DIST:
self._got_target = True
else:
self._got_target = False
else:
self._got_target = False
self._last_target = None
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
if self._target_id == uav.vehicle_id:
self._last_target = self._target_id
self._got_target = True
# If a target has been identified, move towards it
if self._target_id != None:
target_pos = self._reds[self._target_id].state.pose.pose.position
# print "*************************************"
print "ID: %d Target ID: %d\n" % (self._id, self._target_id)
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
# Set the waypoint past the current target, so we don't go into a
# loiter mode
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, tgt_alt
]) # np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
self._last_target = None
self._got_target = False
else:
# Start at the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
self._target_range = np.inf
self._target_score = np.inf
del self._t_id[:] # clear the search lists
del self._t_score[:]
# Search for the closest target every frame
min_dist = MIN_DIST # range to start looking for enemy
min_score = MIN_SCORE
for uav in self._reds.values():
if (uav.vehicle_id not in self._shot):
target_pos = self._reds[
uav.vehicle_id].state.pose.pose.position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
# get distance and bearing to potential enemy
d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
# Convert absolute angles to relative (shooter to target)
pose_quat = (own_orientation.x, own_orientation.y,
own_orientation.z, own_orientation.w)
rpy = qmath.quat_to_euler(pose_quat)
rel_bearing = math.fabs(gps.normalize_angle(bearing - rpy[2]))
#print 'UAV {} to enemy {}: Dist: {} Bearing {}'.format(self._id, uav.vehicle_id), d, rel_bearing)
# is the target UAV in front of me?
score = d * rel_bearing # lower is better
print 'UAV {} to enemy {}: Dist: {} Bearing {} Score {}'.format(
self._id, uav.vehicle_id, d, rel_bearing, score)
if score < min_score:
min_score = score
#self._target_score = score
#self._target_id = uav.vehicle_id
#self._target_range = d
if uav.vehicle_id not in self._t_id:
self._t_id.append(uav.vehicle_id)
self._t_score.append(score) # whoops! was d!
self._target_id = uav.vehicle_id
self._target_score = score
self._target_range = d
#print 'UAV {} to enemy {}: Dist: {} Bearing {} Score {}'.format(self._id, uav.vehicle_id, d, rel_bearing, score)
# If a target has been identified, move towards it
if self._target_id != None:
msg = apmsg.MsgStat()
msg.id = self._id
msg.count = self._target_id
msg.latency = self._target_score
self.pubs['score'].publish(msg)
#print '{} targeting UAV {} with a score of of {}'.format(self._id, self._target_id, self._target_score)
target_pos = self._reds[self._target_id].state.pose.pose.position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
# Set the waypoint past the current target, so we don't go into a
# loiter mode
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(own_lat, own_lon, own_alt, \
(own_orientation.x, \
own_orientation.y, \
own_orientation.z, \
own_orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", self._target_id]
print "=============> Wing %d shot at Target %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
# Search for the closest target every frame
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
# Calculate target and own position and orientation
target_pos = self._reds[uav.vehicle_id].state.pose.pose.position
target_orientation = self._reds[uav.vehicle_id].state.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
if self._bboxes[0].contains(tgt_lat, tgt_lon, tgt_alt): # added to only consider those in battlebox
own_pos = self._own_pose.pose.pose.position
own_lat = own_pos.lat
own_lon = own_pos.lon
own_alt = own_pos.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
# Calculate bearing to target and from target to self
bearing = gps.normalize(gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon))
tgt_bearing = gps.normalize(gps.gps_bearing(tgt_lat, tgt_lon, own_lat, own_lon))
# Calculate heading/yaw of self and target
own_rpy = qmath.quat_to_euler((own_orientation.x,\
own_orientation.y, \
own_orientation.z, \
own_orientation.w))
own_heading = gps.normalize(own_rpy[2])
tgt_rpy = qmath.quat_to_euler((target_orientation.x,\
target_orientation.y, \
target_orientation.z, \
target_orientation.w))
tgt_heading = gps.normalize(tgt_rpy[2])
# Calculate distance to target
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
bearing_adj = 0.0
# Calculate offset between bearing and target heading (measures degree of "head on")
# ----------------------------------------------------------------------------
heading_diff = own_heading - tgt_heading
tgt_bearing_diff = gps.normalize_pi( tgt_heading - tgt_bearing )
if math.fabs(heading_diff) > math.pi/2:
# moving towards each other
if tgt_bearing > (tgt_heading - self._fov_width_radians/2) or tgt_bearing < (tgt_heading + self._fov_width_radians/2):
# we are in the "fan"
if dist > 2*self._max_range and math.fabs(tgt_bearing_diff) < self._fov_width_radians/2:
bearing_adj = math.copysign(ADJ_ANGLE, tgt_bearing_diff)
#print 'ID: {} toward UAV: {}: adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj))
elif dist < self._max_range and math.fabs(tgt_bearing_diff) > self._fov_width_radians*3:
# along side each other
bearing_adj = math.copysign(3*ADJ_ANGLE, tgt_bearing_diff)
#print 'ID: {} along side UAV: {}: adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj))
else:
# heading in same general direction
if math.fabs(tgt_bearing_diff) < math.pi/2:
# I am in front of target
if dist < 2*self._max_range and math.fabs(tgt_bearing_diff) < self._fov_width_radians/2:
bearing_adj = -math.copysign(ADJ_ANGLE, tgt_bearing_diff)
#print 'ID: {} away from UAV: {} adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj))
# ----------------------------------------------------------------------------
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj, 1000)
self._wp = np.array([lat, lon, own_alt]) # keep own alt
# Determine if the target is within firing parameters
if gps.hitable(own_lat, own_lon, own_alt, \
(own_orientation.x, \
own_orientation.y, \
own_orientation.z, \
own_orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", uav.vehicle_id]
print "EA=======================>ID: %d shot at Target %d" % (self._id, uav.vehicle_id)
return True
def step_autonomy(self, t, dt):
self._action = ["None", 0]
if self._target_id in self._shot:
self._target_id = -1
#dont excede max fly dist
#break and check targets
sdist = self.dist_calc(self._own_pose.pose.pose.position.lat, self._own_pose.pose.pose.position.lon)
if sdist > 100:
print "Exceded fly zone. Returning to holding pattern"
sdist = self.dist_calc(self._own_pose.pose.pose.position.lat, self._own_pose.pose.pose.position.lon)
self._target_id = -1
self._wp = np.array([self._desired_lat, self._desired_lon, self._last_ap_wp[2]])
return True
#hunt current target
if self._target_id != -1 and self._pursue <= 50:
print "Continue hunting %d for %d more cycles" % (self._target_id, 50-self._pursue)
self._pursue += 1
#check if target outside max dist
target_pos = self._reds[self._target_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tdist = self.dist_calc(tgt_lat, tgt_lon)
#if target outside airspace, clear and check for other targets
if tdist > 150:
print "%d left airspace; breaking contact" % self._target_id
self._target_id = -1
self._pursue = 0
#target still within pursue distance, check if firing solution available
else:
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, own_alt])
#if firing solution, take shot
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
print "RESERVE===========================> %d Firing on %d" % (self._id, self._target_id)
self._action = ["Fire", self._target_id]
self._pursue = 0
return True
#pursue exceded, reset and find new target
if self._pursue > 50:
self._pursue = 0
print "Pursue exceeded: reset target"
self._target_id = -1
#check if any aircraft in airspace
if self._target_id == -1:
print "Checking for hostiles in airspace"
target = -1
minDist = 1000
#find closest hostile within attack range
for target_id, red in self._reds.iteritems():
if (red.game_status == enums.GAME_ACTIVE_DEFENSE or red.game_status == enums.GAME_ACTIVE_OFFENSE):
target_pos = self._reds[target_id].state.pose.pose.position
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._target_id = target_id
self._action = ["Fire", self._target_id]
print "RESERVE===========================> %d Firing on %d" % (self._id, self._target_id)
return True
dist = self.dist_calc(red.state.pose.pose.position.lat, red.state.pose.pose.position.lon)
prox = self.self_dist(red.state.pose.pose.position.lat, red.state.pose.pose.position.lon)
if dist < 150 and target_id not in self._shot and prox < minDist:
target = target_id
minDist = dist
self._target_id = target
self._pursue = 0
#if this code is hit, no hostile aircraft in airspace, and still in max range
#return home and end step
self._wp = np.array([self._desired_lat, self._desired_lon, self._last_ap_wp[2]])
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
############################################################
# If self._target_id is not None and in the shot group
# then reset to None, otherwise set to last_target
############################################################
min_dist = self._target_dist
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
uav_lat = uav.state.pose.pose.position.lat
uav_lon = uav.state.pose.pose.position.lon
uav_alt = uav.state.pose.pose.position.rel_alt
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav_lat, uav_lon)
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
9999, 180, 180, uav_lat, uav_lon, uav_alt):
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
if self._target_id in self._shot:
self._target_id = None
self._target_dist = np.inf
self._target_count = 0
self._target_last_lat = None
self._target_last_lon = None
self._target_heading = None
self._target_speed = None
# If a target has been identified, move towards it
if self._target_id != None:
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
if self._last_lat is not None:
self._speed = gps.gps_distance(own_lat, own_lon,\
self._last_lat, self._last_lon) / 0.1
self._heading = gps.gps_bearing(self._last_lat, self._last_lon,
own_lat, own_lon)
self._last_lat = own_lat
self._last_lon = own_lon
target_pos = self._reds[self._target_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
tgt_lat,\
tgt_lon)
if self._target_last_lat is not None:
self._target_speed = gps.gps_distance(tgt_lat, tgt_lon,\
self._target_last_lat, self._target_last_lon) / 0.1
self._target_heading = gps.gps_bearing(self._target_last_lat,
self._target_last_lon,
tgt_lat, tgt_lon)
#print "ID: %d Target: %d" % (self._id, self._target_id)
#print "Lat: %f Lon: %f Head: %f T_lat: %f T_lon %f T_head: %f" % (own_lat, own_lon, self._heading, \
#tgt_lat, tgt_lon, self._target_heading)
self._target_last_lat = tgt_lat
self._target_last_lon = tgt_lon
#############################################################################
# Calc intercept point
# swapped lat, lon, sin, cos to see if it changed
t = None
if self._target_heading is not None:
s = self._speed
ox = tgt_lat - own_lat
oy = tgt_lon - own_lon
vx = self._target_speed * math.cos(self._target_heading) # sin
vy = self._target_speed * math.sin(self._target_heading) # cos
h1 = vx**2 + vy**2 - s**2
h2 = ox * vx + oy * vy
if h1 == 0:
t = -(ox**2 + oy**2) / (2 * h2)
else:
minusPHalf = -h2 / h1
disc = minusPHalf**2 - (ox**2 + oy**2) / h1
if disc < 0:
t = None
else:
root = math.sqrt(disc)
t1 = minusPHalf + root
t2 = minusPHalf - root
tmin = t1 if t1 < t2 else t2
tmax = t1 if t1 > t2 else t2
t = tmin if tmin > 0 else tmax
if t < 0:
t = None
#else:
#t *= 1.65
if t is not None:
t_adj = 0.0 # 003 # add equiv of 1 sec
int_lat = tgt_lat + vx * (t + t_adj)
int_lon = tgt_lon + vy * (t + t_adj)
int_dist = gps.gps_distance(own_lat, own_lon, int_lat, int_lon)
bearing = gps.gps_bearing(own_lat, own_lon, int_lat, int_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, int_dist)
#print "ID: %d using intercept bearing %f to Lat: %f Lon: %f Dist: %f Time: %f" % (self._id, bearing, lat, lon, int_dist, t*10000) # I_lat: %f I_lon: %f t: %f
else:
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing,
dist + 250) # range was 1000
#print "ID: %d using direct bearing %f to Lat: %f Lon: %f Dist: %f" % (self._id, bearing, lat, lon, dist)
# Set the waypoint past the current target, so we don't go into a
# loiter mode
#if tgt_alt < 200: # we're getting ground collisions
#tgt_alt = 200
self._wp = np.array([lat, lon, tgt_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", self._target_id]
print "GS Int=============================>ID: %d shot at Target %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target every frame
min_dist = np.inf
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon, \
uav.state.pose.pose.position.rel_alt):
self._action = ["Fire", uav.vehicle_id]
print "RT=========(loop)========>ID: %d shot at Target %d" % (
self._id, uav.vehicle_id)
'''if self._last_target_id == self._target_id:
if d < 100: # how close do we have to be before I call chasing
self._target_count += 1
if self._target_count > 100:
self._target_count = 300
self._target_id = None
elif self._target_count > 0:
self._target_count -= 1
self._target_id = None
'''
# If a target has been identified, move towards it
if self._target_id != None:
self._last_target_id = self._target_id
# Calculate target and own position and orientation
target_pos = self._reds[self._target_id].state.pose.pose.position
target_orientation = self._reds[
self._target_id].state.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
own_pos = self._own_pose.pose.pose.position
own_lat = own_pos.lat
own_lon = own_pos.lon
own_alt = own_pos.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
# Calculate bearing to target and from target to self
bearing = gps.normalize(
gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon))
tgt_bearing = gps.normalize(
gps.gps_bearing(tgt_lat, tgt_lon, own_lat, own_lon))
# Calculate heading/yaw of self and target
own_rpy = qmath.quat_to_euler((own_orientation.x,\
own_orientation.y, \
own_orientation.z, \
own_orientation.w))
own_heading = gps.normalize(own_rpy[2])
tgt_rpy = qmath.quat_to_euler((target_orientation.x,\
target_orientation.y, \
target_orientation.z, \
target_orientation.w))
tgt_heading = gps.normalize(tgt_rpy[2])
# Calculate distance to target
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
bearing_adj = 0.0
#print 'ID: {} targeting UAV: {} at bearing {} and distance {}'.format(self._id, self._target_id, math.degrees(bearing), dist)
#print 'UAV: {} own bearing: {}'.format(self._target_id, math.degrees(tgt_heading))
# Calculate offset between bearing and target heading (measures degree of "head on")
# This section needs more work
# ----------------------------------------------------------------------------
heading_diff = own_heading - tgt_heading
tgt_bearing_diff = gps.normalize_pi(tgt_heading - tgt_bearing)
if math.fabs(heading_diff) > math.pi / 2:
# moving towards each other
if tgt_bearing > (
tgt_heading - self._fov_width_radians / 2
) or tgt_bearing < (tgt_heading + self._fov_width_radians / 2):
# we are in the "fan"
if dist > self._max_range and math.fabs(
tgt_bearing_diff) < self._fov_width_radians / 2:
bearing_adj = math.copysign(ADJ_ANGLE,
tgt_bearing_diff)
print 'ID: {} toward UAV: {}: adjusting bearing by {}'.format(
self._id, self._target_id,
math.degrees(bearing_adj))
else:
# heading in same general direction
if math.fabs(tgt_bearing_diff) < math.pi / 2:
# I am in front of target
if dist < 2 * self._max_range and math.fabs(
tgt_bearing_diff) < self._fov_width_radians / 2:
bearing_adj = -math.copysign(ADJ_ANGLE,
tgt_bearing_diff)
print 'ID: {} away from UAV: {} adjusting bearing by {}'.format(
self._id, self._target_id,
math.degrees(bearing_adj))
# ----------------------------------------------------------------------------
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj,
1000)
self._wp = np.array([lat, lon, own_alt]) # keep own alt
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
print "RT=======================>ID: %d shot at Target %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = self._last_target
# Search for the closest target every frame if you don't already have one
min_dist = np.inf # self._min_dist
if self._got_target is False:
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if self._tgt_find_count >= 100:
self._tracked = []
self._tgt_find_count = 0
print "=======================ID %d reseting targets\n" % (
self._id)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
if self._target_id not in self._tracked:
self._last_target = self._target_id
if d < MIN_DIST:
self._got_target = True
print "ID: %d locked onto Target ID: %d\n" % (
self._id, self._target_id)
self._tracked.append(self._target_id)
break
else:
print "ID: %d Found Last Target %d\n" % (
self._id, self._tgt_find_count)
self._tgt_find_count += 1
self._got_target = False
else:
self._got_target = False
self._target_id = None
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
if self._last_target == uav.vehicle_id:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < MIN_DIST:
self._target_id = self._last_target
self._got_target = True
print "ID: %d tracking Target ID: %d\n" % (
self._id, self._target_id)
else:
print "ID: %d lost Target ID: %d\n" % (
self._id, self._last_target)
self._got_target = False
#self._target_id = None
#self._last_target = None
# If a target has been identified, move towards it
if self._target_id != None:
self._cur_track += 1
target_pos = self._reds[self._target_id].state.pose.pose.position
# print "*************************************"
#print "ID: %d Target ID: %d\n" % (self._id, self._target_id)
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
# Set the waypoint past and above the current target, so we don't go into a
# loiter mode, and can attack from above
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
if self._cur_track >= 100:
print "ID: %d swapping target\n" % (self._id)
self._cur_track = 0
attack_alt = 250
self._got_target = False
lat, lon = gps.gps_newpos(tgt_lat, tgt_lon, bearing, 2000)
else:
lat, lon = gps.gps_newpos(tgt_lat, tgt_lon, bearing, 1000)
if dist < 500:
attack_alt = tgt_alt
else:
if tgt_alt > self._reach_alt:
self._reach_alt = tgt_alt + 100
if self._descent == 1:
attack_alt = own_alt - 50
if own_alt <= 500:
self._descent = 0
else:
if own_alt >= 1000:
attack_alt = self._reach_alt / 2
self._descent = 1
elif own_alt < self._reach_alt:
attack_alt = own_alt + 100
else:
attack_alt = own_alt - 50
self._wp = np.array([
lat + (.00002 * self._id), lon + (.00002 * self._id),
attack_alt
]) # np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
print "GS Single==========================>ID: %d shot at Target %d" % (
self._id, self._target_id)
#self._last_target = None
#self._got_target = False
else:
# Start at the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Get our current position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt # was just alt
# Reset the action every loop
self._action = ["None", 0]
self._last_target_id = self._target_id
self._last_target_bearing = self._target_bearing
self._target_id = None
self._target_bearing = 0.0
bearing_adj = -0.7 # default adjust for CCW circling enemy
# Search for the closest target
min_dist = np.inf
for uav in self._reds.values():
if uav.vehicle_id not in self._shot and (
uav.game_status == enums.GAME_ACTIVE_DEFENSE
or uav.game_status == enums.GAME_ACTIVE_OFFENSE):
target_pos = self._reds[
uav.vehicle_id].state.pose.pose.position
d = gps.gps_distance(own_lat, \
own_lon, \
uav.state.pose.pose.position.lat, \
uav.state.pose.pose.position.lon)
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w), \
self._weapons_range, self._fov_width, self._fov_height, \
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", uav.vehicle_id]
print "Quad=======(loop)=======> ID: %d shot at enemy %d" % (
self._id, uav.vehicle_id)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
# If a target has been identified, move towards it
if self._target_id != None:
target_pos = self._reds[self._target_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
distance_to_target = gps.gps_distance(own_lat, own_lon, tgt_lat,
tgt_lon)
# Set bearing to look at the enemy vehicle
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
self._target_bearing = bearing
if self._target_id == self._last_target_id:
bearing_diff = self._target_bearing - self._last_target_bearing
if bearing_diff < 0.0:
bearing_adj = math.sin(-UAV_SPEED / distance_to_target)
else:
bearing_adj = math.sin(UAV_SPEED / distance_to_target)
# Set waypoint to move forward and strafe sideways once the enemy is within range
self._evasion_range = self._weapons_range * 3
self._engagement_range = self._weapons_range * 2 / 3
if distance_to_target <= self._engagement_range:
lat, lon = gps.gps_newpos(own_lat, own_lon,
gps.normalize(bearing + bearing_adj),
0.1) # was - self._fov_width/2
#print "UAV %d in engagement range with enemy %d bearing %f distance %f using %f bearing adj" % (self._id, self._target_id, math.degrees(bearing), distance_to_target, bearing_adj)
elif distance_to_target <= self._evasion_range:
# Check to see if sidestep right waypoint is outside the battlebox
# (only need to check distance_to_target/2 since enemy is closing)
evade_sign = -2 * (self._id % 2) + 1
#print "UAV %d in evasion range with enemy %d" % (self._id, self._target_id)
lat, lon = gps.gps_newpos(
own_lat, own_lon,
gps.normalize(bearing - evade_sign * (math.pi / 3)),
distance_to_target)
if not self._bboxes[0].contains(lat, lon, own_alt):
lat, lon = gps.gps_newpos(
own_lat, own_lon,
gps.normalize(bearing + evade_sign * (math.pi / 3)),
distance_to_target)
#print "UAV %d sidestepping left" % self._id
else:
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing,
1000) # was 1000
self._wp = np.array([lat, lon, own_alt
]) # don't change alt, was target_pos.rel_alt
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w), \
self._weapons_range, self._fov_width, self._fov_height, \
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
print "Quad====================> ID: %d shot at enemy %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target every frame
min_dist = 500 # np.inf
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
# If a target has been identified, move towards it
if self._target_id != None:
target_pos = self._reds[self._target_id].state.pose.pose.position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
# Set the waypoint past the current target, so we don't go into a
# loiter mode
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
elif t < 10:
# Start at the safe waypoint
self._wp = self._safe_waypoint
elif t > (self._last_update + 1):
# Slowly increment the waypoint's altitude
if self._own_pose.pose.pose.position.alt > 3000:
self._wp[2] -= 5
elif self._own_pose.pose.pose.position.alt < 1000:
self._wp[2] += 5
else:
self._wp[2] += random.randint(-2, 10)
self._last_update = t
return True
def receive_msg_stat(self, msg):
#self._reported.add(msg.id)
if self._id != msg.id:
self._taken.add(msg.count)
#print '{} received message from {}: Targeting {} at distance {}'.format(self._id, msg.id, msg.count, msg.latency)
if msg.count == self._target_id:
if msg.latency < self._target_dist: #self._best_dist:
target_pos = self._reds[msg.count].state.pose.pose.position
blue_lat = self._blues[msg.id].state.pose.pose.position.lat
blue_lon = self._blues[msg.id].state.pose.pose.position.lon
blue_alt = self._blues[
msg.id].state.pose.pose.position.rel_alt
blue_orientation = self._blues[
msg.id].state.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
if gps.hitable(blue_lat, blue_lon, blue_alt, \
(blue_orientation.x, \
blue_orientation.y, \
blue_orientation.z, \
blue_orientation.w ), \
np.inf, 45.0, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
#self._best_dist = msg.latency
#self._best_id = msg.id
print '{} is closer than {} to UAV {} and is within 45 deg'.format(
msg.id, self._id, msg.count)
l = len(self._t_id)
if l > 1:
self._t_id.pop()
self._t_dist.pop()
self._target_id = self._t_id[l - 2]
self._target_dist = self._t_dist[l - 2]
#self._taken.add(msg.count)
print '{} dropped UAV {} due to {} being better candidate at distance {}'.format(
self._id, msg.count, msg.id, msg.latency)
print '{} now targeting UAV {} at a distance of {}'.format(
self._id, self._target_id, self._target_dist)
else:
self._target_id = None
self._target_dist = np.inf
#self._taken.discard(msg.count)
else:
self._taken.discard(msg.count)
'''if self._best_dist < self._target_dist:
l = len(self._t_id)
if l > 1:
self._t_id.pop()
self._t_dist.pop()
self._target_id = self._t_id[l-2]
self._target_dist = self._t_dist[l-2]
#self._taken.add(msg.count)
print '{} dropped UAV {} due to {} being better candidate at distance {}'.format(self._id, msg.count, self._best_id, self._best_dist)
print '{} now targeting UAV {} at a distance of {}'.format(self._id, self._target_id, self._target_dist)
else:
self._target_id = None
self._target_dist = np.inf
#self._taken.discard(msg.count)
else:
self._taken.discard(msg.count)
'''
'''if self._reported == set(self._blues) or len(self._blues) == 1:
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
############################################################
# If self._target_id is not None and in the shot group
# then reset to None, otherwise set to last_target
############################################################
min_dist = self._target_dist
for uav in self._reds.values():
if uav.vehicle_id not in self._reds_shot:
uav_lat = uav.state.pose.pose.position.lat
uav_lon = uav.state.pose.pose.position.lon
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav_lat,\
uav_lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
if self._target_id in self._reds_shot:
self._target_id = None
self._target_dist = np.inf
self._target_count = 0
self._target_last_lat = None
self._target_last_lon = None
self._target_heading = None
self._target_speed = None
# If a target has been identified, move towards it
if self._target_id != None:
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
if self._last_lat is not None:
self._speed = gps.gps_distance(own_lat, own_lon,\
self._last_lat, self._last_lon) / 0.1
self._heading = gps.gps_bearing(self._last_lat, self._last_lon,
own_lat, own_lon)
self._last_lat = own_lat
self._last_lon = own_lon
target_pos = self._reds[self._target_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
tgt_lat,\
tgt_lon)
if self._target_last_lat is not None:
self._target_speed = gps.gps_distance(tgt_lat, tgt_lon,\
self._target_last_lat, self._target_last_lon) / 0.1
self._target_heading = gps.gps_bearing(self._target_last_lat,
self._target_last_lon,
tgt_lat, tgt_lon)
#print "ID: %d Target: %d" % (self._id, self._target_id)
#print "Lat: %f Lon: %f Head: %f T_lat: %f T_lon %f T_head: %f" % (own_lat, own_lon, self._heading, \
#tgt_lat, tgt_lon, self._target_heading)
self._target_last_lat = tgt_lat
self._target_last_lon = tgt_lon
#############################################################################
# Calc intercept point
# Reference: http://jaran.de/goodbits/2011/07/17/calculating-an-intercept-course-to-a-target-with-constant-direction-and-velocity-in-a-2-dimensional-plane/
# Translated from Java to Python
#############################################################################
t = None
if self._target_heading is not None:
s = self._speed
ox = tgt_lat - own_lat
oy = tgt_lon - own_lon
vx = self._target_speed * math.cos(self._target_heading) # sin
vy = self._target_speed * math.sin(self._target_heading) # cos
h1 = vx**2 + vy**2 - s**2
h2 = ox * vx + oy * vy
if h1 == 0:
t = -(ox**2 + oy**2) / (2 * h2)
else:
minusPHalf = -h2 / h1
disc = minusPHalf**2 - (ox**2 + oy**2) / h1
if disc < 0:
t = None
else:
root = math.sqrt(disc)
t1 = minusPHalf + root
t2 = minusPHalf - root
tmin = t1 if t1 < t2 else t2
tmax = t1 if t1 > t2 else t2
t = tmin if tmin > 0 else tmax
if t < 0:
t = None
if t is not None:
t_adj = 0 # add extra time for tweaking
int_lat = tgt_lat + vx * (t + t_adj)
int_lon = tgt_lon + vy * (t + t_adj)
int_dist = gps.gps_distance(own_lat, own_lon, int_lat, int_lon)
bearing = gps.gps_bearing(own_lat, own_lon, int_lat, int_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing,
int_dist + 1000)
#print "Using intercept Lat: %f Lon: %f Time: %f" % (lat, lon, t) # I_lat: %f I_lon: %f t: %f
else:
bearing = gps.gps_bearing(
own_lat, own_lon, tgt_lat, tgt_lon
) # use standard TutorialGreedyShooter method of new waypoint
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing,
1000) # range was 1000
#print "Using bearing %f to Lat: %f Lon: %f" % (bearing, lat, lon)
# Set the waypoint past the current target, so we don't go into a loiter mode
self._wp = np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", self._target_id]
#print "GS Int=============================>ID: %d shot at Target %d" % (self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
if self._target_id in RedGreedyShooter._shot:
RedGreedyShooter._reds_taken.remove(self._target_id)
self._target_id = None
else:
self._target_id = self._last_target
min_dist = np.inf
for uav in self._reds.values():
uav_lat = uav.state.pose.pose.position.lat
uav_lon = uav.state.pose.pose.position.lon
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav_lat,\
uav_lon)
if uav.vehicle_id not in RedGreedyShooter._shot:
if uav_lat == self._reds_last_lat[uav.vehicle_id]: # check to see if target moved since last time
RedGreedyShooter._shot.add(uav.vehicle_id) # add to dead list
print "===============================>Enemy ID: %d added to dead list" % (uav.vehicle_id)
if uav.vehicle_id in RedGreedyShooter._reds_taken:
RedGreedyShooter._reds_taken.remove(uav.vehicle_id)
else:
self._reds_last_lat[uav.vehicle_id] = uav_lat # yes, update position
if uav.vehicle_id not in RedGreedyShooter._reds_taken:
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
self._last_target = self._target_id
# If a target has been identified, move towards it
if self._target_id != None:
#if self._target_id is not self._last_target:
#RedGreedyShooter._reds_taken.remove(self._last_target)
RedGreedyShooter._reds_taken.add(self._target_id)
target_pos = self._reds[self._target_id].state.pose.pose.position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
tgt_lat,\
tgt_lon)
print "ID: %d Target ID: %d Dist: %d" % (self._id, self._target_id, d)
s = ", ".join(str(e) for e in RedGreedyShooter._reds_taken)
print "Reds Taken: "
print s
# Set the waypoint past the current target, so we don't go into a
# loiter mode
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 200) # range was 1000
self._wp = np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", self._target_id]
print "=====>ID: %d shot at Target %d" % (self._id, self._target_id)
'''if tgt_lat != self._reds_last_lat[self._target_id]: # check to see if target moved since last time
self._reds_last_lat[self._target_id] = tgt_lat # yes, update position
else:
RedGreedyShooter._shot.add(self._target_id) # add to dead list
print "=====>Enemy ID: %d added to dead list" % (self._target_id)
RedGreedyShooter._reds_taken.remove(self._target_id)
self._last_target = None
s = ", ".join(str(e) for e in RedGreedyShooter._shot)
print "Reds Shot: "
print s
'''
# RedGreedyShooter._reds_taken.remove(self._target_id) # release enemy ID from set
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Get our current position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt # was just alt
# Reset the action every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target
min_dist = MIN_DIST
del t_id[:]
del t_dist[:]
for uav in self._reds.values():
if (uav.vehicle_id not in self._shot) and (uav.vehicle_id
not in self._taken):
d = gps.gps_distance(own_lat, \
own_lon, \
uav.state.pose.pose.position.lat, \
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
self._bearing_adj = -(self._fov_width / 2)
self._target_range = d
# If a target has been identified, move towards it
if self._target_id != None:
msg = apmsg.MsgStat()
msg.id = self._id
msg.count = self._target_id
msg.latency = self._target_range
self.pubs['taken'].publish(msg)
print '{} targeting UAV {} at a distance of {}'.format(
self._id, self._target_id, self._target_range)
print '{} has taken list of {}'.format(self._id, self._taken)
target_pos = self._reds[self._target_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
distance_to_target = gps.gps_distance(own_lat, own_lon, tgt_lat,
tgt_lon)
# Set bearing to look at the enemy vehicle
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
# Set waypoint to move forward and strafe sideways once the enemy is within range
self._evasion_range = self._weapons_range * 2
self._engagement_range = self._weapons_range
if distance_to_target <= self._engagement_range:
self._bearing_adj += 2.5
if self._bearing_adj > self._fov_width:
self._bearing_adj = -self._fov_width
bearing += math.radians(
self._bearing_adj
) # bearing_adj in degrees; convert to radians
lat, lon = gps.gps_newpos(own_lat, own_lon,
gps.normalize(bearing),
0.1) # was - self._fov_width/2
#print "UAV %d in engagement range with enemy %d bearing %f" % (self._id, self._target_id, math.degrees(bearing))
elif distance_to_target <= self._evasion_range:
# Check to see if sidestep right waypoint is outside the battlebox
# (only need to check distance_to_target/2 since enemy is closing)
#print "UAV %d in evasion range with enemy %d" % (self._id, self._target_id)
lat_right, lon_right = gps.gps_newpos(own_lat, own_lon, \
gps.normalize(bearing + (math.pi / 3)), distance_to_target/2)
if self._bboxes[0].contains(lat_right, lon_right, own_alt):
lat, lon = gps.gps_newpos(own_lat, own_lon, \
gps.normalize(bearing + (math.pi / 3)), distance_to_target)
#print "UAV %d sidestepping right" % self._id
else:
# If right sidestep is outside the battlebox, sidestep to the left
lat, lon = gps.gps_newpos(
own_lat, own_lon,
gps.normalize(bearing - (math.pi / 3)),
distance_to_target)
#print "UAV %d sidestepping left" % self._id
else:
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing,
100) # was 1000
self._wp = np.array([lat, lon, target_pos.rel_alt
]) # +random.randint(-10,10)]
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w), \
self._weapons_range, self._fov_width, self._fov_height, \
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
print "=================>ID: %d shot at Target %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
self._taken.clear(
) # clear the taken list in case it's full of old data
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
self._target_range = np.inf
# print set(self._blues)
# Search for the closest target every frame
min_dist = MIN_DIST # range to start looking for enemy
for uav in self._reds.values():
if (uav.vehicle_id not in self._shot) and (uav.vehicle_id
not in self._taken):
target_pos = self._reds[
uav.vehicle_id].state.pose.pose.position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
# is the target UAV in front of me?
if gps.hitable(own_lat, own_lon, own_alt, \
(own_orientation.x, \
own_orientation.y, \
own_orientation.z, \
own_orientation.w ), \
np.inf, 180.0, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
self._target_range = d
if (self._target_id in self._shot) or (self._target_id in self._taken):
self._target_id = None
self._target_range = np.inf
# If a target has been identified, move towards it
if self._target_id != None:
msg = apmsg.MsgStat()
msg.id = self._id
msg.count = self._target_id
msg.latency = self._target_range
self.pubs['taken'].publish(msg)
print '{} targeting UAV {} at a distance of {}'.format(
self._id, self._target_id, self._target_range)
print '{} has taken list of {}'.format(self._id, self._taken)
target_pos = self._reds[self._target_id].state.pose.pose.position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
# Set the waypoint past the current target, so we don't go into a
# loiter mode
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 100)
self._wp = np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(own_lat, own_lon, own_alt, \
(own_orientation.x, \
own_orientation.y, \
own_orientation.z, \
own_orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", self._target_id]
print "=================>ID: %d shot at Target %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
self._taken.clear(
) # clear the taken list in case it's full of old data
return True
def step_autonomy(self, t, dt):
#swarmCount = 0
# Reset the action and target ID every loop
self._action = ["None", 0]
#self._target_id = self._last_target
if self._team is None:
if self._id in self._blues:
self._team = 'blue'
else:
self._team = 'red'
(self._goal_lat, self._goal_lon, _) = enums.GOAL_POSITS[self._team]
print "lat %f lon %f" % (self._goal_lat, self._goal_lon)
#go to waypoints for storm formation
if self._got_target is False and self._goal_lat is not None:
#self._in_swarm = True
if self._id%5 == 0:
storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + math.pi/2.0, 0)
self._wp = np.array([storm_lat, storm_lon, 500])
#self._safe_waypoint_reserve 35.720785, -120.765860
print "ID: %d sent RESERVE %f, %f\n" % (self._id, storm_lat, storm_lon)
if self._id%5 == 1:
storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT), 100)
self._wp = np.array([storm_lat, storm_lon, 500])
#self._wp = np.array([125, 375, 250])#self._safe_waypoint_nw 35.720785, -120.765860
print "ID: %d sent NW %f, %f\n" % (self._id, storm_lat, storm_lon)
if self._id%5 == 2:
storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + math.pi, 100)
self._wp = np.array([storm_lat, storm_lon, 500])
#self._wp = np.array([125, 125, 300])#self._safe_waypoint_sw 35.719981, -120.769101
print "ID: %d sent SW %f, %f\n" % (self._id, storm_lat, storm_lon)
if self._id%5 == 3:
storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + math.pi/4.0, 100)
self._wp = np.array([storm_lat, storm_lon, 500])
#self._wp = np.array([375, 375, 200])#self._safe_waypoint_ne 35.720785, -120.765860
print "ID: %d sent NE %f, %f\n" % (self._id, storm_lat, storm_lon)
if self._id%5 == 4:
storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + 3.0*math.pi/4.0, 100)
self._wp = np.array([storm_lat, storm_lon, 500])
#self._wp = np.array([375, 125, 350])#self._safe_waypoint_se 35.719981, -120.769101
print "ID: %d sent SE %f, %f\n" % (self._id, storm_lat, storm_lon)
# Search for the closest target every frame if you don't already have one
min_dist = self._target_dist # self._min_dist
if self._got_target is False:
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
self._last_target = self._target_id
if d < self._max_range:
self._got_target = True
self._in_swarm = False
print "UAV %d within range of Target %d\n" % (self._id, self._target_id)
break
else:
self._got_target = False
self._target_id = None
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
if self._last_target == uav.vehicle_id:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < self._max_range:
self._target_id = self._last_target
if self._got_target is False: # only print once
print "UAV %d within range of Target %d\n" % (self._id, self._target_id)
self._got_target = True
else: # was a match, but d > MIN_DIST, reset all
print "UAV %d out of range of Target %d\n" % (self._id, self._last_target)
self._got_target = False
self._target_id = None
self._last_target = None
# If a target has been identified, move towards it
if self._target_id != None:
target_pos = self._reds[self._target_id].state.pose.pose.position
#print "*************************************"
#print "ID: %d Target ID: %d\n" % (self._id, self._target_id)
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
# Set the waypoint past the current target, so we don't go into a
# loiter mode
if self._got_target is True:
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, tgt_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
#self._action = ["None", 0]
print "Storm Shooter==========================>UAV %d shot at Target %d" % (self._id, self._target_id)
#self._last_target = None
#self._got_target = False
#else:
# Start at the safe waypoint
#self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
msg = apmsg.MsgStat()
msg.id = self._id
############################################################
# If self._target_id is not None and in the shot group
# then reset to None, otherwise set to last_target
############################################################
min_dist = self._target_dist
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
if uav.vehicle_id not in self._taken:
uav_lat = uav.state.pose.pose.position.lat
uav_lon = uav.state.pose.pose.position.lon
uav_alt = uav.state.pose.pose.position.rel_alt
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav_lat, uav_lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
self._target_range = d
if self._target_id in self._shot:
self._target_id = None
self._target_dist = np.inf
self._target_count = 0
self._target_last_lat = None
self._target_last_lon = None
self._target_heading = None
self._target_speed = None
# If a target has been identified, move towards it
if self._target_id != None:
msg.count = self._target_id
msg.latency = self._target_range
self.pubs['my_topic'].publish(msg)
print '{} targeting UAV {} at a distance of {}'.format(
self._id, msg.count, msg.latency)
print '{} has taken list of {}'.format(self._id, self._taken)
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
if self._last_lat is not None:
self._speed = gps.gps_distance(own_lat, own_lon,\
self._last_lat, self._last_lon) / 0.1
self._heading = gps.gps_bearing(self._last_lat, self._last_lon,
own_lat, own_lon)
self._last_lat = own_lat
self._last_lon = own_lon
target_pos = self._reds[self._target_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
tgt_lat, tgt_lon)
if self._target_last_lat is not None:
self._target_speed = gps.gps_distance(tgt_lat, tgt_lon,\
self._target_last_lat, self._target_last_lon) / 0.1
self._target_heading = gps.gps_bearing(self._target_last_lat,
self._target_last_lon,
tgt_lat, tgt_lon)
#print "ID: %d Target: %d" % (self._id, self._target_id)
#print "Lat: %f Lon: %f Head: %f T_lat: %f T_lon %f T_head: %f" % (own_lat, own_lon, self._heading, \
#tgt_lat, tgt_lon, self._target_heading)
self._target_last_lat = tgt_lat
self._target_last_lon = tgt_lon
#############################################################################
# Calc intercept point
# Reference: http://jaran.de/goodbits/2011/07/17/calculating-an-intercept-course-to-a-target-with-constant-direction-and-velocity-in-a-2-dimensional-plane/
# Translated from Java to Python
#############################################################################
t = None
if self._target_heading is not None:
s = self._speed
ox = tgt_lat - own_lat
oy = tgt_lon - own_lon
vx = self._target_speed * math.cos(self._target_heading) # sin
vy = self._target_speed * math.sin(self._target_heading) # cos
h1 = vx**2 + vy**2 - s**2
h2 = ox * vx + oy * vy
if h1 == 0:
t = -(ox**2 + oy**2) / (2 * h2)
else:
minusPHalf = -h2 / h1
disc = minusPHalf**2 - (ox**2 + oy**2) / h1
if disc < 0:
t = None
else:
root = math.sqrt(disc)
t1 = minusPHalf + root
t2 = minusPHalf - root
tmin = t1 if t1 < t2 else t2
tmax = t1 if t1 > t2 else t2
t = tmin if tmin > 0 else tmax
if t < 0 or d < self._max_range: # close enough so go right to him (switch to greedy shooter)
t = None
else:
t *= 1.53
if t is not None:
int_lat = tgt_lat + vx * t
int_lon = tgt_lon + vy * t
int_dist = gps.gps_distance(own_lat, own_lon, int_lat, int_lon)
bearing = gps.gps_bearing(own_lat, own_lon, int_lat, int_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, int_dist)
#print "ID: %d using intercept bearing %f to Lat: %f Lon: %f Dist: %f Time: %f" % (self._id, bearing, lat, lon, int_dist, t*10000) # I_lat: %f I_lon: %f t: %f
else:
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing,
dist + 250) # range was 1000
#print "ID: %d using direct bearing %f to Lat: %f Lon: %f Dist: %f" % (self._id, bearing, lat, lon, dist)
# Set the waypoint past the current target, so we don't go into a loiter mode
self._wp = np.array([lat, lon, tgt_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
tgt_lat, tgt_lon, tgt_alt):
self._action = ["Fire", self._target_id]
#print "GS Int=============================>ID: %d shot at Target %d" % (self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
self._taken.clear(
) # clear the taken list in case it's full of old data
return True
def run_behavior(self):
''' Executes one iteration of the behavior
First state is order determination. After the order has been determined
intercept waypoints will be generated for each iteration
'''
if self._behavior_state == GreedyShooter.HUNT_TGT:
self._target_id = self._identify_target()
if self._target_id == None:
self._behavior_state = GreedyShooter.NO_TGTS
else:
self._behavior_state = GreedyShooter.TRACK_TGT
elif self._behavior_state == GreedyShooter.TRACK_TGT:
intercept = self._track_target()
if intercept:
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon,
self._own_pose.pose.pose.position.alt,
( self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height, \
self._target_lat, self._target_lon, \
self._target_alt):
self._envelope_counter = 1
self._behavior_state = GreedyShooter.SHOOT_TGT
self.log_dbug("Target UAV in firing envelope")
else:
self.log_info("Lost contact with target")
self._behavior_state = GreedyShooter.HUNT_TGT
elif self._behavior_state == GreedyShooter.SHOOT_TGT:
intercept = self._track_target()
if intercept:
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon,
self._own_pose.pose.pose.position.alt,
( self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height, \
self._target_lat, self._target_lon, \
self._target_alt):
self._envelope_counter += 1
self._shot_time = rospy.Time.now()
if self._envelope_counter >= GreedyShooter.RQD_IN_ENVELOPE:
self._make_firing_reports()
self._reds_shot.add(self._target_id)
self._behavior_state = GreedyShooter.HUNT_TGT
else:
self.log_dbug("Target UAV out of firing envelope")
self._behavior_state = GreedyShooter.TRACK_TGT
else:
self.log_info("Lost contact with target")
self._behavior_state = GreedyShooter.HUNT_TGT
elif self._behavior_state == GreedyShooter.NO_TGTS:
stdby_wp_cmd = stdmsg.UInt16()
stdby_wp_cmd.data = enums.SWARM_STANDBY_WP
self._behavior_state = GreedyShooter.TERMINAL_STDBY
self._to_wp_publisher.publish(stdby_wp_cmd)
self.log_info("No targets available to greedy shooter--standby")
elif self._behavior_state == GreedyShooter.TERMINAL_STDBY:
pass # Don't actually do anything--just wait
else: # shouldn't get here, but just in case
self.set_ready_state(False)
self.log_warn("Invalid greedy shooter state--disabling")
def step_autonomy(self, t, dt):
# Reset the action every loop
self._action = ["None", 0]
self._target_id = None
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = 400
if self._state == States.new_start:
self.ordered_corners = self.closestCorners()
lat, lon = self.fixed_wing_waypoint(own_lat, own_lon,
self.ordered_corners[0][0],
self.ordered_corners[0][1])
if gps.gps_distance(own_lat, own_lon, self.ordered_corners[0][0],
self.ordered_corners[0][1]) < 50:
#print("Starting patrol!")
self.ordered_corners = self.closestCorners()
self._state = States.patrol
self.check_for_supply_route()
elif self._state == States.patrol:
lat, lon = self.fixed_wing_waypoint(
own_lat, own_lon, self.ordered_corners[2][0],
self.ordered_corners[2][1]) #At the time of ordering,
#3rd closes waypoint is next in path,
#1st closest is waypoint that you're currently at,
#2nd closest is waypoint you visited previously
if gps.gps_distance(own_lat, own_lon, self.ordered_corners[2][0],
self.ordered_corners[2][1]) < 25:
#print("Reached Waypoint at %f,%f!" % (lat, lon))
self.ordered_corners = self.closestCorners()
self.check_for_supply_route()
elif self._state == States.kill_supply:
lat, lon = self.fixed_wing_waypoint(own_lat, own_lon,
self.ordered_corners[1][0],
self.ordered_corners[1][1])
if gps.gps_distance(own_lat, own_lon, self.ordered_corners[1][0],
self.ordered_corners[1][1]) < 50:
#print("Reached supply point at %f,%f!" % (lat, lon))
self.ordered_corners = self.closestCorners()
######################################################################
#If a target is in range, pursue and fire
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target every frame
min_dist = self._weapons_range + 30
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
#We also want to constrain our pursuits to enemies in front of us
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w), \
min_dist, self._fov_width*2, self._fov_height, \
uav.state.pose.pose.position.lat, \
uav.state.pose.pose.position.lon, \
uav.state.pose.pose.position.alt):
min_dist = d
self._target_id = uav.vehicle_id
# If a target has been identified, move towards it
if self._target_id != None:
#print("Target Aquired!")
target_pos = self._reds[self._target_id].state.pose.pose.position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
lat = tgt_lat
lon = tgt_lon
if self._vehicle_type == enums.AC_FIXED_WING:
# When using fixed wing, Set the waypoint past the current
# target, so we don't go into a loiter mode
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._weapons_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
#################################################################################
#Safety to make sure you don't fly out of bounds
rpy = qmath.quat_to_euler((self._own_pose.pose.pose.orientation.x,\
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w))
own_bearing = rpy[2]
left_lat, left_lon = gps.gps_newpos(
own_lat, own_lon, gps.normalize(own_bearing - (math.pi / 2)), 20)
right_lat, right_lon = gps.gps_newpos(
own_lat, own_lon, gps.normalize(own_bearing + (math.pi / 2)), 20)
front_lat, front_lon = gps.gps_newpos(
own_lat, own_lon, gps.normalize(own_bearing + (math.pi / 2)), 50)
if not self._bboxes[0].contains(left_lat, left_lon, own_alt) or \
not self._bboxes[0].contains(right_lat, right_lon, own_alt) or \
not self._bboxes[0].contains(front_lat, front_lon, own_alt):
lat, lon = self.box_center
self._wp = np.array([lat, lon, own_alt])
return True
def handle_fire(self, fire, shooter_snap, target_snap,
shooter_score, target_score):
# Make sure neither shooter nor target are "hit" already
# NOTE shooter won't know if target was hit, so might change this
if shooter_score.already_hit():
self.log_message(self.LOG_INFO, sfire(fire, 'Shooter already hit'))
return
if target_score.already_hit():
self.log_message(self.LOG_INFO, sfire(fire, 'Target already hit'))
return
# Get poses at time of firing for shooter and target
shooter_pose = shooter_snap.get_uav_pose_at(fire.id, fire.time)
target_pose = target_snap.get_uav_pose_at(fire.target, fire.time)
if shooter_pose is None:
self.log_message(self.LOG_INFO, sfire(fire, 'No shooter pose'))
return
if target_pose is None:
self.log_message(self.LOG_INFO, sfire(fire, 'No target pose'))
return
# See if hit occurred
hit = gps_utils.hitable(shooter_pose.lat,
shooter_pose.lon,
shooter_pose.alt,
(shooter_pose.q_x,
shooter_pose.q_y,
shooter_pose.q_z,
shooter_pose.q_w),
self.__weapon_range,
self.__weapon_FOV_w,
self.__weapon_FOV_h,
target_pose.lat,
target_pose.lon,
target_pose.alt)
if not hit:
return
# Hit occurred, handle consequences
# Report the hit
self.log_message(self.LOG_INFO, sfire(fire, 'HIT!'))
# Send network message to GCSs, red & blue UAVs, and other modules
parser = bitmapped_bytes.UShortParser()
parser.value = fire.target
msg = acs_messages.SwarmBehaviorData()
msg.msg_src = self.state.net_id
msg.msg_sub = 0
msg.msg_seq = 0
msg.msg_ack = 0
msg.msg_secs = 0
msg.msg_nsecs = 0
msg.data_type = bitmapped_bytes.HIT_REPORT
msg.data = parser.pack()
msg.msg_fl_rel = False
for i in range(0, 4): # Better odds that it gets through
self.state.blue_network.send_message_to(Socket.ID_BCAST_ALL, msg)
self.state.red_network.send_message_to(Socket.ID_BCAST_ALL, msg)
for module in self.state.modules.values():
module.on_blue_message_received(msg)
module.on_red_message_received(msg)
# Denote successful hit for shooter
shooter_score.record_fire(fire)
# Denote that target has been hit
target_score.set_hit_time(fire.time)
