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 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 _identify_target(self):
''' Determines which red UAV will be targeted
Will always be the closest red UAV that has not already been shot
@return the ID (int) of the UAV to target (None if no targets left)
'''
t = rospy.Time.now()
self._target_id = None
min_range = 1e6
with self._reds_lock:
for uav_id in self._reds:
uav = self._reds[uav_id]
if uav_id not in self._reds_shot and \
(t - uav.state.header.stamp) < GreedyShooter.MAX_TIME_LATE:
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_range:
self._target_id = uav_id
min_range = d
if self._target_id == None:
self.log_info("no targets identified for greedy shooter")
else:
self.log_info("greedy shooter identified red UAV %d as target" \
%self._target_id)
self._wp_calc.set_params(self._target_id, 0.0, 0.0, \
self._ap_intent.z, \
pputils.InterceptCalculator.BASE_ALT_MODE)
return self._target_id
def identify_target(self, t):
self._target_id = None
self_pos = self._own_pose.pose.pose.position
min_range = np.inf
for uav_id in self._reds:
red_veh_state = self._reds[uav_id]
red_pos = red_veh_state.state.pose.pose.position
shot = uav_id in self._shot
pursued = uav_id in self.reds_pursued
active_game = \
(red_veh_state.game_status == enums.GAME_ACTIVE_DEFENSE or
red_veh_state.game_status == enums.GAME_ACTIVE_OFFENSE)
recent_pose = \
(t - red_veh_state.state.header.stamp.to_sec() <
self.MAX_TIME_LATE.to_sec())
if not shot and not pursued and recent_pose and active_game:
d = gps.gps_distance(self_pos.lat, self_pos.lon, red_pos.lat,
red_pos.lon)
if d < min_range:
self._target_id = uav_id
self.target_dist = d
min_range = d
if self._target_id:
self.pursuit_status = True
self.wp_calc.set_params(self._target_id, 0.0, 0.0, self_pos.alt,
pputils.InterceptCalculator.BASE_ALT_MODE)
self.pursuit_status_update()
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:
# print uav.vehicle_id,
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
#print self._blues.values()
# If a target has been identified, move towards it
if self._target_id != None:
#print self._reds[self._target_id]
target_pos = self._reds[self._target_id].state.pose.pose.position
# s = ", ".join(str(e) for e in self._shot)
# print "Reds Shot: "
# print s
#print "ID: %d Target ID: %d Dist: %d" % (self._id, self._target_id, min_dist)
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
# Set the waypoint past the current target, so we don't go into a
# loiter mode
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
#print "=====>ID: %d shot at Target %d" % (self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target every frame
min_dist = np.inf
for uav in self._reds.values():
if uav.vehicle_id not in self._shot and (uav.game_status == enums.GAME_ACTIVE_DEFENSE or uav.game_status == enums.GAME_ACTIVE_OFFENSE):
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
# If a target has been identified, move towards it
if self._target_id != None:
target_pos = self._reds[self._target_id].state.pose.pose.position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
lat = tgt_lat
lon = tgt_lon
if self._vehicle_type == enums.AC_FIXED_WING:
# When using fixed wing, Set the waypoint past the current
# target, so we don't go into a loiter mode
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, target_pos.rel_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
# Maintain altitude
self._wp[2] = self._last_ap_wp[2]
return True
def 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 closestCorners(self):
order = []
uav_lat = self._own_pose.pose.pose.position.lat
uav_lon = self._own_pose.pose.pose.position.lon
for corner in self._safe_corners:
if order == []:
order.append(corner)
else:
index = 0
inserted = False
for otherCorner in order:
if not inserted and gps.gps_distance(
uav_lat, uav_lon, corner[0],
corner[1]) < gps.gps_distance(
uav_lat, uav_lon, otherCorner[0],
otherCorner[1]):
order.insert(index, corner)
inserted = True
index += 1
if inserted == False:
order.append(corner)
return order
def _set_ctrl_params(self, own_state, own_vel, tgt_state, tgt_vel):
''' Sets the control parameter values based on own and target state
@param own_state: pursuer vehicle state (lat, lon, psi)
@param own_vel: pursuer velocity (x_dot, y_dot)
@param tgt_state: target vehicle state (lat, lon, psi)
@param tgt_vel: target velocity (x_dot, y_dot)
'''
self._range = gps.gps_distance(own_state[0], own_state[1], \
tgt_state[0], tgt_state[1])
self._theta = gps.gps_bearing(own_state[0], own_state[1], \
tgt_state[0], tgt_state[1])
own_spd = math.hypot(own_vel[0], own_vel[1])
tgt_spd = math.hypot(tgt_vel[0], tgt_vel[1])
crossing_spd = own_spd * math.sin(self._theta - own_state[2]) + \
tgt_spd * math.sin(self._theta + math.pi - tgt_state[2])
self._theta_dot = crossing_spd / self._range
def _set_ctrl_params(self, own_state, own_vel, tgt_state, tgt_vel):
''' Sets the control parameter values based on own and target state
@param own_state: pursuer vehicle state (lat, lon, psi)
@param own_vel: pursuer velocity (x_dot, y_dot)
@param tgt_state: target vehicle state (lat, lon, psi)
@param tgt_vel: target velocity (x_dot, y_dot)
'''
self._range = gps.gps_distance(own_state[0], own_state[1], \
tgt_state[0], tgt_state[1])
self._theta = gps.gps_bearing(own_state[0], own_state[1], \
tgt_state[0], tgt_state[1])
own_spd = math.hypot(own_vel[0], own_vel[1])
tgt_spd = math.hypot(tgt_vel[0], tgt_vel[1])
crossing_spd = own_spd * math.sin(self._theta - own_state[2]) + \
tgt_spd * math.sin(self._theta + math.pi - tgt_state[2])
self._theta_dot = crossing_spd / self._range
def step_autonomy(self, t, dt):
# remove any red entries in sorter
to_del = [key for key in self.sorter._data if key in self._reds]
for key in to_del:
del self.sorter._data[key]
self.sorter.send_requested(t)
self.wp_calc._swarm = self._blues
self.sorter._subswarm = set(self.wp_calc._swarm.keys())
if self.behavior_state == States.setup:
self._wp = self._safe_waypoint
if not self.setup_formation(t):
return True
self.behavior_state = States.fly
else:
if self.formation_params['lead']:
pos = self._own_pose.pose.pose.position
if gps.gps_distance(pos.lat, pos.lon, self._wp[0], self._wp[1]) < 2 * self.cube_offset:
print 'selecting a new waypoint'
self.curr_corner = (self.curr_corner + 1) % 4
wp = self.geobox._corners[self.curr_corner]
self._wp = np.array([wp[0], wp[1], self._wp[2]])
elif not self.formation_params['lead']:
wp = self.wp_calc.compute_intercept_waypoint(None, t)
if wp is None:
self.log_warn("calculated a None waypoint to leader in LinearFormation")
else:
self._wp = np.array([wp.lat, wp.lon, wp.alt])
return True
def step_autonomy(self, t, dt):
# Go to desired latitude, longitude, and maintain altitude
# deconfliction:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat, self._own_pose.pose.pose.position.lon, self._desired_lat, self._desired_lon)
if d < self._wp_dist:
if self._step < 3:
if (self._loc[self._step + 1]) in range(0,16): # wp 0-15
self._step += 1
elif (self._loc[self._step + 1]) < 0: # -1
self._step = 0
elif (self._loc[self._step + 1]) > 16:
pass # stay at current waypoint forever
else:
self._step = 0 # go back to beginning
self._desired_lat = float(usma_enums.WP_LOC[self._loc[self._step]][0])
self._desired_lon = float(usma_enums.WP_LOC[self._loc[self._step]][1])
self._wp = np.array([self._desired_lat, self._desired_lon, self._last_ap_wp[2]])
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
# 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]
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):
# Execute this portion of the code on the loop only
if self._first_tick == True:
self._first_tick = False
# Set initial altitude settings
self._desired_alt = self._last_ap_wp[2]
self._original_alt = self._last_ap_wp[2]
# Determine your own subswarm ID
#key #value
for blue_id, blue in self._blues.iteritems():
print "vehicle id = ", blue.vehicle_id, "self._id = ", self._id
if blue.vehicle_id == self._id:
self._subswarm_id = blue.subswarm_id
break
print "subswarm_id: ", self._subswarm_id
# Build a list of all vehicle IDs within your subswarm
# produces the list (and therefore number) of specific swarm
blue_in_subswarm = dict()
i = 0
for blue_id, blue in self._blues.iteritems():
if blue.subswarm_id == self._subswarm_id:
self._id_in_subswarm.append(blue.vehicle_id)
self._subswarm_num_to_assign.append(0)
blue_in_subswarm[i] = blue
i = i + 1
print "id_in_subswarm: ", self._id_in_subswarm
# Divide # of lanes by # of vehicles and create empty bundle of lanes for each
num_in_subwarm = len(
self._id_in_subswarm) # in our case 5 vehicles
for i in range(0, num_in_subwarm):
self._subswarm_num_to_assign[i] = int(
math.floor(len(self.lanes) / num_in_subwarm))
if i < (len(self.lanes) % num_in_subwarm):
self._subswarm_num_to_assign[
i] = self._subswarm_num_to_assign[i] + 1
print "num_to_assign: ", self._subswarm_num_to_assign #should be 2
# Logic for assigning lanes and movement of subswarm
# Loop over each UAS in subswarm.
for i in range(0, num_in_subwarm): #from 0 to 5
# Set the start location to current UAS position
place_in_lane = 0
if i > 0:
place_in_lane += self._subswarm_num_to_assign[i - 1]
temp_lat = blue_in_subswarm[i].state.pose.pose.position.lat
temp_lon = blue_in_subswarm[i].state.pose.pose.position.lon
assignment_list = []
# Loop over each element of the lanes bundle
for j in range(0, self._subswarm_num_to_assign[i]
): #self._sub_num_to_assign[0] = 6
# Loop over each end of lane defined in the mission
if i > 0:
for k in self.lanes[j + place_in_lane]:
coord_to_assign = k
assignment_list.append(coord_to_assign)
self._subswarm_wp_assignment[i] = assignment_list
else:
for k in self.lanes[j]: #looking at both
#coordinates in a single lane--should be 2
coord_to_assign = k #full coordinate
assignment_list.append(coord_to_assign)
self._subswarm_wp_assignment[i] = assignment_list
#print "list for drone: ",i, ": ", self._subswarm_wp_assignment[i], "\n"
# Assign yourself your own bundle of lanes
print "vehicle id: ", blue_in_subswarm[
i].vehicle_id, "self._id: ", self._id
#if blue_in_subswarm[i].vehicle_id == self._id:
if self._id == blue_in_subswarm[i].vehicle_id:
self._wp_id_list = self._subswarm_wp_assignment[i]
#print "vehicle id: ", self._id, " waypoints: ", self._subswarm_wp_assignment[i]
print "it has reached this point"
#self._loc = self._wp_id_list[self._wp_id_list_id]
print "way point id list :", self._wp_id_list
# Proceed to the first Waypoint in the bundle
self._loc = self._wp_id_list[0]
self._desired_lat = float([self._loc][0][0])
self._desired_lon = float([self._loc][0][1])
print "subswarm_wp_assignment: ", self._subswarm_wp_assignment, "\n"
# Proceed to the first coordinate in the lanes bundle
print "Going to coordinate: ", self._loc
# Go to desired latitude, longitude, and maintain altitude
# deconfliction:
self._wp = np.array(
[self._desired_lat, self._desired_lon, self._desired_alt])
# Determine current position
pos = self._own_pose.pose.pose.position
dist = gps.gps_distance(pos.lat, pos.lon, self._desired_lat,
self._desired_lon)
#if close enough to a hotspot (using hotspot threshold as test), send info to one drone on standby
#call hotspot_identified function
if (self._vehicle_type == 2
and dist < DIST2WP_ZEPH) or (self._vehicle_type == 1
and dist < DIST2WP_QUAD):
if self._at_wp == False:
self._time_start = timeit.default_timer()
self._at_wp = True
self._time_at_wp = timeit.default_timer() - self._time_start
else:
self._at_wp = False
self._time_at_wp = 0
# If you reach your point, proceed to the next one
if self._at_wp == True:
self._wp_id_list_id = self._wp_id_list_id + 1
# If you get to the end of your bundle, repeat from its beginning
# and reset to original altitude
if self._wp_id_list_id > (len(self._wp_id_list) - 1):
print("test")
self._wp_id_list_id = 0
self._desired_alt = self._original_alt
self._loc = self._wp_id_list[self._wp_id_list_id]
self._desired_lat = float(self._loc[0])
self._desired_lon = float(self._loc[1])
# Reset these so that UAV knows it's no longer at its goal WP
self._at_wp = False
print "Going to coordinate: ", self._loc
self._wp = np.array(
[self._desired_lat, self._desired_lon, self._desired_alt])
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target every frame
min_dist = np.inf
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon, \
uav.state.pose.pose.position.rel_alt):
self._action = ["Fire", uav.vehicle_id]
print "RT=========(loop)========>ID: %d shot at Target %d" % (
self._id, uav.vehicle_id)
'''if self._last_target_id == self._target_id:
if d < 100: # how close do we have to be before I call chasing
self._target_count += 1
if self._target_count > 100:
self._target_count = 300
self._target_id = None
elif self._target_count > 0:
self._target_count -= 1
self._target_id = None
'''
# If a target has been identified, move towards it
if self._target_id != None:
self._last_target_id = self._target_id
# Calculate target and own position and orientation
target_pos = self._reds[self._target_id].state.pose.pose.position
target_orientation = self._reds[
self._target_id].state.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
own_pos = self._own_pose.pose.pose.position
own_lat = own_pos.lat
own_lon = own_pos.lon
own_alt = own_pos.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
# Calculate bearing to target and from target to self
bearing = gps.normalize(
gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon))
tgt_bearing = gps.normalize(
gps.gps_bearing(tgt_lat, tgt_lon, own_lat, own_lon))
# Calculate heading/yaw of self and target
own_rpy = qmath.quat_to_euler((own_orientation.x,\
own_orientation.y, \
own_orientation.z, \
own_orientation.w))
own_heading = gps.normalize(own_rpy[2])
tgt_rpy = qmath.quat_to_euler((target_orientation.x,\
target_orientation.y, \
target_orientation.z, \
target_orientation.w))
tgt_heading = gps.normalize(tgt_rpy[2])
# Calculate distance to target
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
bearing_adj = 0.0
#print 'ID: {} targeting UAV: {} at bearing {} and distance {}'.format(self._id, self._target_id, math.degrees(bearing), dist)
#print 'UAV: {} own bearing: {}'.format(self._target_id, math.degrees(tgt_heading))
# Calculate offset between bearing and target heading (measures degree of "head on")
# This section needs more work
# ----------------------------------------------------------------------------
heading_diff = own_heading - tgt_heading
tgt_bearing_diff = gps.normalize_pi(tgt_heading - tgt_bearing)
if math.fabs(heading_diff) > math.pi / 2:
# moving towards each other
if tgt_bearing > (
tgt_heading - self._fov_width_radians / 2
) or tgt_bearing < (tgt_heading + self._fov_width_radians / 2):
# we are in the "fan"
if dist > self._max_range and math.fabs(
tgt_bearing_diff) < self._fov_width_radians / 2:
bearing_adj = math.copysign(ADJ_ANGLE,
tgt_bearing_diff)
print 'ID: {} toward UAV: {}: adjusting bearing by {}'.format(
self._id, self._target_id,
math.degrees(bearing_adj))
else:
# heading in same general direction
if math.fabs(tgt_bearing_diff) < math.pi / 2:
# I am in front of target
if dist < 2 * self._max_range and math.fabs(
tgt_bearing_diff) < self._fov_width_radians / 2:
bearing_adj = -math.copysign(ADJ_ANGLE,
tgt_bearing_diff)
print 'ID: {} away from UAV: {} adjusting bearing by {}'.format(
self._id, self._target_id,
math.degrees(bearing_adj))
# ----------------------------------------------------------------------------
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj,
1000)
self._wp = np.array([lat, lon, own_alt]) # keep own alt
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
print "RT=======================>ID: %d shot at Target %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Execute this portion of the code on the loop only
if self._first_tick == True:
raddir = self.radeyeDir + 'radeye.py'
subprocess.Popen(["python",
raddir]) # Runs Radeye in the background
self._first_tick = False
finishedset = set([])
#create a TCP/IP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
serverflag = 1
if (SERVER_FLAG == 1):
server_address = ('192.168.11.202', 10000)
else:
server_address = ('127.0.0.1', 10000)
#server_address = ('192.168.11.202',10000)
#connect the socket to the port where the server is listening
#server_address = ('127.0.0.1',10000)
print >> sys.stderr, '---connecting to %s port %s---' % server_address
self._parent.log_info("stepautonomy0++++=++++++++++++++++++++++")
self._parent.log_info("stepautonomy1++++=++++++++++++++++++++++")
try:
sock.connect(server_address)
messageArray = [
self._id, 100000, 100000, 99999, 100000, self._desired_lat,
self._desired_lon, 0, self._desired_alt, "PRDER"
]
message = str(messageArray)
sock.sendall(message)
#look for response
num_symbols = 4096
delta = 4096
while delta == num_symbols:
data = sock.recv(num_symbols)
delta = len(data)
print >> sys.stderr, 'Received: %s' % data
finishedset = eval(data)
except:
self._parent.log_info("socket communication failed")
finally:
print >> sys.stderr, '---closing socket---'
sock.close()
self._parent.log_info("stepautonomy2++++=++++++++++++++++++++++")
# Set initial altitude settings
self._desired_alt = self._last_ap_wp[2]
self._original_alt = self._last_ap_wp[2]
# Determine your own subswarm ID
#key #value
for blue_id, blue in self._blues.iteritems():
if blue.vehicle_id == self._id:
self._subswarm_id = blue.subswarm_id
break
print "subswarm_id: ", self._subswarm_id
print(finishedset)
# Build a list of all vehicle IDs within your subswarm
blue_in_subswarm = dict()
i = 0
self._parent.log_info("stepautonomy++++=++++++++++++++++++++++")
for blue_id, blue in self._blues.iteritems():
if blue.subswarm_id == self._subswarm_id:
self._id_in_subswarm.append(blue.vehicle_id)
self._subswarm_num_to_assign.append(0)
blue_in_subswarm[i] = blue
i = i + 1
self._parent.log_info("id_in_subswarm: ", self._id_in_subswarm)
print(self._vehicle_type)
for n in finishedset:
self._wp_assigned[n] = True
# Divide # of waypoints by # of vehicles and create empty bundle of wpts for each
num_in_subwarm = len(self._id_in_subswarm)
for i in range(0, num_in_subwarm):
self._subswarm_num_to_assign[i] = (
len(self._enumList) - len(finishedset)) / (num_in_subwarm)
if i < (
(len(self._enumList) - len(finishedset)) % num_in_subwarm):
self._subswarm_num_to_assign[
i] = self._subswarm_num_to_assign[i] + 1
print "num_to_assign: ", self._subswarm_num_to_assign
# Perform sequencial greedy wpt assignment. Loop over each UAS in subswarm.
for i in range(0, num_in_subwarm):
# Set the start location to current UAS position
temp_lat = blue_in_subswarm[i].state.pose.pose.position.lat
temp_lon = blue_in_subswarm[i].state.pose.pose.position.lon
assignment_list = []
# Loop over each element of the waypoint bundle
for j in range(0, self._subswarm_num_to_assign[i]):
min_dist = 99999 #initialize as large number
new_wp_assigned = False
# Loop over each waypoint defined in the mission
for k in range(0, len(self._enumList)):
# Skip to next if that waypoint is already assigned
if self._wp_assigned[k] == False:
# Set the end location to that waypoint
temp2_lat = float(self._enumList[k][0])
temp2_lon = float(self._enumList[k][1])
# Check if start to end location distance is new minimum, if so mark
# if for assignment
temp_dist = gps.gps_distance(
temp_lat, temp_lon, temp2_lat, temp2_lon)
if temp_dist < min_dist:
min_dist = temp_dist
wp_to_assign = k
new_wp_assigned = True
# Add the next closest waypoint to the bundle
if new_wp_assigned == True:
assignment_list.append(wp_to_assign)
self._subswarm_wp_assignment[i] = assignment_list
# Mark that waypoint as "assigned" so unavailable to others
self._wp_assigned[wp_to_assign] = True
# Update the start location to that waypoint
temp_lat = float(self._enumList[wp_to_assign][0])
temp_lon = float(self._enumList[wp_to_assign][1])
# Assign yourself your own bundle of waypoints
if blue_in_subswarm[i].vehicle_id == self._id:
self._wp_id_list = self._subswarm_wp_assignment[i]
print "subswarm_wp_assignment: ", self._subswarm_wp_assignment
# Proceed to the first Waypoint in the bundle
self._loc = self._wp_id_list[0]
self._desired_lat = float(self._enumList[self._loc][0])
self._desired_lon = float(self._enumList[self._loc][1])
print "Going to WP: ", self._loc
# Go to desired latitude, longitude, and maintain altitude
# deconfliction:
self._wp = np.array(
[self._desired_lat, self._desired_lon, self._desired_alt])
#self._wp = np.array([self._desired_lat, self._desired_lon,
#self._last_ap_wp[2]])
pos = self._own_pose.pose.pose.position
dist = gps.gps_distance(pos.lat, pos.lon, self._desired_lat,
self._desired_lon)
# Detect whether UAS has arrived at WP (within threshold distance), track time at WP
# Zephyrs (type 2) loitersocket.error: [Errno 104] Connection reset by peer around point, so set threshold distance > loiter radius
# Set threshold distance for Quads (type 1), much smaller
SURVEY_ALT = self._base_alt + (
self._alt_change * self._id_in_subswarm.index(self._id)
) #redefine SURVEY_ALT within the loop to stack behavior
if (self._vehicle_type == 2
and dist < DIST2WP_ZEPH) or (self._vehicle_type == 1
and dist < DIST2WP_QUAD):
if self._at_wp == False:
if (pos.rel_alt > SURVEY_ALT - BUFFER) and (
pos.rel_alt < SURVEY_ALT + BUFFER):
self._time_start = timeit.default_timer()
self._at_wp = True
self._time_at_wp = timeit.default_timer() - self._time_start
else:
self._at_wp = False
self._time_at_wp = 0
# Detect if Quad is close enough to first WP to descend to survey altitude
if self._vehicle_type == 1 and dist < DIST_START_DESCENT:
self._desired_alt = SURVEY_ALT
# After X time has elapsed at WP, move onto next WP in your bundle
if self._time_at_wp > TIME_AT_WP:
############################# HEATMAP VARS ##############################
def getcounts():
with open(self.radeyeDir + 'radfile.csv', 'r') as radfile:
firstline = radfile.readline().split(',')
print(firstline)
return ((firstline[0], firstline[1]))
lat = pos.lat
lon = pos.lon
alt = pos.alt
counts, radtype = getcounts()
print(counts)
print(radtype)
#########################################################################
#create a TCP/IP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#192.168.11.202
#connect the socket to the port where the server is listening
#server_address = ('192.168.11.202',10000)
#server_address = ('127.0.0.1', 10000)
serverflag = 1
if (SERVER_FLAG == 1):
server_address = ('192.168.11.202', 10000)
else:
server_address = ('127.0.0.1', 10000)
print >> sys.stderr, '---connecting to %s port %s---' % server_address
try:
sock.connect(server_address)
#send data
#str(self._subswarm_wp_assignment)
#messageArray = [self._id, [0,0,0,0,0], self._loc, self._wp_id_list.index(self._loc), [0,0,0,0], lat, lon, counts, alt]
messageArray = [
self._id,
len(self._wp_id_list), self._loc,
self._wp_id_list.index(self._loc),
len(self._enumList), lat, lon, counts, alt, radtype
]
message = str(messageArray)
print("UAV ID: " + str(self._id))
print("Sending Message to Base Station...")
sock.sendall(message)
#look for response 4194304
amount_received = 0
num_symbols = 4096
delta = 4096
while delta == num_symbols:
data = sock.recv(num_symbols)
delta = len(data)
#print >>sys.stderr,'Received: %s' % data
except:
#self._parent.log_info("socket communication failed")
pass
finally:
print >> sys.stderr, '---closing socket---'
sock.close()
self._wp_id_list_id = self._wp_id_list_id + 1
# If you get to the end of your bundle, repeat from its beginning
# and reset to original altitude
if self._wp_id_list_id > (len(self._wp_id_list) - 1):
self._wp_id_list_id = 0
self._desired_alt = self._original_alt
self._loc = self._wp_id_list[self._wp_id_list_id]
self._desired_lat = float(self._enumList[self._loc][0])
self._desired_lon = float(self._enumList[self._loc][1])
# Reset these so that UAV knows it's no longer at its goal WP
self._at_wp = False
self._time_at_wp = 0
print "Going to WP: ", self._loc
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):
# Execute this portion of the code on the loop only
if self._first_tick == True:
self._first_tick = False
# Set initial altitude settings
self._desired_alt = self._last_ap_wp[2]
self._original_alt = self._last_ap_wp[2]
# Determine your own subswarm ID
#key #value
for blue_id, blue in self._blues.iteritems():
if blue.vehicle_id == self._id:
self._subswarm_id = blue.subswarm_id
break
print "subswarm_id: ", self._subswarm_id
# Build a list of all vehicle IDs within your subswarm
blue_in_subswarm = dict()
i = 0
for blue_id, blue in self._blues.iteritems():
if blue.subswarm_id == self._subswarm_id:
self._id_in_subswarm.append(blue.vehicle_id)
self._subswarm_num_to_assign.append(0)
blue_in_subswarm[i] = blue
i = i+1
print "id_in_subswarm: ", self._id_in_subswarm
# Divide # of waypoints by # of vehicles and create empty bundle of wpts for each
num_in_subwarm = len(self._id_in_subswarm)
for i in range(0, num_in_subwarm):
self._subswarm_num_to_assign[i] = int(math.floor(len(usma_enums.WP_LOC)/num_in_subwarm))
if i < (len(usma_enums.WP_LOC) % num_in_subwarm):
self._subswarm_num_to_assign[i] = self._subswarm_num_to_assign[i] + 1
print "num_to_assign: ", self._subswarm_num_to_assign
# Perform sequencial greedy wpt assignment. Loop over each UAS in subswarm.
for i in range(0, num_in_subwarm):
# Set the start location to current UAS position
temp_lat = blue_in_subswarm[i].state.pose.pose.position.lat
temp_lon = blue_in_subswarm[i].state.pose.pose.position.lon
assignment_list = []
# Loop over each element of the waypoint bundle
for j in range(0, self._subswarm_num_to_assign[i]):
min_dist = 99999 #initialize as large number
new_wp_assigned = False
# Loop over each waypoint defined in the mission
for k in range(0, len(usma_enums.WP_LOC)):
# Skip to next if that waypoint is already assigned
if self._wp_assigned[k] == False:
# Set the end location to that waypoint
temp2_lat = float(usma_enums.WP_LOC[k][0])
temp2_lon = float(usma_enums.WP_LOC[k][1])
# Check if start to end location distance is new minimum, if so mark
# if for assignment
temp_dist = gps.gps_distance(temp_lat, temp_lon, temp2_lat, temp2_lon)
if temp_dist < min_dist:
min_dist = temp_dist
wp_to_assign = k
new_wp_assigned = True
# Add the next closest waypoint to the bundle
if new_wp_assigned == True:
assignment_list.append(wp_to_assign)
self._subswarm_wp_assignment[i] = assignment_list
# Mark that waypoint as "assigned" so unavailable to others
self._wp_assigned[wp_to_assign] = True
# Update the start location to that waypoint
temp_lat = float(usma_enums.WP_LOC[wp_to_assign][0])
temp_lon = float(usma_enums.WP_LOC[wp_to_assign][1])
# Assign yourself your own bundle of waypoints
if blue_in_subswarm[i].vehicle_id == self._id:
self._wp_id_list = self._subswarm_wp_assignment[i]
print "subswarm_wp_assignment: ", self._subswarm_wp_assignment
# Proceed to the first Waypoint in the bundle
self._loc = self._wp_id_list[0]
self._desired_lat = float(usma_enums.WP_LOC[self._loc][0])
self._desired_lon = float(usma_enums.WP_LOC[self._loc][1])
print "Going to wp: ", self._loc
# Go to desired latitude, longitude, and maintain altitude
# deconfliction:
self._wp = np.array([self._desired_lat, self._desired_lon,
self._desired_alt])
#self._wp = np.array([self._desired_lat, self._desired_lon,
#self._last_ap_wp[2]])
pos = self._own_pose.pose.pose.position
dist = gps.gps_distance(pos.lat, pos.lon, self._desired_lat, self._desired_lon)
# Detect whether UAS has arrived at WP (within threshold distance), track time at WP
# Zephyrs (type 2) loiter around point, so set threshold distance > loiter radius
# Set threshold distance for Quads (type 1), much smaller
if (self._vehicle_type == 2 and dist < DIST2WP_ZEPH) or (self._vehicle_type == 1 and dist < DIST2WP_QUAD):
if self._at_wp == False:
self._time_start = timeit.default_timer()
self._at_wp = True
self._time_at_wp = timeit.default_timer() - self._time_start
else:
self._at_wp = False
self._time_at_wp = 0
# Detect if Quad is close enough to first WP to descend to survey altitude
if self._vehicle_type == 1 and dist < DIST_START_DESCENT:
self._desired_alt = SURVEY_ALT
# After X time has elapsed at WP, move onto next WP in your bundle
if self._time_at_wp > TIME_AT_WP:
self._wp_id_list_id = self._wp_id_list_id + 1
# If you get to the end of your bundle, repeat from its beginning
# and reset to original altitude
if self._wp_id_list_id > (len(self._wp_id_list)-1):
self._wp_id_list_id = 0
self._desired_alt = self._original_alt
self._loc = self._wp_id_list[self._wp_id_list_id]
self._desired_lat = float(usma_enums.WP_LOC[self._loc][0])
self._desired_lon = float(usma_enums.WP_LOC[self._loc][1])
# Reset these so that UAV knows it's no longer at its goal WP
self._at_wp = False
self._time_at_wp = 0
print "Going to wp: ", self._loc
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):
#swarmCount = 0
# Reset the action and target ID every loop
self._action = ["None", 0]
#self._target_id = self._last_target
if self._team is None:
if self._id in self._blues:
self._team = 'blue'
else:
self._team = 'red'
(self._goal_lat, self._goal_lon, _) = enums.GOAL_POSITS[self._team]
print "lat %f lon %f" % (self._goal_lat, self._goal_lon)
#go to waypoints for storm formation
if self._got_target is False and self._goal_lat is not None:
#self._in_swarm = True
if self._id%5 == 0:
storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + math.pi/2.0, 0)
self._wp = np.array([storm_lat, storm_lon, 500])
#self._safe_waypoint_reserve 35.720785, -120.765860
print "ID: %d sent RESERVE %f, %f\n" % (self._id, storm_lat, storm_lon)
if self._id%5 == 1:
storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT), 100)
self._wp = np.array([storm_lat, storm_lon, 500])
#self._wp = np.array([125, 375, 250])#self._safe_waypoint_nw 35.720785, -120.765860
print "ID: %d sent NW %f, %f\n" % (self._id, storm_lat, storm_lon)
if self._id%5 == 2:
storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + math.pi, 100)
self._wp = np.array([storm_lat, storm_lon, 500])
#self._wp = np.array([125, 125, 300])#self._safe_waypoint_sw 35.719981, -120.769101
print "ID: %d sent SW %f, %f\n" % (self._id, storm_lat, storm_lon)
if self._id%5 == 3:
storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + math.pi/4.0, 100)
self._wp = np.array([storm_lat, storm_lon, 500])
#self._wp = np.array([375, 375, 200])#self._safe_waypoint_ne 35.720785, -120.765860
print "ID: %d sent NE %f, %f\n" % (self._id, storm_lat, storm_lon)
if self._id%5 == 4:
storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + 3.0*math.pi/4.0, 100)
self._wp = np.array([storm_lat, storm_lon, 500])
#self._wp = np.array([375, 125, 350])#self._safe_waypoint_se 35.719981, -120.769101
print "ID: %d sent SE %f, %f\n" % (self._id, storm_lat, storm_lon)
# Search for the closest target every frame if you don't already have one
min_dist = self._target_dist # self._min_dist
if self._got_target is False:
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
self._last_target = self._target_id
if d < self._max_range:
self._got_target = True
self._in_swarm = False
print "UAV %d within range of Target %d\n" % (self._id, self._target_id)
break
else:
self._got_target = False
self._target_id = None
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
if self._last_target == uav.vehicle_id:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < self._max_range:
self._target_id = self._last_target
if self._got_target is False: # only print once
print "UAV %d within range of Target %d\n" % (self._id, self._target_id)
self._got_target = True
else: # was a match, but d > MIN_DIST, reset all
print "UAV %d out of range of Target %d\n" % (self._id, self._last_target)
self._got_target = False
self._target_id = None
self._last_target = None
# If a target has been identified, move towards it
if self._target_id != None:
target_pos = self._reds[self._target_id].state.pose.pose.position
#print "*************************************"
#print "ID: %d Target ID: %d\n" % (self._id, self._target_id)
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
# Set the waypoint past the current target, so we don't go into a
# loiter mode
if self._got_target is True:
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000)
self._wp = np.array([lat, lon, tgt_alt])
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
#self._action = ["None", 0]
print "Storm Shooter==========================>UAV %d shot at Target %d" % (self._id, self._target_id)
#self._last_target = None
#self._got_target = False
#else:
# Start at the safe waypoint
#self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
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._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._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):
# 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 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 run_behavior(self):
''' Runs one iteration of the behavior
'''
self._sorter.send_requested()
with self._swarm_lock:
# Make sure the aircraft this one is following is still valid
# If it is not, need to figure out a new aircraft to follow
if (((self._leader_id not in self._subswarm_keys) or \
(self._leader_id in self._crashed_keys)) and \
(self._behavior_state != NEGOTIATE_ORDER) and \
(self._behavior_state != ON_FINAL) and \
(self._swarm[self._leader_id].swarm_state != enums.LANDING)):
self._negotiate_landing_order()
# Newly activated behavior--determine landing sequence first
if self._behavior_state == NEGOTIATE_ORDER:
if self._negotiate_landing_order():
self._behavior_state = START_TRANSIT
# Sequence computed, but need to send the transit waypoint
elif self._behavior_state == START_TRANSIT:
self.publishWaypoint(self.wp_msg)
self._behavior_state = IN_TRANSIT
self.log_dbug("transit to landing stack location initiated")
return
# Transiting to the landing stack location
elif self._behavior_state == IN_TRANSIT:
state = self._swarm[self._ownID]
d = gps.gps_distance(state.state.pose.pose.position.lat, \
state.state.pose.pose.position.lon, \
self.wp_msg.lat, self.wp_msg.lon)
if gps.gps_distance(state.state.pose.pose.position.lat, \
state.state.pose.pose.position.lon, \
self.wp_msg.lat, self.wp_msg.lon) < LAND_WP_DIST:
self.log_dbug("transit to landing stack location complete")
self._behavior_state = STACK_ARRIVAL
self._stack_time = rospy.Time.now()
else:
return # Not there yet
elif self._behavior_state == STACK_ARRIVAL:
if (rospy.Time.now() - self._stack_time) > STACK_LOITER_TIME:
self._behavior_state = IN_STACK
# In the landing stack--descend after the leader to block 0, then land
elif self._behavior_state == IN_STACK:
ldrState = self._swarm[self._leader_id]
if ldrState.swarm_state == enums.LANDING:
self._leader_id = self._ownID
# It's this aircraft's turn to land
if self._leader_id == self._ownID:
ldg_wp_cmd = stdmsg.UInt16()
ldg_wp_cmd.data = self._ldg_wpt_index
self._behavior_state = ON_FINAL
self._ldg_wp_publisher.publish(ldg_wp_cmd)
self.log_info("landing waypoint ordered")
return
# Determine what altitude block we need to be in
block = int((ldrState.state.pose.pose.position.rel_alt - enums.BASE_REL_ALT) + \
(enums.ALT_BLOCK_SIZE - ALT_BLOCK_CAPTURE)) / int(enums.ALT_BLOCK_SIZE)
block = max(0, block + 1) # Our block is leaderBlock+1
if block != self._alt_block: # New order only rqd on change
self.wp_msg.alt = enums.BASE_REL_ALT + enums.ALT_BLOCK_SIZE * block
self.log_dbug("ordered to altitude block %d (%f meters)"\
%(self._alt_block, self.wp_msg.alt))
self._alt_block = block
self.publishWaypoint(self.wp_msg)
# If landing has already been ordered, we're done
elif self._behavior_state == ON_FINAL:
return
# Invalid state--should never get here
else:
self._log_warn("invalid behavior state: %d" %self._behavior_state)
self._behavior_state = None
self.set_ready_state(False)
