def angle_between(angle, min_angle, max_angle):
''' Returns True if min_angle <= angle <= max_angle
on a normalized range of [0, 2pi)
'''
span = gps.normalize(max_angle - min_angle)
to_test_angle = gps.normalize(angle - min_angle)
return to_test_angle < span
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 _s_turn(self):
''' Calculations associated with the S-turn to the virtual target
'''
if self._s_turn_state == self.S1: # Offset turn
self._gain = 2.0
self._bias = 0.5 * gps.signum(self._theta_dot)
if math.fabs(gps.signum(self._theta_dot) - \
gps.signum(self._theta_dot_last)) == 2:
self._s_turn_state = self.S2
self._theta2 = self._theta
self._theta3 = self._theta2 + 0.75 * (self._theta_VT_init -
self._theta2)
self._theta3 = gps.normalize(self._theta3)
self._s2_check = \
gps.signum(gps.normalize_pi(self._theta - self._theta3))
self.log_info("Shift to S-turn phase S2:\tTheta=%f\tTheta3=%f\tTheta_dot=%f"\
%(self._theta, self._theta3, self._theta_dot))
if self._s_turn_state == self.S2: # Middle portion of the S
self._bias = -0.25 * gps.signum(self._theta_dot)
self._gain = -2.0
if self._s2_check != \
gps.signum(gps.normalize_pi(self._theta - self._theta3)):
self._s_turn_state = self.S3
self._theta3 = self._theta
self._psi_p3 = self._xp[2]
self.log_info("Shift to S-turn phase S3")
self._bias_initialized = False # to facilitate a log entry
if self._s_turn_state == self.S3: # Final turn to VT
if (self._theta_dot_VT_init < 0.0 and \
angle_between(self._psi_p_f, self._theta_VT_init, \
2.0 * self._theta3 - self._psi_p3)) or \
(self._theta_dot_VT_init >= 0.0 and \
angle_between(self._psi_p_f, \
2.0 * self._theta3 - self._psi_p3, \
self._theta_VT_init)):
self._gain = math.fabs((gps.normalize_pi(self._psi_p_f - self._psi_p3)) / \
(gps.normalize_pi(self._psi_p_f - self._theta3)))
self._bias = 0.0
if not self._bias_initialized:
self._bias_initialized = True
self.log_info("Phase S3 using 1-bias turn")
else:
if math.fabs(gps.normalize_pi(self._psi_p_f - self._xp[2]) / \
gps.normalize_pi(self._psi_p_f - self._theta)) < 2.0:
self._gain = gps.normalize_pi(self._psi_p3) / \
gps.normalize_pi(math.pi/2.0 + self._theta3)
self._bias = 0.0
else:
self._gain = 2.0
self._bias = 0.0
if not self._bias_initialized:
self._bias_initialized = True
self.log_info("Phase S3 using 2-bias turn")
def _calculate_E(self):
''' Calculates the approximated integral of control energy square (E)
@return calculated value
'''
theta_p = gps.normalize(self._xp[2] - self._theta)
theta_t = gps.normalize(self._xt[2] - self._theta)
theta_tr = self._theta
R = self._range
delta_theta = 0.0025 * gps.signum(self._theta_dot)
v_p = math.hypot(self._vp[0], self._vp[1])
v_t = math.hypot(self._vt[0], self._vt[1])
v_r = v_t * math.cos(theta_t) - v_p * math.cos(theta_p)
v_theta = v_t * math.sin(theta_t) - v_p * math.sin(theta_p)
E = 0.0
while math.cos(delta_theta) > math.cos(self._psi_p_f - theta_tr):
E = E + math.pow((self._gain * v_p), 2) * \
(v_theta / R) * delta_theta
R = R * math.exp(v_r / v_theta * delta_theta)
theta_t = theta_t - delta_theta
theta_p = theta_p + (self._gain - 1) * delta_theta
v_r = v_t * math.cos(theta_t) - v_p * math.cos(theta_p)
v_theta = v_t * math.sin(theta_t) - v_p * math.sin(theta_p)
theta_tr = theta_tr + delta_theta
return E
def _calculate_E(self):
''' Calculates the approximated integral of control energy square (E)
@return calculated value
'''
theta_p = gps.normalize(self._xp[2] - self._theta)
theta_t = gps.normalize(self._xt[2] - self._theta)
theta_tr = self._theta
R = self._range
delta_theta = 0.0025 * gps.signum(self._theta_dot)
v_p = math.hypot(self._vp[0], self._vp[1])
v_t = math.hypot(self._vt[0], self._vt[1])
v_r = v_t * math.cos(theta_t) - v_p * math.cos(theta_p)
v_theta = v_t * math.sin(theta_t) - v_p * math.sin(theta_p)
E = 0.0
while math.cos(delta_theta) > math.cos(self._psi_p_t_f - theta_tr):
E = E + math.pow((self._gain * v_p), 2) * \
(v_theta / R) * delta_theta
R = R * math.exp(v_r / v_theta * delta_theta)
theta_t = theta_t - delta_theta
theta_p = theta_p + (self._gain - 1) * delta_theta
v_r = v_t * math.cos(theta_t) - v_p * math.cos(theta_p)
v_theta = v_t * math.sin(theta_t) - v_p * math.sin(theta_p)
theta_tr = theta_tr + delta_theta
return E
def _parallel_2_state(self):
''' Performs calculations and operations required for the "parallel 2" state
'''
if not self._state_initialized:
self._psi_p_t_f = gps.normalize(self._xt[2] - PNInterceptor.FIRE_ANGLE)
self._bias = 0.0
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt)
self._gain = math.fabs((self._psi_p_t_f - self._xp[2]) / \
(self._psi_p_t_f - self._theta))
self._E = self._calculate_E()
if self._E > min(numpy.mean(self._E_trend), \
numpy.median(self._E_trend)): # reached minimum
self._state_initialized = False
self._state = PNInterceptor.INTERCEPT
self.log_info("Transition to intercept phase")
self._E_trend = self._E_trend[1:] + [ self._E ]
def _parallel_1_state(self):
''' Performs calculations and operations required for the "parallel 1" state
'''
if not self._state_initialized:
bearing = gps.normalize(self._xt[2] - math.pi)
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xp[0], self._xp[1], bearing, 10000.0)
self.publishWaypoint(self.wp_msg)
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt)
if self._range <= PNInterceptor.R_TH2:
self._state = PNInterceptor.PARALLEL_2
self._gain = 0.0
self._bias = 0.0
self._state_initialized = False
self.log_info("Transition to parallel_2 state")
def _parallel_1_state(self):
''' Performs calculations and operations required for the "parallel 1" state
'''
if not self._state_initialized:
bearing = gps.normalize(self._xt[2] - math.pi)
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xp[0], self._xp[1], bearing, 10000.0)
self.publishWaypoint(self.wp_msg)
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt)
if self._range <= USMAInterceptor.R_TH2:
self._state = USMAInterceptor.PARALLEL_2
self._gain = 0.0
self._bias = 0.0
self._state_initialized = False
self.log_info("Transition to parallel_2 state")
def _parallel_2_state(self):
''' Performs calculations and operations required for the "parallel 2" state
'''
if not self._state_initialized:
self._psi_p_f = gps.normalize(self._xt[2] -
USMAInterceptor.FIRE_ANGLE)
self._bias = 0.0
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt)
self._gain = math.fabs(gps.normalize_pi(self._psi_p_f - self._xp[2]) / \
gps.normalize_pi(self._psi_p_f - self._theta))
self._E = self._calculate_E()
if self._E > min(numpy.mean(self._E_trend), \
numpy.median(self._E_trend)): # reached minimum
self._state_initialized = False
self._state = USMAInterceptor.INTERCEPT
self.log_info("Transition to intercept phase")
self._E_trend = self._E_trend[1:] + [self._E]
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target every frame
min_dist = np.inf
for uav in self._reds.values():
if uav.vehicle_id not in self._shot:
d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\
self._own_pose.pose.pose.position.lon,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
uav.state.pose.pose.position.lat,\
uav.state.pose.pose.position.lon, \
uav.state.pose.pose.position.rel_alt):
self._action = ["Fire", uav.vehicle_id]
print "RT=========(loop)========>ID: %d shot at Target %d" % (
self._id, uav.vehicle_id)
'''if self._last_target_id == self._target_id:
if d < 100: # how close do we have to be before I call chasing
self._target_count += 1
if self._target_count > 100:
self._target_count = 300
self._target_id = None
elif self._target_count > 0:
self._target_count -= 1
self._target_id = None
'''
# If a target has been identified, move towards it
if self._target_id != None:
self._last_target_id = self._target_id
# Calculate target and own position and orientation
target_pos = self._reds[self._target_id].state.pose.pose.position
target_orientation = self._reds[
self._target_id].state.pose.pose.orientation
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
tgt_alt = target_pos.rel_alt
own_pos = self._own_pose.pose.pose.position
own_lat = own_pos.lat
own_lon = own_pos.lon
own_alt = own_pos.rel_alt
own_orientation = self._own_pose.pose.pose.orientation
# Calculate bearing to target and from target to self
bearing = gps.normalize(
gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon))
tgt_bearing = gps.normalize(
gps.gps_bearing(tgt_lat, tgt_lon, own_lat, own_lon))
# Calculate heading/yaw of self and target
own_rpy = qmath.quat_to_euler((own_orientation.x,\
own_orientation.y, \
own_orientation.z, \
own_orientation.w))
own_heading = gps.normalize(own_rpy[2])
tgt_rpy = qmath.quat_to_euler((target_orientation.x,\
target_orientation.y, \
target_orientation.z, \
target_orientation.w))
tgt_heading = gps.normalize(tgt_rpy[2])
# Calculate distance to target
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
bearing_adj = 0.0
#print 'ID: {} targeting UAV: {} at bearing {} and distance {}'.format(self._id, self._target_id, math.degrees(bearing), dist)
#print 'UAV: {} own bearing: {}'.format(self._target_id, math.degrees(tgt_heading))
# Calculate offset between bearing and target heading (measures degree of "head on")
# This section needs more work
# ----------------------------------------------------------------------------
heading_diff = own_heading - tgt_heading
tgt_bearing_diff = gps.normalize_pi(tgt_heading - tgt_bearing)
if math.fabs(heading_diff) > math.pi / 2:
# moving towards each other
if tgt_bearing > (
tgt_heading - self._fov_width_radians / 2
) or tgt_bearing < (tgt_heading + self._fov_width_radians / 2):
# we are in the "fan"
if dist > self._max_range and math.fabs(
tgt_bearing_diff) < self._fov_width_radians / 2:
bearing_adj = math.copysign(ADJ_ANGLE,
tgt_bearing_diff)
print 'ID: {} toward UAV: {}: adjusting bearing by {}'.format(
self._id, self._target_id,
math.degrees(bearing_adj))
else:
# heading in same general direction
if math.fabs(tgt_bearing_diff) < math.pi / 2:
# I am in front of target
if dist < 2 * self._max_range and math.fabs(
tgt_bearing_diff) < self._fov_width_radians / 2:
bearing_adj = -math.copysign(ADJ_ANGLE,
tgt_bearing_diff)
print 'ID: {} away from UAV: {} adjusting bearing by {}'.format(
self._id, self._target_id,
math.degrees(bearing_adj))
# ----------------------------------------------------------------------------
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj,
1000)
self._wp = np.array([lat, lon, own_alt]) # keep own alt
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w ), \
self._max_range, self._fov_width, self._fov_height,\
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
print "RT=======================>ID: %d shot at Target %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def step_autonomy(self, t, dt):
# Reset the action 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):
# Get our current position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt # was just alt
# Reset the action every loop
self._action = ["None", 0]
self._last_target_id = self._target_id
self._last_target_bearing = self._target_bearing
self._target_id = None
self._target_bearing = 0.0
bearing_adj = -0.7 # default adjust for CCW circling enemy
# Search for the closest target
min_dist = np.inf
for uav in self._reds.values():
if uav.vehicle_id not in self._shot and (
uav.game_status == enums.GAME_ACTIVE_DEFENSE
or uav.game_status == enums.GAME_ACTIVE_OFFENSE):
target_pos = self._reds[
uav.vehicle_id].state.pose.pose.position
d = gps.gps_distance(own_lat, \
own_lon, \
uav.state.pose.pose.position.lat, \
uav.state.pose.pose.position.lon)
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w), \
self._weapons_range, self._fov_width, self._fov_height, \
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", uav.vehicle_id]
print "Quad=======(loop)=======> ID: %d shot at enemy %d" % (
self._id, uav.vehicle_id)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
# If a target has been identified, move towards it
if self._target_id != None:
target_pos = self._reds[self._target_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
distance_to_target = gps.gps_distance(own_lat, own_lon, tgt_lat,
tgt_lon)
# Set bearing to look at the enemy vehicle
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
self._target_bearing = bearing
if self._target_id == self._last_target_id:
bearing_diff = self._target_bearing - self._last_target_bearing
if bearing_diff < 0.0:
bearing_adj = math.sin(-UAV_SPEED / distance_to_target)
else:
bearing_adj = math.sin(UAV_SPEED / distance_to_target)
# Set waypoint to move forward and strafe sideways once the enemy is within range
self._evasion_range = self._weapons_range * 3
self._engagement_range = self._weapons_range * 2 / 3
if distance_to_target <= self._engagement_range:
lat, lon = gps.gps_newpos(own_lat, own_lon,
gps.normalize(bearing + bearing_adj),
0.1) # was - self._fov_width/2
#print "UAV %d in engagement range with enemy %d bearing %f distance %f using %f bearing adj" % (self._id, self._target_id, math.degrees(bearing), distance_to_target, bearing_adj)
elif distance_to_target <= self._evasion_range:
# Check to see if sidestep right waypoint is outside the battlebox
# (only need to check distance_to_target/2 since enemy is closing)
evade_sign = -2 * (self._id % 2) + 1
#print "UAV %d in evasion range with enemy %d" % (self._id, self._target_id)
lat, lon = gps.gps_newpos(
own_lat, own_lon,
gps.normalize(bearing - evade_sign * (math.pi / 3)),
distance_to_target)
if not self._bboxes[0].contains(lat, lon, own_alt):
lat, lon = gps.gps_newpos(
own_lat, own_lon,
gps.normalize(bearing + evade_sign * (math.pi / 3)),
distance_to_target)
#print "UAV %d sidestepping left" % self._id
else:
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing,
1000) # was 1000
self._wp = np.array([lat, lon, own_alt
]) # don't change alt, was target_pos.rel_alt
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w), \
self._weapons_range, self._fov_width, self._fov_height, \
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
print "Quad====================> ID: %d shot at enemy %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
return True
def _offset_state(self):
''' Performs calculations and operations required for the "offset" state
'''
if not self._state_initialized:
# Set the virtual waypoint D2 meters along the target's path and
# offset by D1 meters to the side that the pursuer is currently on
theta = gps.gps_bearing(self._xp[0], self._xp[1], \
self._xt[0], self._xt[1])
offset_dir = \
gps.signum(gps.normalize_pi(theta + math.pi - self._xt[2]))
tgt_to_VT = self._xt[2] + math.atan2(PNInterceptor.D1 * offset_dir, \
PNInterceptor.D2)
self._VT = \
gps.gps_newpos(self._xt[0], self._xt[1], tgt_to_VT, \
math.hypot(PNInterceptor.D1, PNInterceptor.D2))
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
self._theta_VT_init = self._theta
self._theta_dot_VT_init = self._theta_dot
self._psi_p_VT_init = self._xp[2]
self._psi_p_t_f = gps.normalize(self._xt[2] + math.pi)
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
if self._range < PNInterceptor.R_CAP_PARALLEL:
self._state_initialized = False
self._state = PNInterceptor.PARALLEL_1
self.log_info("Transition to parallel_1 phase")
# Determine how to maneuver to the virtual target point
# Direct turn to the virtual target
if (self._theta_VT_init <= self._psi_p_t_f) and \
(self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - self._psi_p_VT_init)):
self._gain = math.fabs((self._psi_p_t_f - self._psi_p_VT_init) / \
(self._psi_p_t_f - self._theta_VT_init))
self._bias = 0.0
# Single turn to virtual target with 2 gains
elif ((-math.pi - self._psi_p_VT_init + \
(2.0 * self._theta_VT_init)) <= self._psi_p_t_f) and \
(self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - \
self._psi_p_VT_init)):
if (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) < 2.0:
self._gain = (2.0 / math.pi) * self._psi_p_VT_init
self._bias = 0.0
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) == 2.0:
self._gain = 2.0
self._bias = 0.0
else: # should never get here
self.log_warn("Unable to determine phase 2 2-step case")
self.set_ready_state(False)
# S-turn to virtual target
else:
if gps.signum(self._theta_dot) == \
gps.signum(self._theta_dot_VT_init):
self._gain = 2.5
self._bias = 0.1 * gps.signum(self._theta_dot_VT_init)
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) < 2.0:
self._gain = 2.5
self._bias = 0.05 * gps.signum(self._theta_dot_VT_init)
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) >= 2.0:
self._gain = math.fabs((self._psi_p_t_f - self._xp[2]) / \
(self._psi_p_t_f - self._theta))
self._bias = 0.0
else: # should never get here
self.log_warn("Unable to determine phase 2 3-step case")
self.set_ready_state(False)
def _offset_state(self):
''' Performs calculations and operations required for the "offset" state
'''
if not self._state_initialized:
# Set the virtual waypoint D2 meters along the target's path and
# offset by D1 meters to the side that the pursuer is currently on
theta = gps.gps_bearing(self._xp[0], self._xp[1], \
self._xt[0], self._xt[1])
offset_dir = \
gps.signum(gps.normalize_pi(theta + math.pi - self._xt[2]))
tgt_to_VT = self._xt[2] + math.atan2(USMAInterceptor.D1 * offset_dir, \
USMAInterceptor.D2)
self._VT = \
gps.gps_newpos(self._xt[0], self._xt[1], tgt_to_VT, \
math.hypot(USMAInterceptor.D1, USMAInterceptor.D2))
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], self._xt[2]), \
(0.0, 0.0))
self._theta_VT_init = self._theta
self._theta_dot_VT_init = self._theta_dot
self._psi_p_VT_init = self._xp[2]
self._psi_p_f = gps.normalize(self._xt[2] + math.pi)
psi_p_f_rel = gps.normalize(self._psi_p_f - self._theta_VT_init)
if gps.normalize_pi(self._theta_VT_init -
self._psi_p_VT_init) > 0.0:
psi_p_f_rel = gps.normalize(-psi_p_f_rel)
if angle_between(psi_p_f_rel, 0.0, math.pi):
self._VT_turn_type = USMAInterceptor.S_TURN
self._s_turn_state = USMAInterceptor.S1
elif angle_between(psi_p_f_rel, (self._theta_VT_init - \
self._psi_p_VT_init), 0.0):
self._VT_turn_type = USMAInterceptor.DIRECT_TURN
else:
self._VT_turn_type = USMAInterceptor.DIRECT_2BIAS
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
if self._range < USMAInterceptor.R_CAP_PARALLEL:
self._state_initialized = False
self._state = USMAInterceptor.PARALLEL_1
self.log_info("Transition to parallel_1 phase")
# Determine how to maneuver to the virtual target point
# Direct turn to the virtual target
if self._VT_turn_type == USMAInterceptor.DIRECT_TURN:
self._gain = math.fabs((gps.normalize_pi(self._psi_p_f - \
self._psi_p_VT_init)) / \
(gps.normalize_pi(self._psi_p_f - \
self._theta_VT_init)))
self._bias = 0.0
# Single turn to virtual target with 2 gains
elif self._VT_turn_type == USMAInterceptor.DIRECT_2BIAS:
if math.fabs(gps.normalize_pi(self._psi_p_f - self._xp[2]) / \
gps.normalize_pi(self._psi_p_f - self._theta)) < 2.0:
self._gain = gps.normalize_pi(self._psi_p_VT_init) / \
gps.normalize_pi(math.pi/2 + self._theta_VT_init)
self._bias = 0.0
else:
self._gain = 2.0
self._bias = 0.0
# S-turn to virtual target
else:
self._s_turn()
def step_autonomy(self, t, dt):
# Get our current position
own_lat = self._own_pose.pose.pose.position.lat
own_lon = self._own_pose.pose.pose.position.lon
own_alt = self._own_pose.pose.pose.position.rel_alt # was just alt
# Reset the action every loop
self._action = ["None", 0]
self._target_id = None
# Search for the closest target
min_dist = MIN_DIST
del t_id[:]
del t_dist[:]
for uav in self._reds.values():
if (uav.vehicle_id not in self._shot) and (uav.vehicle_id
not in self._taken):
d = gps.gps_distance(own_lat, \
own_lon, \
uav.state.pose.pose.position.lat, \
uav.state.pose.pose.position.lon)
if d < min_dist:
min_dist = d
self._target_id = uav.vehicle_id
self._bearing_adj = -(self._fov_width / 2)
self._target_range = d
# If a target has been identified, move towards it
if self._target_id != None:
msg = apmsg.MsgStat()
msg.id = self._id
msg.count = self._target_id
msg.latency = self._target_range
self.pubs['taken'].publish(msg)
print '{} targeting UAV {} at a distance of {}'.format(
self._id, self._target_id, self._target_range)
print '{} has taken list of {}'.format(self._id, self._taken)
target_pos = self._reds[self._target_id].state.pose.pose.position
tgt_lat = target_pos.lat
tgt_lon = target_pos.lon
distance_to_target = gps.gps_distance(own_lat, own_lon, tgt_lat,
tgt_lon)
# Set bearing to look at the enemy vehicle
bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
# Set waypoint to move forward and strafe sideways once the enemy is within range
self._evasion_range = self._weapons_range * 2
self._engagement_range = self._weapons_range
if distance_to_target <= self._engagement_range:
self._bearing_adj += 2.5
if self._bearing_adj > self._fov_width:
self._bearing_adj = -self._fov_width
bearing += math.radians(
self._bearing_adj
) # bearing_adj in degrees; convert to radians
lat, lon = gps.gps_newpos(own_lat, own_lon,
gps.normalize(bearing),
0.1) # was - self._fov_width/2
#print "UAV %d in engagement range with enemy %d bearing %f" % (self._id, self._target_id, math.degrees(bearing))
elif distance_to_target <= self._evasion_range:
# Check to see if sidestep right waypoint is outside the battlebox
# (only need to check distance_to_target/2 since enemy is closing)
#print "UAV %d in evasion range with enemy %d" % (self._id, self._target_id)
lat_right, lon_right = gps.gps_newpos(own_lat, own_lon, \
gps.normalize(bearing + (math.pi / 3)), distance_to_target/2)
if self._bboxes[0].contains(lat_right, lon_right, own_alt):
lat, lon = gps.gps_newpos(own_lat, own_lon, \
gps.normalize(bearing + (math.pi / 3)), distance_to_target)
#print "UAV %d sidestepping right" % self._id
else:
# If right sidestep is outside the battlebox, sidestep to the left
lat, lon = gps.gps_newpos(
own_lat, own_lon,
gps.normalize(bearing - (math.pi / 3)),
distance_to_target)
#print "UAV %d sidestepping left" % self._id
else:
lat, lon = gps.gps_newpos(own_lat, own_lon, bearing,
100) # was 1000
self._wp = np.array([lat, lon, target_pos.rel_alt
]) # +random.randint(-10,10)]
# Determine if the target is within firing parameters
if gps.hitable(self._own_pose.pose.pose.position.lat, \
self._own_pose.pose.pose.position.lon, \
self._own_pose.pose.pose.position.rel_alt, \
(self._own_pose.pose.pose.orientation.x, \
self._own_pose.pose.pose.orientation.y, \
self._own_pose.pose.pose.orientation.z, \
self._own_pose.pose.pose.orientation.w), \
self._weapons_range, self._fov_width, self._fov_height, \
target_pos.lat, target_pos.lon, target_pos.rel_alt):
self._action = ["Fire", self._target_id]
print "=================>ID: %d shot at Target %d" % (
self._id, self._target_id)
else:
# If a target hasn't been identified, return to the safe waypoint
self._wp = self._safe_waypoint
self._taken.clear(
) # clear the taken list in case it's full of old data
return True
def step_autonomy(self, t, dt):
# Reset the action and target ID every loop
self._action = ["None", 0]
# 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 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 _offset_state(self):
''' Performs calculations and operations required for the "offset" state
'''
if not self._state_initialized:
# Set the virtual waypoint D2 meters along the target's path and
# offset by D1 meters to the side that the pursuer is currently on
theta = gps.gps_bearing(self._xp[0], self._xp[1], \
self._xt[0], self._xt[1])
offset_dir = \
gps.signum(gps.normalize_pi(theta + math.pi - self._xt[2]))
tgt_to_VT = self._xt[2] + math.atan2(PNInterceptor.D1 * offset_dir, \
PNInterceptor.D2)
self._VT = \
gps.gps_newpos(self._xt[0], self._xt[1], tgt_to_VT, \
math.hypot(PNInterceptor.D1, PNInterceptor.D2))
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
self._theta_VT_init = self._theta
self._theta_dot_VT_init = self._theta_dot
self._psi_p_VT_init = self._xp[2]
self._psi_p_t_f = gps.normalize(self._xt[2] + math.pi)
self._state_initialized = True
self._set_ctrl_params(self._xp, self._vp, \
(self._VT[0], self._VT[1], 0.0), (0.0, 0.0))
if self._range < PNInterceptor.R_CAP_PARALLEL:
self._state_initialized = False
self._state = PNInterceptor.PARALLEL_1
self.log_info("Transition to parallel_1 phase")
# Determine how to maneuver to the virtual target point
# Direct turn to the virtual target
if (self._theta_VT_init <= self._psi_p_t_f) and \
(self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - self._psi_p_VT_init)):
self._gain = math.fabs((self._psi_p_t_f - self._psi_p_VT_init) / \
(self._psi_p_t_f - self._theta_VT_init))
self._bias = 0.0
# Single turn to virtual target with 2 gains
elif ((-math.pi - self._psi_p_VT_init + \
(2.0 * self._theta_VT_init)) <= self._psi_p_t_f) and \
(self._psi_p_t_f <= ((2.0 * self._theta_VT_init) - \
self._psi_p_VT_init)):
if (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) < 2.0:
self._gain = (2.0 / math.pi) * self._psi_p_VT_init
self._bias = 0.0
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) == 2.0:
self._gain = 2.0
self._bias = 0.0
else: # should never get here
self.log_warn("Unable to determine phase 2 2-step case")
self.set_ready_state(False)
# S-turn to virtual target
else:
if gps.signum(self._theta_dot) == \
gps.signum(self._theta_dot_VT_init):
self._gain = 2.5
self._bias = 0.1 * gps.signum(self._theta_dot_VT_init)
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) < 2.0:
self._gain = 2.5
self._bias = 0.05 * gps.signum(self._theta_dot_VT_init)
elif (gps.normalize(self._psi_p_t_f - self._xp[2]) / \
gps.normalize(self._psi_p_t_f - self._theta)) >= 2.0:
self._gain = math.fabs((self._psi_p_t_f - self._xp[2]) / \
(self._psi_p_t_f - self._theta))
self._bias = 0.0
else: # should never get here
self.log_warn("Unable to determine phase 2 3-step case")
self.set_ready_state(False)