def compute_intercept_waypoint(self, wpt=None):
''' Computes an intercept waypoint for the UAV to steer to
@param wpt: waypoint object to use for computed values
@return waypoint object (LLA) with the intercept waypoint (or None)
'''
lead_uav = self._pre_check()
if not lead_uav:
return None
# Convenience variables for state info
own_lat = self.own_pose.pose.pose.position.lat
own_lon = self.own_pose.pose.pose.position.lon
own_spd = math.hypot(self.own_pose.twist.twist.linear.x, \
self.own_pose.twist.twist.linear.y)
lead_lat = lead_uav.state.pose.pose.position.lat
lead_lon = lead_uav.state.pose.pose.position.lon
lead_alt = lead_uav.state.pose.pose.position.rel_alt
lead_x_dot = lead_uav.state.twist.twist.linear.x
lead_y_dot = lead_uav.state.twist.twist.linear.y
lead_crs = math.atan2(lead_y_dot, lead_x_dot)
lead_spd = math.hypot(lead_y_dot, lead_x_dot)
# Compute tgt positions and distances for potential calculation
tgt_lat, tgt_lon = gps.gps_newpos(lead_lat, lead_lon, \
(lead_crs + self._follow_angle), \
self._follow_distance)
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
angle = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
x_dist = dist * math.cos(angle)
y_dist = dist * math.sin(angle)
time_to_intercept = 0.0
if own_spd > 0.1:
time_to_intercept = \
gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) / own_spd
time_to_intercept = min(time_to_intercept, self.lookahead)
lead_travel = lead_spd * time_to_intercept
x_travel = lead_travel * lead_x_dot
y_travel = lead_travel * lead_y_dot
# Compute potential values and target waypoint
x_potential = self._dist_coefficient * x_dist + \
self._align_coefficient * lead_x_dot/lead_spd + \
self._intercept_coefficient * x_travel
y_potential = self._dist_coefficient * y_dist + \
self._align_coefficient * lead_y_dot/lead_spd + \
self._intercept_coefficient * y_travel
if not wpt: wpt = brdgmsg.LLA()
wpt.lat, wpt.lon = \
gps.gps_newpos(own_lat, own_lon, \
math.atan2(y_potential, x_potential), \
InterceptCalculator.OVERSHOOT)
if self._alt_mode == InterceptCalculator.BASE_ALT_MODE:
wpt.alt = self._alt
else:
wpt.alt = lead_uav.state.pose.pose.rel_alt + self._alt
self._owner.log_dbug(
"intercept waypoint (lat, lon, alt) = (%f, %f, %f)" %
(wpt.lat, wpt.lon, wpt.alt))
return wpt
def compute_intercept_waypoint(self, wpt=None):
''' Computes an intercept waypoint for the UAV to steer to
@param wpt: waypoint object to use for computed values
@return waypoint object (LLA) with the intercept waypoint (or None)
'''
lead_uav = self._pre_check()
if not lead_uav:
return None
# Convenience variables for state info
own_lat = self.own_pose.pose.pose.position.lat
own_lon = self.own_pose.pose.pose.position.lon
own_spd = math.hypot(self.own_pose.twist.twist.linear.x, \
self.own_pose.twist.twist.linear.y)
lead_lat = lead_uav.state.pose.pose.position.lat
lead_lon = lead_uav.state.pose.pose.position.lon
lead_alt = lead_uav.state.pose.pose.position.rel_alt
lead_x_dot = lead_uav.state.twist.twist.linear.x
lead_y_dot = lead_uav.state.twist.twist.linear.y
lead_crs = math.atan2(lead_y_dot, lead_x_dot)
lead_spd = math.hypot(lead_y_dot, lead_x_dot)
# Compute tgt positions and distances for potential calculation
tgt_lat, tgt_lon = gps.gps_newpos(lead_lat, lead_lon, \
(lead_crs + self._follow_angle), \
self._follow_distance)
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
angle = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
x_dist = dist * math.cos(angle)
y_dist = dist * math.sin(angle)
time_to_intercept = 0.0
if own_spd > 0.1:
time_to_intercept = \
gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) / own_spd
time_to_intercept = min(time_to_intercept, self.lookahead)
lead_travel = lead_spd * time_to_intercept
x_travel = lead_travel * lead_x_dot
y_travel = lead_travel * lead_y_dot
# Compute potential values and target waypoint
x_potential = self._dist_coefficient * x_dist + \
self._align_coefficient * lead_x_dot/lead_spd + \
self._intercept_coefficient * x_travel
y_potential = self._dist_coefficient * y_dist + \
self._align_coefficient * lead_y_dot/lead_spd + \
self._intercept_coefficient * y_travel
if not wpt: wpt = brdgmsg.LLA()
wpt.lat, wpt.lon = \
gps.gps_newpos(own_lat, own_lon, \
math.atan2(y_potential, x_potential), \
InterceptCalculator.OVERSHOOT)
if self._alt_mode == InterceptCalculator.BASE_ALT_MODE:
wpt.alt = self._alt
else:
wpt.alt = lead_uav.state.pose.pose.rel_alt + self._alt
self._owner.log_dbug("intercept waypoint (lat, lon, alt) = (%f, %f, %f)"
%(wpt.lat, wpt.lon, wpt.alt))
return wpt
def compute_intercept_waypoint(self, wpt=None):
''' Computes an intercept waypoint for the UAV to steer to
@param wpt: waypoint object to use for computed values
@return waypoint object (LLA) with the intercept waypoint (or None)
'''
lead_uav = self._pre_check()
if not lead_uav:
return None
# Update pursuer and target position and velocity info
lead_lat, lead_lon = \
gps.gps_newpos(lead_uav.state.pose.pose.position.lat, \
lead_uav.state.pose.pose.position.lon, \
self._follow_distance, self._follow_angle)
lead_spd = math.hypot(lead_uav.state.twist.twist.linear.x, \
lead_uav.state.twist.twist.linear.y)
lead_crs = math.atan2(lead_uav.state.twist.twist.linear.y, \
lead_uav.state.twist.twist.linear.x)
own_lat = self.own_pose.pose.pose.position.lat
own_lon = self.own_pose.pose.pose.position.lon
own_spd = math.hypot(self.own_pose.twist.twist.linear.x, \
self.own_pose.twist.twist.linear.y)
if (own_spd - lead_spd) < 1.0: own_spd = lead_spd + 1.0
own_crs = math.atan2(self.own_pose.twist.twist.linear.y, \
self.own_pose.twist.twist.linear.x)
# Compute the control parameters
self._theta = gps.gps_bearing(own_lat, own_lon, lead_lat, lead_lon)
tgt_distance = gps.gps_distance(own_lat, own_lon, lead_lat, lead_lon)
crossing_spd = own_spd * math.sin(self._theta - own_crs) + \
lead_spd * math.sin(self._theta + math.pi - lead_crs)
self._theta_dot = crossing_spd / tgt_distance
theta_diff = gps.normalize_pi(self._theta - own_crs)
self._bias = min(gps.signum(theta_diff) * 2.5, theta_diff / 10.0)
# Use the PN calculation to compute the ordered acceleration
ordered_a = (self._gain * self._theta_dot + self._bias) * own_spd
a_max = min(self._a_max, ((2.0 * math.pow(own_spd, 2.0)) / self._L1))
if math.fabs(ordered_a) > a_max:
ordered_a = gps.signum(ordered_a) * a_max
nu = math.asin((ordered_a * self._L1) / (2.0 * math.pow(own_spd, 2.0)))
# Compute the waypoint command
if not wpt: wpt = brdgmsg.LLA()
wpt.lat, wpt.lon = \
gps.gps_newpos(lead_lat, lead_lon, nu + own_crs, self._L1)
if self._alt_mode == InterceptCalculator.BASE_ALT_MODE:
wpt.alt = self._alt
else:
wpt.alt = lead_uav.state.pose.pose.rel_alt + self._alt
self._owner.log_dbug(
"intercept waypoint (lat, lon, alt) = (%f, %f, %f)" %
(wpt.lat, wpt.lon, wpt.alt))
return wpt
def compute_intercept_waypoint(self, wpt=None):
''' Computes an intercept waypoint for the UAV to steer to
@param wpt: waypoint object to use for computed values
@return waypoint object (LLA) with the intercept waypoint (or None)
'''
lead_uav = self._pre_check()
if not lead_uav:
return None
# Update pursuer and target position and velocity info
lead_lat, lead_lon = \
gps.gps_newpos(lead_uav.state.pose.pose.position.lat, \
lead_uav.state.pose.pose.position.lon, \
self._follow_distance, self._follow_angle)
lead_spd = math.hypot(lead_uav.state.twist.twist.linear.x, \
lead_uav.state.twist.twist.linear.y)
lead_crs = math.atan2(lead_uav.state.twist.twist.linear.y, \
lead_uav.state.twist.twist.linear.x)
own_lat = self.own_pose.pose.pose.position.lat
own_lon = self.own_pose.pose.pose.position.lon
own_spd = math.hypot(self.own_pose.twist.twist.linear.x, \
self.own_pose.twist.twist.linear.y)
if (own_spd - lead_spd) < 1.0: own_spd = lead_spd + 1.0
own_crs = math.atan2(self.own_pose.twist.twist.linear.y, \
self.own_pose.twist.twist.linear.x)
# Compute the control parameters
self._theta = gps.gps_bearing(own_lat, own_lon, lead_lat, lead_lon)
tgt_distance = gps.gps_distance(own_lat, own_lon, lead_lat, lead_lon)
crossing_spd = own_spd * math.sin(self._theta - own_crs) + \
lead_spd * math.sin(self._theta + math.pi - lead_crs)
self._theta_dot = crossing_spd / tgt_distance
theta_diff = gps.normalize_pi(self._theta - own_crs)
self._bias = min(gps.signum(theta_diff) * 2.5, theta_diff / 10.0)
# Use the PN calculation to compute the ordered acceleration
ordered_a = (self._gain * self._theta_dot + self._bias) * own_spd
a_max = min(self._a_max, ((2.0 * math.pow(own_spd, 2.0)) / self._L1))
if math.fabs(ordered_a) > a_max:
ordered_a = gps.signum(ordered_a) * a_max
nu = math.asin((ordered_a * self._L1) / (2.0 * math.pow(own_spd, 2.0)))
# Compute the waypoint command
if not wpt: wpt = brdgmsg.LLA()
wpt.lat, wpt.lon = \
gps.gps_newpos(lead_lat, lead_lon, nu + own_crs, self._L1)
if self._alt_mode == InterceptCalculator.BASE_ALT_MODE:
wpt.alt = self._alt
else:
wpt.alt = lead_uav.state.pose.pose.rel_alt + self._alt
self._owner.log_dbug("intercept waypoint (lat, lon, alt) = (%f, %f, %f)"
%(wpt.lat, wpt.lon, wpt.alt))
return wpt
def compute_intercept_waypoint(self, wpt=None):
''' Computes an intercept waypoint for the UAV to steer to
@param wpt: waypoint object to use for computed values
@return waypoint object (LLA) with the intercept waypoint (or None)
'''
lead_uav = self._pre_check()
if not lead_uav:
return None
# Grab & compute required own & leader state info
own_lat = self.own_pose.pose.pose.position.lat
own_lon = self.own_pose.pose.pose.position.lon
own_crs = math.atan2(self.own_pose.twist.twist.linear.y, \
self.own_pose.twist.twist.linear.x)
own_spd = math.hypot(self.own_pose.twist.twist.linear.x, \
self.own_pose.twist.twist.linear.y)
lead_lat = lead_uav.state.pose.pose.position.lat
lead_lon = lead_uav.state.pose.pose.position.lon
lead_alt = lead_uav.state.pose.pose.position.rel_alt
lead_crs = math.atan2(lead_uav.state.twist.twist.linear.y, \
lead_uav.state.twist.twist.linear.x)
lead_spd = math.hypot(lead_uav.state.twist.twist.linear.x, \
lead_uav.state.twist.twist.linear.y)
# Compute tgt point & project it forward to intercept + l1_distance
tgt_lat, tgt_lon = gps.gps_newpos(lead_lat, lead_lon, \
(lead_crs + self._follow_angle), \
self._follow_distance)
time_to_intercept = 0.0
if own_spd > 0.1:
time_to_intercept = \
gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) / own_spd
time_to_intercept = min(time_to_intercept, self.lookahead)
tgt_travel = (lead_spd * time_to_intercept) + self.l1_distance
tgt_lat, tgt_lon = \
gps.gps_newpos(tgt_lat, tgt_lon, lead_crs, tgt_travel)
# compute a wpt along the line from the current posit to the intercept
if not wpt: wpt = brdgmsg.LLA()
to_tgt = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
wpt.lat, wpt.lon = \
gps.gps_newpos(own_lat, own_lon, to_tgt, InterceptCalculator.OVERSHOOT)
if self._alt_mode == InterceptCalculator.BASE_ALT_MODE:
wpt.alt = self._alt
else:
wpt.alt = lead_uav.state.pose.pose.rel_alt + self._alt
self._owner.log_dbug(
"intercept waypoint (lat, lon, alt) = (%f, %f, %f)" %
(wpt.lat, wpt.lon, wpt.alt))
return wpt
def compute_intercept_waypoint(self, wpt=None):
''' Computes an intercept waypoint for the UAV to steer to
@param wpt: waypoint object to use for computed values
@return waypoint object (LLA) with the intercept waypoint (or None)
'''
lead_uav = self._pre_check()
if not lead_uav:
return None
# Grab & compute required own & leader state info
own_lat = self.own_pose.pose.pose.position.lat
own_lon = self.own_pose.pose.pose.position.lon
own_crs = math.atan2(self.own_pose.twist.twist.linear.y, \
self.own_pose.twist.twist.linear.x)
own_spd = math.hypot(self.own_pose.twist.twist.linear.x, \
self.own_pose.twist.twist.linear.y)
lead_lat = lead_uav.state.pose.pose.position.lat
lead_lon = lead_uav.state.pose.pose.position.lon
lead_alt = lead_uav.state.pose.pose.position.rel_alt
lead_crs = math.atan2(lead_uav.state.twist.twist.linear.y, \
lead_uav.state.twist.twist.linear.x)
lead_spd = math.hypot(lead_uav.state.twist.twist.linear.x, \
lead_uav.state.twist.twist.linear.y)
# Compute tgt point & project it forward to intercept + l1_distance
tgt_lat, tgt_lon = gps.gps_newpos(lead_lat, lead_lon, \
(lead_crs + self._follow_angle), \
self._follow_distance)
time_to_intercept = 0.0
if own_spd > 0.1:
time_to_intercept = \
gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) / own_spd
time_to_intercept = min(time_to_intercept, self.lookahead)
tgt_travel = (lead_spd * time_to_intercept) + self.l1_distance
tgt_lat, tgt_lon = \
gps.gps_newpos(tgt_lat, tgt_lon, lead_crs, tgt_travel)
# compute a wpt along the line from the current posit to the intercept
if not wpt: wpt = brdgmsg.LLA()
to_tgt = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
wpt.lat, wpt.lon = \
gps.gps_newpos(own_lat, own_lon, to_tgt, InterceptCalculator.OVERSHOOT)
if self._alt_mode == InterceptCalculator.BASE_ALT_MODE:
wpt.alt = self._alt
else:
wpt.alt = lead_uav.state.pose.pose.rel_alt + self._alt
self._owner.log_dbug("intercept waypoint (lat, lon, alt) = (%f, %f, %f)"
%(wpt.lat, wpt.lon, wpt.alt))
return wpt
def _setup_state(self):
''' Performs calculations and operations required for the "setup" state
NOTE: Deprecated
'''
# If init required, set waypoint fwd of the target by the run-in distance
if not self._state_initialized:
self.log_info("Commence setup phase")
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xt[0], self._xt[1], self._xt[2], \
PNInterceptor.R_RUNIN)
self.wp_msg.alt = self._ap_intent.z
self.publishWaypoint(self.wp_msg)
self._state_initialized = True
self.log_dbug("Setup waypoint order to lat %f, lon %f"\
%(self.wp_msg.lat, self.wp_msg.lon))
self._range = gps.gps_distance(self._xp[0], self._xp[1], \
self._xt[0], self._xt[1])
# Keep going until we're in front of the target
# Then set the waypoint to the target location for the run-in
if gps.gps_distance(self._xp[0], self._xp[1], \
self.wp_msg.lat, self.wp_msg.lon) < \
PNInterceptor.R_CAP_SETUP:
self._state_initialized = False
self._state = PNInterceptor.RUN_IN
self.log_info("Transition to run-in phase")
def _setup_state(self):
''' Performs calculations and operations required for the "setup" state
NOTE: Deprecated
'''
# If init required, set waypoint fwd of the target by the run-in distance
if not self._state_initialized:
self.log_info("Commence setup phase")
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xt[0], self._xt[1], self._xt[2], \
PNInterceptor.R_RUNIN)
self.wp_msg.alt = self._ap_intent.z
self.publishWaypoint(self.wp_msg)
self._state_initialized = True
self.log_dbug("Setup waypoint order to lat %f, lon %f"\
%(self.wp_msg.lat, self.wp_msg.lon))
self._range = gps.gps_distance(self._xp[0], self._xp[1], \
self._xt[0], self._xt[1])
# Keep going until we're in front of the target
# Then set the waypoint to the target location for the run-in
if gps.gps_distance(self._xp[0], self._xp[1], \
self.wp_msg.lat, self.wp_msg.lon) < \
PNInterceptor.R_CAP_SETUP:
self._state_initialized = False
self._state = PNInterceptor.RUN_IN
self.log_info("Transition to run-in phase")
def intercept_target(self, target_id, alt_deconflict=True):
'''Returns a waypoint that will intercept the target.
If the vehicle is a fixed-wing, the waypoint will be extended beyond
the waypoint loiter distance
'''
own_pos = self._own_pose.pose.pose.position
target_pos = self._reds[target_id].state.pose.pose.position
own_lla = (own_pos.lat, own_pos.lon, own_pos.alt)
tgt_lla = (target_pos.lat, target_pos.lon, target_pos.alt)
lat = tgt_lla[0]
lon = tgt_lla[1]
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_lla[0], own_lla[1], tgt_lla[0],
tgt_lla[1])
lat, lon = gps.gps_newpos(own_lla[0], own_lla[1], bearing, 1000)
wp = np.array([lat, lon, tgt_lla[2]])
# Handle altitude deconfliction
if alt_deconflict:
wp[2] = self._last_ap_wp[2]
return wp
def step_autonomy(self, t, dt):
# Go to desired latitude, longitude, and maintain altitude
# deconfliction:
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, self._desired_lat,
self._desired_lon))
lat = self._desired_lat
lon = self._desired_lon
dist = gps.gps_distance(own_lat, own_lon, self._desired_lat,
self._desired_lon)
bearing_adj = 0.0
if dist > 100:
bearing_adj = math.pi / 4 * (-1 + self._id % 2 * 2)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj,
1000)
if bearing_adj > 0:
print 'ID: {} orbiting CW'.format(self._id)
else:
print 'ID: {} orbiting CCW'.format(self._id)
self._wp = np.array([lat, 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]
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 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 get_battleboxes():
''' Utility function for returning a tuple of relevant battle cube objects
@return tuple of objects as follows (all GeoBox exc centerline
which is a tuple of (lat, lon) endpoints):
(battleBox, centerline, blueBox, blueStage, redBox, redStage)
'''
bBox = gps.GeoBox(BATTLE_CUBE_SW_LAT, BATTLE_CUBE_SW_LON, \
BATTLE_CUBE_LENGTH, BATTLE_CUBE_WIDTH, \
BATTLE_CUBE_ORIENT, BATTLE_CUBE_MIN_ALT, \
BATTLE_CUBE_MAX_ALT)
if SITL_LOCATION == 3:
ctr1 = (BATTLE_CUBE_SW_LAT, BATTLE_CUBE_SW_LON)
elif SITL_LOCATION == 4:
ctr1 = (BATTLE_CUBE_SW_LAT, BATTLE_CUBE_SW_LON)
else:
ctr1 = gps.gps_newpos(BATTLE_CUBE_SW_LAT, \
BATTLE_CUBE_SW_LON, \
math.radians(BATTLE_CUBE_ORIENT) + math.pi/2.0, \
BATTLE_CUBE_WIDTH / 2.0)
ctr2 = gps.gps_newpos(ctr1[0], ctr1[1],
math.radians(BATTLE_CUBE_ORIENT), \
BATTLE_CUBE_LENGTH)
centerLine = (ctr1, ctr2)
blueBox = gps.GeoBox(BATTLE_CUBE_SW_LAT, BATTLE_CUBE_SW_LON, \
BATTLE_CUBE_LENGTH, BATTLE_CUBE_WIDTH / 2.0, \
BATTLE_CUBE_ORIENT, BATTLE_CUBE_MIN_ALT, \
BATTLE_CUBE_MAX_ALT)
blueStage = gps.GeoBox(BLUE_STAGE_SW_LAT, BLUE_STAGE_SW_LON, \
BATTLE_CUBE_LENGTH, STAGE_CUBE_WIDTH, \
BATTLE_CUBE_ORIENT, 0.0, BATTLE_CUBE_MAX_ALT)
redBox_SW_Corner = gps.gps_newpos(BATTLE_CUBE_SW_LAT, \
BATTLE_CUBE_SW_LON, \
math.radians(BATTLE_CUBE_ORIENT) + math.pi / 2.0, \
BATTLE_CUBE_WIDTH / 2.0)
redBox = gps.GeoBox(redBox_SW_Corner[0], redBox_SW_Corner[1], \
BATTLE_CUBE_LENGTH, BATTLE_CUBE_WIDTH / 2.0, \
BATTLE_CUBE_ORIENT, BATTLE_CUBE_MIN_ALT, \
BATTLE_CUBE_MAX_ALT)
redStage = gps.GeoBox(RED_STAGE_SW_LAT, RED_STAGE_SW_LON, \
BATTLE_CUBE_LENGTH, STAGE_CUBE_WIDTH, \
BATTLE_CUBE_ORIENT, 0.0, BATTLE_CUBE_MAX_ALT)
return tuple((bBox, centerLine, blueBox, blueStage, redBox, redStage))
def _send_ctrl_wp(self):
''' Uses the computed gain, bias, and theta_dot to compute a waypoint
'''
v_p = math.hypot(self._vp[0], self._vp[1])
a_p = (self._gain * self._theta_dot + self._bias) * v_p
if math.fabs(a_p) > PNInterceptor.A_P_MAX:
a_p = gps.signum(a_p) * PNInterceptor.A_P_MAX
L1 = 150.0 #min((v_p * 5.0 * self._dt), self._range)
nu = math.asin((a_p * L1) / (2.0 * math.pow(v_p, 2.0)))
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xp[0], self._xp[1], nu + self._xp[2], L1)
self.publishWaypoint(self.wp_msg)
def _send_ctrl_wp(self):
''' Uses the computed gain, bias, and theta_dot to compute a waypoint
'''
v_p = math.hypot(self._vp[0], self._vp[1])
a_p = (self._gain * self._theta_dot + self._bias) * v_p
if math.fabs(a_p) > PNInterceptor.A_P_MAX:
a_p = gps.signum(a_p) * PNInterceptor.A_P_MAX
L1 = 150.0 #min((v_p * 5.0 * self._dt), self._range)
nu = math.asin((a_p * L1) / (2.0 * math.pow(v_p, 2.0)))
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xp[0], self._xp[1], nu + self._xp[2], L1)
self.publishWaypoint(self.wp_msg)
def _parallel_1_state(self):
''' Performs calculations and operations required for the "parallel 1" state
'''
if not self._state_initialized:
bearing = gps.normalize(self._xt[2] - math.pi)
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xp[0], self._xp[1], bearing, 10000.0)
self.publishWaypoint(self.wp_msg)
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt)
if self._range <= PNInterceptor.R_TH2:
self._state = PNInterceptor.PARALLEL_2
self._gain = 0.0
self._bias = 0.0
self._state_initialized = False
self.log_info("Transition to parallel_2 state")
def _parallel_1_state(self):
''' Performs calculations and operations required for the "parallel 1" state
'''
if not self._state_initialized:
bearing = gps.normalize(self._xt[2] - math.pi)
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xp[0], self._xp[1], bearing, 10000.0)
self.publishWaypoint(self.wp_msg)
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt)
if self._range <= USMAInterceptor.R_TH2:
self._state = USMAInterceptor.PARALLEL_2
self._gain = 0.0
self._bias = 0.0
self._state_initialized = False
self.log_info("Transition to parallel_2 state")
def _send_ctrl_wp(self):
''' Uses the computed gain, bias, and theta_dot to compute a waypoint
'''
v_p = math.hypot(self._vp[0], self._vp[1])
a_p = (self._gain * self._theta_dot + self._bias) * v_p
if math.fabs(a_p) > USMAInterceptor.A_P_MAX:
a_p = gps.signum(a_p) * USMAInterceptor.A_P_MAX
L1 = 150.0 #min((v_p * 5.0 * self._dt), self._range)
nu = 0
if v_p != 0:
nu = math.asin((a_p * L1) / (2.0 * math.pow(v_p, 2.0)))
self.wp_msg = self._own_pose.pose.pose.position
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xp[0], self._xp[1], nu + self._xp[2], L1)
self._wp = numpy.array(
[self.wp_msg.lat, self.wp_msg.lon,
self.wp_msg.rel_alt]) # publishWaypoint(self.wp_msg)
def vector_2_waypoint(self, vec, alt_deconflict=True):
'''Convert velocity in local cartesian frame to waypoint in GPS
Convert a velocity in the local cartesian frame to a waypoint in GPS
frame. if alt_deconflict is True, then the current altitude is
maintained.
'''
lla = self.state.local_2_lla(self.state.pos[:2] + vec)
if self._vehicle_type == enums.AC_FIXED_WING:
# Get vector's direction
theta = math.atan2(vec[1], vec[0])
# Extend waypoint in direction of desired velocity
lat, lon = gps.gps_newpos(lla[0], lla[1], from_gps_heading(theta),
1000)
wp = np.array([lat, lon, lla[2]])
if alt_deconflict:
wp[2] = self._last_ap_wp[2]
return wp
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
BATTLE_CUBE_WIDTH = 660 # E/W dimension (meters) of the battle cube
BATTLE_CUBE_ORIENT = 55.32 # Battle cube orientation (clockwise degrees)
BATTLE_CUBE_MIN_ALT = 267 # Battle cube floor (meters MSL)
BATTLE_CUBE_MAX_ALT = 500 # Battle cube ceiling (meters MSL)
else:
BATTLE_CUBE_SW_LAT = 35.720680 # Latitude of the battle cube SW corner
BATTLE_CUBE_SW_LON = -120.771775 # Longitude of the battle cube SW corner
BATTLE_CUBE_LENGTH = 500 # N/S dimension (meters) of the battle cube
BATTLE_CUBE_WIDTH = 500 # E/W dimension (meters) of the battle cube
BATTLE_CUBE_ORIENT = 25.183537917993224 # Battle cube orientation (clockwise degrees)
BATTLE_CUBE_MIN_ALT = 354 # Battle cube floor (meters MSL)
BATTLE_CUBE_MAX_ALT = 854 # Battle cube ceiling (meters MSL)
BATTLE_CUBE_CTR_LAT, BATTLE_CUBE_CTR_LON = \
gps.gps_newpos(BATTLE_CUBE_SW_LAT, BATTLE_CUBE_SW_LON, \
math.radians(BATTLE_CUBE_ORIENT) + math.pi/2.0, \
BATTLE_CUBE_WIDTH / 2.0)
if SITL_LOCATION == 2:
BLUE_STAGE_SW_LAT = 41.390547
BLUE_STAGE_SW_LON = -73.952655
RED_STAGE_SW_LAT = 41.391474
RED_STAGE_SW_LON = -73.952205
STAGE_CUBE_WIDTH = 30
elif SITL_LOCATION == 1:
BLUE_STAGE_SW_LAT = 41.359965
BLUE_STAGE_SW_LON = -74.030650
RED_STAGE_SW_LAT = 41.356595
RED_STAGE_SW_LON = -74.030420
STAGE_CUBE_WIDTH = 100
elif SITL_LOCATION == 3:
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]
############################################################
# 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 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._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 = -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 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 = 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):
# 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 _offset_state(self):
''' Performs calculations and operations required for the "offset" state
'''
if not self._state_initialized:
# Set the virtual waypoint D2 meters along the target's path and
# offset by D1 meters to the side that the pursuer is currently on
theta = gps.gps_bearing(self._xp[0], self._xp[1], \
self._xt[0], self._xt[1])
offset_dir = \
gps.signum(gps.normalize_pi(theta + math.pi - self._xt[2]))
tgt_to_VT = self._xt[2] + math.atan2(PNInterceptor.D1 * offset_dir, \
PNInterceptor.D2)
self._VT = \
gps.gps_newpos(self._xt[0], self._xt[1], tgt_to_VT, \
math.hypot(PNInterceptor.D1, PNInterceptor.D2))
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
self._theta_VT_init = self._theta
self._theta_dot_VT_init = self._theta_dot
self._psi_p_VT_init = self._xp[2]
self._psi_p_t_f = gps.normalize(self._xt[2] + math.pi)
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
if self._range < PNInterceptor.R_CAP_PARALLEL:
self._state_initialized = False
self._state = PNInterceptor.PARALLEL_1
self.log_info("Transition to parallel_1 phase")
# Determine how to maneuver to the virtual target point
# Direct turn to the virtual target
if (self._theta_VT_init <= self._psi_p_t_f) and \
(self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - self._psi_p_VT_init)):
self._gain = math.fabs((self._psi_p_t_f - self._psi_p_VT_init) / \
(self._psi_p_t_f - self._theta_VT_init))
self._bias = 0.0
# Single turn to virtual target with 2 gains
elif ((-math.pi - self._psi_p_VT_init + \
(2.0 * self._theta_VT_init)) <= self._psi_p_t_f) and \
(self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - \
self._psi_p_VT_init)):
if (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) < 2.0:
self._gain = (2.0 / math.pi) * self._psi_p_VT_init
self._bias = 0.0
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) == 2.0:
self._gain = 2.0
self._bias = 0.0
else: # should never get here
self.log_warn("Unable to determine phase 2 2-step case")
self.set_ready_state(False)
# S-turn to virtual target
else:
if gps.signum(self._theta_dot) == \
gps.signum(self._theta_dot_VT_init):
self._gain = 2.5
self._bias = 0.1 * gps.signum(self._theta_dot_VT_init)
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) < 2.0:
self._gain = 2.5
self._bias = 0.05 * gps.signum(self._theta_dot_VT_init)
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) >= 2.0:
self._gain = math.fabs((self._psi_p_t_f - self._xp[2]) / \
(self._psi_p_t_f - self._theta))
self._bias = 0.0
else: # should never get here
self.log_warn("Unable to determine phase 2 3-step case")
self.set_ready_state(False)
def fixed_wing_waypoint(self, own_lat, own_lon, waypoint_lat,
waypoint_lon):
bearing = gps.gps_bearing(own_lat, own_lon, waypoint_lat, waypoint_lon)
return gps.gps_newpos(own_lat, own_lon, bearing, 1000)
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 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]
# 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):
# 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 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 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