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 _send_ctrl_wp(self):
''' Uses the computed gain, bias, and theta_dot to compute a waypoint
'''
v_p = math.hypot(self._vp[0], self._vp[1])
a_p = (self._gain * self._theta_dot + self._bias) * v_p
if math.fabs(a_p) > PNInterceptor.A_P_MAX:
a_p = gps.signum(a_p) * PNInterceptor.A_P_MAX
L1 = 150.0 #min((v_p * 5.0 * self._dt), self._range)
nu = math.asin((a_p * L1) / (2.0 * math.pow(v_p, 2.0)))
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xp[0], self._xp[1], nu + self._xp[2], L1)
self.publishWaypoint(self.wp_msg)
def _send_ctrl_wp(self):
''' Uses the computed gain, bias, and theta_dot to compute a waypoint
'''
v_p = math.hypot(self._vp[0], self._vp[1])
a_p = (self._gain * self._theta_dot + self._bias) * v_p
if math.fabs(a_p) > PNInterceptor.A_P_MAX:
a_p = gps.signum(a_p) * PNInterceptor.A_P_MAX
L1 = 150.0 #min((v_p * 5.0 * self._dt), self._range)
nu = math.asin((a_p * L1) / (2.0 * math.pow(v_p, 2.0)))
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xp[0], self._xp[1], nu + self._xp[2], L1)
self.publishWaypoint(self.wp_msg)
def _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 _send_ctrl_wp(self):
''' Uses the computed gain, bias, and theta_dot to compute a waypoint
'''
v_p = math.hypot(self._vp[0], self._vp[1])
a_p = (self._gain * self._theta_dot + self._bias) * v_p
if math.fabs(a_p) > USMAInterceptor.A_P_MAX:
a_p = gps.signum(a_p) * USMAInterceptor.A_P_MAX
L1 = 150.0 #min((v_p * 5.0 * self._dt), self._range)
nu = 0
if v_p != 0:
nu = math.asin((a_p * L1) / (2.0 * math.pow(v_p, 2.0)))
self.wp_msg = self._own_pose.pose.pose.position
self.wp_msg.lat, self.wp_msg.lon = \
gps.gps_newpos(self._xp[0], self._xp[1], nu + self._xp[2], L1)
self._wp = numpy.array(
[self.wp_msg.lat, self.wp_msg.lon,
self.wp_msg.rel_alt]) # publishWaypoint(self.wp_msg)
def _runin_state(self):
''' Performs calculations and operations required for the "runin" state
'''
if not self._state_initialized:
self.log_info("Commence run-in phase")
self._gain = 2.5
self._state_initialized = True
# Compute the control parameters & set a bias value if rqd
# to force the pursuer towards the target
self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt)
theta_diff = gps.normalize_pi(self._theta - self._xp[2])
self._bias = min(gps.signum(theta_diff) * 2.5, theta_diff / 10.0)
# Keep going until the offset range is reached
if self._range <= PNInterceptor.R_TH1:
self._state_initialized = False
self._state = PNInterceptor.OFFSET
self.log_info("Transition to offset phase")
def _runin_state(self):
''' Performs calculations and operations required for the "runin" state
'''
if not self._state_initialized:
self.log_info("Commence run-in phase")
self._gain = 2.5
self._state_initialized = True
# Compute the control parameters & set a bias value if rqd
# to force the pursuer towards the target
self._set_ctrl_params(self._xp, self._vp, self._xt, self._vt)
theta_diff = gps.normalize_pi(self._theta - self._xp[2])
self._bias = min(gps.signum(theta_diff) * 2.5, theta_diff / 10.0)
# Keep going until the offset range is reached
if self._range <= PNInterceptor.R_TH1:
self._state_initialized = False
self._state = PNInterceptor.OFFSET
self.log_info("Transition to offset phase")
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 _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 _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(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)