def compute_intercept_waypoint(self, wpt=None):
''' Computes an intercept waypoint for the UAV to steer to
@param wpt: waypoint object to use for computed values
@return waypoint object (LLA) with the intercept waypoint (or None)
'''
lead_uav = self._pre_check()
if not lead_uav:
return None
# Convenience variables for state info
own_lat = self.own_pose.pose.pose.position.lat
own_lon = self.own_pose.pose.pose.position.lon
own_spd = math.hypot(self.own_pose.twist.twist.linear.x, \
self.own_pose.twist.twist.linear.y)
lead_lat = lead_uav.state.pose.pose.position.lat
lead_lon = lead_uav.state.pose.pose.position.lon
lead_alt = lead_uav.state.pose.pose.position.rel_alt
lead_x_dot = lead_uav.state.twist.twist.linear.x
lead_y_dot = lead_uav.state.twist.twist.linear.y
lead_crs = math.atan2(lead_y_dot, lead_x_dot)
lead_spd = math.hypot(lead_y_dot, lead_x_dot)
# Compute tgt positions and distances for potential calculation
tgt_lat, tgt_lon = gps.gps_newpos(lead_lat, lead_lon, \
(lead_crs + self._follow_angle), \
self._follow_distance)
dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon)
angle = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)
x_dist = dist * math.cos(angle)
y_dist = dist * math.sin(angle)
time_to_intercept = 0.0
if own_spd > 0.1:
time_to_intercept = \
gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) / own_spd
time_to_intercept = min(time_to_intercept, self.lookahead)
lead_travel = lead_spd * time_to_intercept
x_travel = lead_travel * lead_x_dot
y_travel = lead_travel * lead_y_dot
# Compute potential values and target waypoint
x_potential = self._dist_coefficient * x_dist + \
self._align_coefficient * lead_x_dot/lead_spd + \
self._intercept_coefficient * x_travel
y_potential = self._dist_coefficient * y_dist + \
self._align_coefficient * lead_y_dot/lead_spd + \
self._intercept_coefficient * y_travel
if not wpt: wpt = brdgmsg.LLA()
wpt.lat, wpt.lon = \
gps.gps_newpos(own_lat, own_lon, \
math.atan2(y_potential, x_potential), \
InterceptCalculator.OVERSHOOT)
if self._alt_mode == InterceptCalculator.BASE_ALT_MODE:
wpt.alt = self._alt
else:
wpt.alt = lead_uav.state.pose.pose.rel_alt + self._alt
self._owner.log_dbug(
"intercept waypoint (lat, lon, alt) = (%f, %f, %f)" %
(wpt.lat, wpt.lon, wpt.alt))
return wpt
def compute_intercept_waypoint(self, wpt=None):
''' Computes an intercept waypoint for the UAV to steer to
@param wpt: waypoint object to use for computed values
@return waypoint object (LLA) with the intercept waypoint (or None)
'''
lead_uav = self._pre_check()
if not lead_uav:
return None
# Update pursuer and target position and velocity info
lead_lat, lead_lon = \
gps.gps_newpos(lead_uav.state.pose.pose.position.lat, \
lead_uav.state.pose.pose.position.lon, \
self._follow_distance, self._follow_angle)
lead_spd = math.hypot(lead_uav.state.twist.twist.linear.x, \
lead_uav.state.twist.twist.linear.y)
lead_crs = math.atan2(lead_uav.state.twist.twist.linear.y, \
lead_uav.state.twist.twist.linear.x)
own_lat = self.own_pose.pose.pose.position.lat
own_lon = self.own_pose.pose.pose.position.lon
own_spd = math.hypot(self.own_pose.twist.twist.linear.x, \
self.own_pose.twist.twist.linear.y)
if (own_spd - lead_spd) < 1.0: own_spd = lead_spd + 1.0
own_crs = math.atan2(self.own_pose.twist.twist.linear.y, \
self.own_pose.twist.twist.linear.x)
# Compute the control parameters
self._theta = gps.gps_bearing(own_lat, own_lon, lead_lat, lead_lon)
tgt_distance = gps.gps_distance(own_lat, own_lon, lead_lat, lead_lon)
crossing_spd = own_spd * math.sin(self._theta - own_crs) + \
lead_spd * math.sin(self._theta + math.pi - lead_crs)
self._theta_dot = crossing_spd / tgt_distance
theta_diff = gps.normalize_pi(self._theta - own_crs)
self._bias = min(gps.signum(theta_diff) * 2.5, theta_diff / 10.0)
# Use the PN calculation to compute the ordered acceleration
ordered_a = (self._gain * self._theta_dot + self._bias) * own_spd
a_max = min(self._a_max, ((2.0 * math.pow(own_spd, 2.0)) / self._L1))
if math.fabs(ordered_a) > a_max:
ordered_a = gps.signum(ordered_a) * a_max
nu = math.asin((ordered_a * self._L1) / (2.0 * math.pow(own_spd, 2.0)))
# Compute the waypoint command
if not wpt: wpt = brdgmsg.LLA()
wpt.lat, wpt.lon = \
gps.gps_newpos(lead_lat, lead_lon, nu + own_crs, self._L1)
if self._alt_mode == InterceptCalculator.BASE_ALT_MODE:
wpt.alt = self._alt
else:
wpt.alt = lead_uav.state.pose.pose.rel_alt + self._alt
self._owner.log_dbug(
"intercept waypoint (lat, lon, alt) = (%f, %f, %f)" %
(wpt.lat, wpt.lon, wpt.alt))
return wpt
def append(self, lat, lon, rel_alt):
''' Appends a new waypoint to the end of the sequence
@param lat: latitude of the new waypoint
@param lon: longitude of the new waypoint
@param rel_alt: rel_alt of the new waypoint
'''
new_wpt = brdgmsg.LLA()
new_wpt.lat = lat
new_wpt.lon = lon
new_wpt.alt = rel_alt
self._sequence.append(new_wpt)
def set_sequence(self, waypoints):
''' Initializes the waypoint sequence
@param waypoints: list of waypoints of the form (lat, lon, rel_alt)
'''
self._sequence = []
for wpt in waypoints:
new_wpt = brdgmsg.LLA()
new_wpt.lat = wpt[0]
new_wpt.lon = wpt[1]
new_wpt.alt = wpt[2]
self._sequence.append(new_wpt)
self._current = 0
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 net_guided_goto(message, bridge):
msg = pilot_msg.LLA()
msg.lat = message.lat
msg.lon = message.lon
msg.alt = message.alt
bridge.publish('recv_guided_goto', msg, latched=True)