def sub_recv_pose(msg):
if msg.vehicle_id not in msg_rates:
msg_rates[msg.vehicle_id] = ap_msg.MsgStat()
msg_rates[msg.vehicle_id].id = msg.vehicle_id
# Increment count of messages seen this period
msg_rates[msg.vehicle_id].count += 1
# Accumulate latency (will be averaged during re-publish)
time_sent = msg.state.header.stamp
time_rcvd = msg.received_at
latency = float(str(time_rcvd - time_sent)) / 1e9
msg_rates[msg.vehicle_id].latency += latency
def receive_msg_stat(self, msg):
#print '{} received message from {} with latency {}'.format(self._id, msg.id, msg.latency)
print '{} received message from {}: Targeting {} at distance {}'.format(
self._id, msg.id, msg.count, msg.latency)
self._taken.add(msg.count)
if msg.count == self._target_id:
if msg.latency < self._target_range:
#self._target_id = None
#self._target_range = np.inf
t_id.pop()
l = t_id.len() - 1
t_dist.pop()
self._target_id = t_id[l]
self._target_dist = t_dist[l]
print '{} dropped UAV {} due to {} being closer at distance {}'.format(
self._id, msg.count, msg.id, msg.latency)
msg = apmsg.MsgStat()
msg.id = self._id
msg.count = self._target_id
msg.latency = self._target_range
self.pubs['taken'].publish(msg)
else:
self._taken.discard(
msg.count) # remove from my list if I am close
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 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):
# 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
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):
# 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