Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mshopcha/autonomy-payload
  • dtdavi1/autonomy-payload
  • eric.squires_gtri.gatech.edu/autonomy-payload
  • tfoote_osrfoundation.org/autonomy-payload
  • sasc/autonomy-payload
5 results
Show changes
Commits on Source (223)
Showing
with 2559 additions and 279 deletions
......@@ -3,3 +3,4 @@
lib/acs.egg-info/
lib/build/
lib/dist/
repos
\ No newline at end of file
# autonomy-payload : a ROS-based stack for cooperative aerial robot autonomy
# swarm-autonomy : a ROS-based stack for cooperative aerial robot autonomy
## License
......@@ -16,19 +16,19 @@ A collection of ROS nodes for perception, path planning, safety monitoring, etc,
## Requirements
_Note: Typically this software is installed using a script (see deploy/odroid-installer.sh or the acs-installer repository). The following instructions won't generally be used and are only provided for reference._
This is currently being developed against ROS Indigo (Ubuntu 14.04) and Kinetic (Ubuntu 16.06) on an Ubuntu-esque system. For best compatibility, use Ubuntu 14.04 LTS or newer (Xubuntu, etc should be fine as well).
This is currently being developed against ROS Hydro on an Ubuntu-esque system. For best compatibility, use Ubuntu 12.04 LTS or newer (Xubuntu, etc should be fine as well).
_Note: Typically this software is installed using a script (see deploy/odroid-installer-14.04.sh, deploy/odroid-installer-16.04 or the acs-installer repository). The following instructions won't generally be used and are only provided for reference._
You will need the following ROS packages for your distribution:
sudo apt-get install ros-hydro-ros-base ros-hydro-sensor-msgs ros-hydro-robot-pose-ekf
sudo apt-get install ros-[distro]-ros-base ros-[distro]-sensor-msgs ros-[distro]-robot-pose-ekf
See your distribution's ROS documentation for install instructions. The short version, which generally works:
sudo rosdep init
rosdep update
echo "source /opt/ros/hydro/setup.bash" >> ~/.bashrc
echo "source /opt/ros/[distro]/setup.bash" >> ~/.bashrc
source /opt/ros/hydro/setup.bash
In addition, to interface with MAVLink-based autopilots, you will need:
......
......@@ -580,28 +580,46 @@ class PrevMsgAP(Message):
self.msg = _dec_str(fields[2])
"""
Takeoff Messsage class.
Camera Control Message class.
ArduCopter requires a "Takeoff" command to be sent.
Command utilized to control an onboard camera if equipped Also can be used to activate a servo if the servo
is installed where the camera control normally is installed.
"""
class CameraControl(Message):
msg_type = 0x10
msg_fmt = ''
def __init__(self):
Message.__init__(self)
pass
def _pack(self):
return ''
def _unpack(self, data):
pass
Single field is commanded takeoff altitude
"""
class Takeoff(Message):
msg_type = 0x0d
msg_fmt = '>h'
Tactic Camera Control Message class.
Same as the Camera Control Message class with the exception that this message is intended to be sent to tactics
instead of directly to the autopilot. This allows tactics to 'catch' the payload message before sending it to autopilot.
"""
class TacticCameraControl(Message):
msg_type = 0x11
msg_fmt = ''
def __init__(self):
Message.__init__(self)
self.takeoff_alt = 0
pass
def _pack(self):
tupl = (int(self.takeoff_alt),)
return struct.pack(type(self).msg_fmt, *tupl)
return ''
def _unpack(self, data):
fields = list(struct.unpack_from(type(self).msg_fmt, data, 0))
self.takeoff_alt = fields.pop(0)
pass
#---------------------------------------------------
# Ground/Air communication and connectivity messages
......@@ -740,6 +758,32 @@ class Demo(Message):
fields = struct.unpack_from(type(self).msg_fmt, data, 0)
self.demo = int(fields[0])
class GenericWPConfig(Message):
msg_type = 0x44
msg_fmt = '>llI'
def __init__(self):
Message.__init__(self)
self.lat = None # Decimal degrees (e.g. 35.123456)
self.lon = None # Decimal degrees (e.g. -120.123456)
self.alt = None # (RELATIVE) altitude (m)
# 1 padding byte
def _pack(self):
tupl = (int(self.lat * 1e07),
int(self.lon * 1e07),
int(self.alt))
return struct.pack(type(self).msg_fmt, *tupl)
def _unpack(self, data):
fields = struct.unpack_from(type(self).msg_fmt, data, 0)
self.lat = fields[0] / 1e07
self.lon = fields[1] / 1e07
self.alt = int(fields[2])
#-------------------------------
# Direct flight control messages
......@@ -787,8 +831,25 @@ class WaypointGoto(Message):
fields = struct.unpack_from(type(self).msg_fmt, data, 0)
self.index = int(fields[0])
class Land(Message):
class Takeoff(Message):
msg_type = 0x62
msg_fmt = '>h'
def __init__(self):
Message.__init__(self)
self.takeoff_alt = 0
def _pack(self):
tupl = (int(self.takeoff_alt),)
return struct.pack(type(self).msg_fmt, *tupl)
def _unpack(self, data):
fields = list(struct.unpack_from(type(self).msg_fmt, data, 0))
self.takeoff_alt = fields.pop(0)
class Land(Message):
msg_type = 0x63
msg_fmt = ''
def __init__(self):
......@@ -803,7 +864,7 @@ class Land(Message):
pass
class LandAbort(Message):
msg_type = 0x63
msg_type = 0x64
msg_fmt = '>h2x'
def __init__(self):
......@@ -820,6 +881,55 @@ class LandAbort(Message):
fields = struct.unpack_from(type(self).msg_fmt, data, 0)
self.alt = int(fields[0])
class Yaw(Message):
msg_type = 0x65
msg_fmt = '>h'
def __init__(self):
Message.__init__(self)
self.yaw_degrees = 0
def _pack(self):
tupl = (int(self.yaw_degrees),)
return struct.pack(type(self).msg_fmt, *tupl)
def _unpack(self, data):
fields = list(struct.unpack_from(type(self).msg_fmt, data, 0))
self.yaw_degrees = fields.pop(0)
# Initiate drop of next available external store
class StoresDrop(Message):
msg_type = 0x66
msg_fmt = ''
def __init__(self):
Message.__init__(self)
pass
def _pack(self):
return ''
def _unpack(self, data):
pass
# Manually initiate jettison of all stores
class StoresJettison(Message):
msg_type = 0x67
msg_fmt = ''
def __init__(self):
Message.__init__(self)
pass
def _pack(self):
return ''
def _unpack(self, data):
pass
#--------------------------------------
# UAV position/state reporting messages
......@@ -890,20 +1000,21 @@ class Pose(Message):
class RedPose(Message):
''' Pose data for a "red" UAV '''
msg_type = 0x81
msg_fmt = '>BB2xlllhhh2x'
msg_fmt = '>BB2xlllhhhBx'
def __init__(self):
Message.__init__(self)
# Define message fields (setting to None helps raise Exceptions later)
self.uav_id = 0 # ID (int) of the red UAV
self.game_state = 0 #
self.game_state = 0 # Status of the UAV in the game
self.lat = 0.0 # Decimal degrees (e.g. 35.123456)
self.lon = 0.0 # Decimal degrees (e.g. -120.123456)
self.alt = 0.0 # Decimal meters MSL (WGS84)
self.vlx = 0.0 # Linear velocity x (cm/s)
self.vly = 0.0 # Linear velocity y (cm/s)
self.vlz = 0.0 # Linear velocity z (cm/s)
self.vehicle_type = 0 #Vehicle type (AC_COPTER, AC_FIXED_WING)
def _pack(self):
# Convert message elements into pack-able fields and form tuple
......@@ -913,7 +1024,8 @@ class RedPose(Message):
int(self.alt * 1e03),
int(self.vlx * 1e02),
int(self.vly * 1e02),
int(self.vlz * 1e02))
int(self.vlz * 1e02),
self.vehicle_type)
# Pack into a byte string
return struct.pack(type(self).msg_fmt, *tupl)
......@@ -931,6 +1043,7 @@ class RedPose(Message):
self.vlx = fields[5] / 1e02
self.vly = fields[6] / 1e02
self.vlz = fields[7] / 1e02
self.vehicle_type = fields[8]
class VehicleIntent(Message):
msg_type = 0x82
......@@ -1225,20 +1338,24 @@ class AssessPenalty(Message):
class HitReport(Message):
msg_type = 0xF2
msg_fmt = '>B3x'
msg_fmt = '>BB?x'
def __init__(self):
Message.__init__(self)
self.hit_uav_id = 0
self.shooter_id = 0
self.is_kill = False
def _pack(self):
tupl = (int(self.hit_uav_id), )
tupl = (int(self.hit_uav_id), self.shooter_id, self.is_kill)
return struct.pack(type(self).msg_fmt, *tupl)
def _unpack(self, data):
fields = struct.unpack_from(type(self).msg_fmt, data, 0)
self.hit_uav_id = fields[0]
self.shooter_id = fields[1]
self.is_kill = fields[2]
class AttackReport(Message):
msg_type = 0xF3
......@@ -1257,9 +1374,28 @@ class AttackReport(Message):
fields = struct.unpack_from(type(self).msg_fmt, data, 0)
self.attacker_id = fields[0]
class ScoreSummary(Message):
class WinchesterReport(Message):
''' Used by the Arbiter to tell a UAV it's out of ammo '''
msg_type = 0xF4
msg_fmt = '>ffBBBBBBBBBB2x'
msg_fmt = '>B3x'
def __init__(self):
Message.__init__(self)
self.id = 0
def _pack(self):
tupl = (int(self.id), )
return struct.pack(type(self).msg_fmt, *tupl)
def _unpack(self, data):
fields = struct.unpack_from(type(self).msg_fmt, data, 0)
self.id = fields[0]
class ScoreSummary(Message):
msg_type = 0xF5
msg_fmt = '>ffBBBBBBBBBBBB'
msg_fmt_base_sz = struct.calcsize(msg_fmt)
msg_fmt_uav = 'B'
msg_fmt_uav_sz = struct.calcsize(msg_fmt_uav)
......@@ -1273,21 +1409,25 @@ class ScoreSummary(Message):
self.num_blue_dead = 0
self.num_blue_penalty = 0
self.num_blue_offense = 0
self.num_blue_attack = 0
self.num_red_in_area = 0
self.num_red_out_of_area = 0
self.num_red_dead = 0
self.num_red_penalty = 0
self.num_red_offense = 0
self.num_red_attack = 0
self.blues_in_area = []
self.blues_out_of_area = []
self.blues_dead = []
self.blues_penalty = []
self.blues_offense = []
self.blues_attack = []
self.reds_in_area = []
self.reds_out_of_area = []
self.reds_dead = []
self.reds_penalty = []
self.reds_offense = []
self.reds_attack = []
def _pack(self):
self.num_blue_in_area = len(self.blues_in_area)
......@@ -1295,23 +1435,26 @@ class ScoreSummary(Message):
self.num_blue_dead = len(self.blues_dead)
self.num_blue_penalty = len(self.blues_penalty)
self.num_blue_offense = len(self.blues_offense)
self.num_blue_attack = len(self.blues_attack)
self.num_red_in_area = len(self.reds_in_area)
self.num_red_out_of_area = len(self.reds_out_of_area)
self.num_red_dead = len(self.reds_dead)
self.num_red_penalty = len(self.reds_penalty)
self.num_red_offense = len(self.reds_offense)
self.num_red_attack = len(self.reds_attack)
num_uavs = self.num_blue_in_area + self.num_blue_out_of_area + \
self.num_blue_dead + self.num_blue_penalty + \
self.num_red_in_area + self.num_red_out_of_area + \
self.num_red_dead + self.num_red_penalty
padding = (num_uavs + self.num_red_offense + self.num_blue_offense) % 4
padding = (num_uavs + self.num_red_offense + self.num_blue_offense + \
self.num_blue_attack + self.num_red_attack) % 4
tupl = (self.blue_score, self.red_score, \
self.num_blue_in_area, self.num_blue_out_of_area, \
self.num_blue_dead, self.num_blue_penalty, \
self.num_blue_offense, \
self.num_blue_offense, self.num_blue_attack, \
self.num_red_in_area, self.num_red_out_of_area, \
self.num_red_dead, self.num_red_penalty, \
self.num_red_offense)
self.num_red_offense, self.num_red_attack)
for uav in range(0, self.num_blue_in_area):
tupl += (self.blues_in_area[uav],)
......@@ -1328,6 +1471,9 @@ class ScoreSummary(Message):
for uav in range(0, self.num_blue_offense):
tupl += (self.blues_offense[uav],)
for uav in range(0, self.num_blue_attack):
tupl += (self.blues_attack[uav],)
for uav in range(0, self.num_red_in_area):
tupl += (self.reds_in_area[uav],)
......@@ -1343,10 +1489,15 @@ class ScoreSummary(Message):
for uav in range(0, self.num_red_offense):
tupl += (self.reds_offense[uav],)
for uav in range(0, self.num_red_attack):
tupl += (self.reds_attack[uav],)
fmt = type(self).msg_fmt + \
(num_uavs * type(self).msg_fmt_uav) + \
(self.num_blue_offense * type(self).msg_fmt_uav) + \
(self.num_blue_attack * type(self).msg_fmt_uav) + \
(self.num_red_offense * type(self).msg_fmt_uav) + \
(self.num_red_attack * type(self).msg_fmt_uav) + \
(padding * 'x')
self.msg_size = self.hdr_size + struct.calcsize(fmt) # Variable size
return struct.pack(fmt, *tupl)
......@@ -1355,15 +1506,16 @@ class ScoreSummary(Message):
self.msg_size = self.hdr_size + len(data)
(self.blue_score, self.red_score, \
self.num_blue_in_area, self.num_blue_out_of_area, \
self.num_blue_dead, self.num_blue_penalty, self.num_blue_on_offense, \
self.num_red_in_area, self.num_red_out_of_area, \
self.num_red_dead, self.num_red_penalty, self.num_red_on_offense) = \
self.num_blue_dead, self.num_blue_penalty, self.num_blue_offense, \
self.num_blue_attack, self.num_red_in_area, self.num_red_out_of_area, \
self.num_red_dead, self.num_red_penalty, self.num_red_offense, \
self.num_red_attack) = \
struct.unpack_from(type(self).msg_fmt, data, 0)
(self.blues_in_area, self.blues_out_of_area, \
self.blues_dead, self.blues_penalty, self.blues_offense, \
self.blues_dead, self.blues_penalty, self.blues_offense, self.blues_attack, \
self.reds_in_area, self.reds_out_of_area, \
self.reds_dead, self.reds_penalty, self.reds_offense) = \
([], [], [], [], [], [], [], [], [], [])
self.reds_dead, self.reds_penalty, self.reds_offense, self.reds_attack) = \
([], [], [], [], [], [], [], [], [], [], [], [])
offset = type(self).msg_fmt_base_sz
for uav in range(0, self.num_blue_in_area):
next, = struct.unpack_from(type(self).msg_fmt_uav, data, offset)
......@@ -1385,11 +1537,16 @@ class ScoreSummary(Message):
self.blues_penalty += [next]
offset += type(self).msg_fmt_uav_sz
for uav in range(0, self.num_blue_on_offense):
for uav in range(0, self.num_blue_offense):
next, = struct.unpack_from(type(self).msg_fmt_uav, data, offset)
self.blues_offense += [next]
offset += type(self).msg_fmt_uav_sz
for uav in range(0, self.num_blue_attack):
next, = struct.unpack_from(type(self).msg_fmt_uav, data, offset)
self.blues_attack += [next]
offset += type(self).msg_fmt_uav_sz
for uav in range(0, self.num_red_in_area):
next, = struct.unpack_from(type(self).msg_fmt_uav, data, offset)
self.reds_in_area += [next]
......@@ -1410,8 +1567,13 @@ class ScoreSummary(Message):
self.reds_penalty += [next]
offset += type(self).msg_fmt_uav_sz
for uav in range(0, self.num_red_on_offense):
for uav in range(0, self.num_red_offense):
next, = struct.unpack_from(type(self).msg_fmt_uav, data, offset)
self.reds_offense += [next]
offset += type(self).msg_fmt_uav_sz
for uav in range(0, self.num_red_attack):
next, = struct.unpack_from(type(self).msg_fmt_uav, data, offset)
self.reds_attack += [next]
offset += type(self).msg_fmt_uav_sz
......@@ -112,13 +112,18 @@ class ReliableResend(threading.Thread):
# Resend everything that hasn't been acknowledged yet
with self._rel_lock:
# msg_l is a list of the form [ msg, #retries_left ]
to_delete = []
for seq in self._rel_sent.keys():
msg_l = self._rel_sent[seq]
if msg_l[1] > 0:
self._socket.sendExact(msg_l[0])
msg_l[1] -= 1
else:
self.delete(seq)
to_delete.append(seq)
for seq in to_delete:
self.delete(seq)
time.sleep(0.25)
......
This diff is collapsed.
......@@ -38,7 +38,9 @@ class Behavior(nodeable.Nodeable):
_crashed_keys: key values (IDs) of swarm UAVs suspected of crashing
_swarm_subscriber: subscriber object for swarm_tracker reports
_red_subscriber: subscriber object for red_tracker reports
_ap_wp_lock: lock object to avoid multiple access of autopilot wp object
_ap_wp: current autopilot waypoint ID
_ap_wp_time: ros time that the most recent waypoint was ordered
_ap_intent: most recently ordered autopilot waypoint
_swarm_lock: reentrant lock enforcing thread-safe swarm dictionary access
_reds_lock: reentrant lock enforcing thread-safe reds dictionary access
......@@ -97,8 +99,10 @@ class Behavior(nodeable.Nodeable):
self._subswarm_keys = set()
self._crashed_keys = set()
self._reds = dict()
self._ap_wp_lock = threading.RLock()
self._ap_wp = 0
self._ap_intent = None
self._ap_wp_time = None
self._uses_wp_control = False
self.is_ready = False
self.is_active = False
......@@ -456,5 +460,7 @@ class Behavior(nodeable.Nodeable):
''' Processes autopilot intent messages
@param wptMsg waypoint message
'''
self._ap_intent = wptMsg
with self._ap_wp_lock:
self._ap_intent = wptMsg
self._ap_wp_time = rospy.Time.now()
......@@ -3,6 +3,7 @@
import sys
import os
import imp
import collections
import xml.etree.ElementTree as ET
import struct
......@@ -86,7 +87,28 @@ class BehaviorParser():
index += 1
return params_dict
def test_load_behaviors(self):
for id, params in self.behaviors.items():
# Load the custom python tactic module
fp, pathname, description = imp.find_module(params['module'])
if fp is None:
self.log_error('Could not find tactic module: ' + params['module'])
return False
else:
try:
tactic_module = imp.load_module(params['module'], fp, pathname, \
description)
return True
except Exception as e:
print('***********************************************')
print(e)
print('Python tactic file contains error: %s' % params)
return False
if __name__ == '__main__':
bp = BehaviorParser()
print(bp)
sys.exit()
#print(bp)
if not bp.test_load_behaviors():
sys.exit(-1)
else:
sys.exit(0)
This diff is collapsed.
......@@ -105,6 +105,8 @@ class LossyDataExchange(object):
''' Returns True if the message publisher has been set
'''
return self._msg_publisher != None
def process_message(self, msg):
''' Processes BehaviorParameter messages from the recv_swarm_data topic
This method must be implemented by the inheriting class and should be
......
......@@ -4,11 +4,13 @@
# Most functions were copied directly from MAVProxy's mp_util.y
from ap_lib import quaternion_math as qmath
from ap_lib import math_utils as math_utils
import math
import os
import numpy
import random
import operator
EARTH_RADIUS = 6378100.0 # Approximate equatorial radius of the earth in meters
......@@ -91,7 +93,9 @@ def radius_at_latitude(lat):
def normalize_angle(angle, radians = True):
"""normalizes an angle to the range ( -180, 180 ] """
"""normalizes an angle to the range ( -180, 180 ]
NOTE: deprecated--functionality duplicated by 'normalize_pi' below
"""
if not radians:
angle = math.radians(angle)
......@@ -110,21 +114,21 @@ def hitable(lat, lon, alt, pose_quat, max_range, FOV_width, FOV_height,
targ_lat, targ_lon, targ_alt):
# Compute absolute target distance, bearing, and elevation
distance = gps_distance(lat, lon, targ_lat, targ_lon)
flat_distance = gps_distance(lat, lon, targ_lat, targ_lon)
bearing = gps_bearing(lat, lon, targ_lat, targ_lon)
alt_diff = targ_alt - alt
distance = math.hypot(distance, alt_diff)
distance = math.hypot(flat_distance, alt_diff)
elevation = math.asin(alt_diff/distance)
# Convert absolute angles to relative (shooter to target)
rpy = qmath.quat_to_euler(pose_quat)
rel_bearing = normalize_angle(bearing - rpy[2])
rel_elevation = normalize_angle(elevation - rpy[1])
rel_bearing = normalize_pi(bearing - rpy[2])
rel_elevation = normalize_pi(elevation - rpy[1])
# Determine if relative parameters are within weapon envelope
if (distance <= max_range) and \
(math.fabs(rel_bearing) <= FOV_width/2.0) and \
(math.fabs(rel_elevation) <= FOV_height/2.0):
if (flat_distance <= max_range) and \
(math.fabs(rel_bearing) <= math.radians(FOV_width)/2.0) and \
(math.fabs(rel_elevation) <= math.radians(FOV_height)/2.0):
return True
return False
......@@ -133,6 +137,7 @@ def hitable(lat, lon, alt, pose_quat, max_range, FOV_width, FOV_height,
def normalize(angle):
''' Normalizes a radian angle to the range [ 0.0, 2*pi )
NOTE: Version deprecated--moved to ap_lib.math_utils
@param angle: angle being normalized
@return an equivalent angle on the range [ 0.0, 2*pi )
'''
......@@ -143,6 +148,7 @@ def normalize(angle):
def normalize_pi(angle):
''' Normalizes a radian angle to the range [ -pi, pi )
NOTE: Version deprecated--moved to ap_lib.math_utils
@param angle: angle being normalized
@return an equivalent angle on the range [ -pi, pi )
'''
......@@ -153,6 +159,7 @@ def normalize_pi(angle):
def signum(number):
''' Returns the "sign" of the argument
NOTE: Version deprecated--moved to ap_lib.math_utils
@param number: numerical argument being evaluated
@return -1 if number < 0, 1 if number > 0, or 1 if number == 0
'''
......@@ -162,52 +169,119 @@ def signum(number):
def scalar_multiply(v1, s):
''' Multiplies the vector v1 by the scalar s
NOTE: Deprecated--function moved to ap_lib.math_utils
'''
return [x * s for x in v1]
def vector_magnitude(v):
''' Returns the magnitude of the vector v
NOTE: Deprecated--function moved to ap_lib.math_utils
'''
return math.sqrt(sum(i**2 for i in v))
def unit_vector(v):
''' Returns the unit vector aligned with the vector v
NOTE: Deprecated--function moved to ap_lib.math_utils
'''
mag = vector_magnitude(v)
mag = math_utils.vector_magnitude(v)
return [ x/mag for x in v ]
def vector_projection(v1, v2):
''' Returns a tuple consisting of the projection of vector v1 onto
vector v2 and the component (magnitude) of the projection--(p, c)
NOTE: Deprecated--function moved to ap_lib.math_utils
'''
component = numpy.dot(v1, v2) / vector_magnitude(v2)
projection = scalar_multiply(unit_vector(v2), component)
component = numpy.dot(v1, v2) / math_utils.vector_magnitude(v2)
projection = scalar_multiply(math_utils.unit_vector(v2), component)
return (projection, component)
def segment_magnitude(a, b):
''' Returns the length of a line segment defined by two end points in Cartesian space
NOTE: Deprecated--function moved to ap_lib.math_utils
@param a: Cartesian endpoint (x, y) of the line
@param b: endpoint (x, y) of the line
@return: the magnitude of the segment AB
'''
return math.sqrt(sum([((a[x]-b[x])**2) for x in range(0, len(a))]))
def line_orthogonal(a, b, c):
''' Returns a tuple representing the orthogonal projection of a point in
NOTE: Deprecated--function moved to ap_lib.math_utils
2-dimensional space, c, on a line defined by points a1 and a2
@param a: a 2-dimensional Cartesian point (x, y) on the line
@param b: a 2-dimensional Cartesian point (x, y) on the line
@param c: a Cartesian point for which the orthogonal point on line AB is being computed
@return: the Cartesian point of the orthogonal and the signed distance along the line from point a to the orthogonal
'''
relative = math_utils.vector_projection((c[0]-a[0], c[1]-a[1]), (b[0]-a[0], b[1]-a[1]))
orthogonal = (relative[0][0] + a[0], relative[0][1] + a[1])
return (orthogonal, relative[1])
def segment_orthogonal(a, b, c):
''' Returns a tuple representing the orthogonal projection of a point in
2-dimensional space, c, on a line segment defined by points a1 and a2
NOTE: Deprecated--function moved to ap_lib.math_utils
@param a: a Cartesian endpoint point of the segment
@param b: a Cartesian endpoint point of the segment
@param c: a Cartesian point for which the orthogonal point is being computed
@return: the Cartesian point of the orthogonal (or None)
'''
(orthogonal, distance) = math_utils.line_orthogonal(a, b, c)
if (distance < 0.0) or (distance > math_utils.segment_magnitude(a, b)):
return None
return orthogonal
def closest_segment_pt(a, b, c):
''' Returns a tuple representing the point on a line segment AB
closest to a third point c
NOTE: Deprecated--function moved to ap_lib.math_utils
@param a: a Cartesian endpoint point of the segment
@param b: a Cartesian endpoint point of the segment
@param c: a Cartesian point for which the closest point is being computed
@return: the Cartesian point of the orthogonal (or none)
'''
(orthogonal, distance) = math_utils.line_orthogonal(a, b, c)
if (distance < 0.0):
return a
elif (distance > math_utils.segment_magnitude(a, b)):
return b
return orthogonal
def get_battleboxes():
from ap_lib import ap_enumerations as enums
battleBox = GeoBox(enums.BATTLE_CUBE_SW_LAT, enums.BATTLE_CUBE_SW_LON, \
enums.BATTLE_CUBE_LENGTH, enums.BATTLE_CUBE_WIDTH, \
enums.BATTLE_CUBE_ORIENT, enums.BATTLE_CUBE_MIN_ALT, \
enums.BATTLE_CUBE_MAX_ALT)
enums.BATTLE_CUBE_LENGTH, enums.BATTLE_CUBE_WIDTH, \
enums.BATTLE_CUBE_ORIENT, enums.BATTLE_CUBE_MIN_ALT, \
enums.BATTLE_CUBE_MAX_ALT)
blueBox = GeoBox(enums.BATTLE_CUBE_SW_LAT, enums.BATTLE_CUBE_SW_LON, \
enums.BATTLE_CUBE_LENGTH / 2.0, enums.BATTLE_CUBE_WIDTH, \
enums.BATTLE_CUBE_ORIENT, enums.BATTLE_CUBE_MIN_ALT, \
enums.BATTLE_CUBE_MAX_ALT)
enums.BATTLE_CUBE_LENGTH / 2.0, enums.BATTLE_CUBE_WIDTH, \
enums.BATTLE_CUBE_ORIENT, enums.BATTLE_CUBE_MIN_ALT, \
enums.BATTLE_CUBE_MAX_ALT)
blueStage = GeoBox(enums.BLUE_STAGE_SW_LAT, enums.BLUE_STAGE_SW_LON, \
enums.BATTLE_CUBE_LENGTH, enums.STAGE_CUBE_WIDTH, \
enums.BATTLE_CUBE_ORIENT, 0.0, enums.BATTLE_CUBE_MAX_ALT)
redBox_SW_Corner = gps_newpos(enums.BATTLE_CUBE_SW_LAT, \
enums.BATTLE_CUBE_SW_LON, \
math.radians(enums.BATTLE_CUBE_ORIENT) + math.pi / 2.0, \
enums.BATTLE_CUBE_WIDTH / 2.0)
enums.BATTLE_CUBE_SW_LON, \
math.radians(enums.BATTLE_CUBE_ORIENT) + math.pi / 2.0, \
enums.BATTLE_CUBE_WIDTH / 2.0)
redBox = GeoBox(redBox_SW_Corner[0], redBox_SW_Corner[1], \
enums.BATTLE_CUBE_LENGTH, enums.BATTLE_CUBE_WIDTH / 2.0, \
enums.BATTLE_CUBE_ORIENT, enums.BATTLE_CUBE_MIN_ALT, \
enums.BATTLE_CUBE_MAX_ALT)
return tuple((battleBox, redBox, blueBox))
enums.BATTLE_CUBE_LENGTH, enums.BATTLE_CUBE_WIDTH / 2.0, \
enums.BATTLE_CUBE_ORIENT, enums.BATTLE_CUBE_MIN_ALT, \
enums.BATTLE_CUBE_MAX_ALT)
redStage = GeoBox(enums.RED_STAGE_SW_LAT, enums.RED_STAGE_SW_LON, \
enums.BATTLE_CUBE_LENGTH, enums.STAGE_CUBE_WIDTH, \
enums.BATTLE_CUBE_ORIENT, 0.0, enums.BATTLE_CUBE_MAX_ALT)
return tuple((battleBox, blueBox, blueStage, redBox, redStage))
class GeoBox():
''' Class for defining a geographically placed rectangular box
......@@ -236,13 +310,14 @@ class GeoBox():
ne_corner[0], ne_corner[1]), \
cartesian_offset(sw_lat, sw_lon, \
se_corner[0], se_corner[1]) )
center_up = gps_newpos(sw_lat, sw_lon, orientation, length/2.0)
self._center = gps_newpos(center_up[0], center_up[1], \
orientation + math.pi/2.0, width/2.0)
self._floor = floor
self._ceiling = ceiling
self._length = length
self._width = width
self._orientation = orientation
# Corners as vectors and unit vectors used in distance calculations
vector01 = self._cart_corners[1]
uv01 = ((vector01[0] / length), (vector01[1] / length))
vector12 = ((self._cart_corners[2][0] - self._cart_corners[1][0]), \
......@@ -254,8 +329,8 @@ class GeoBox():
vector30 = ((self._cart_corners[0][0] - self._cart_corners[3][0]), \
(self._cart_corners[0][1] - self._cart_corners[3][1]))
uv30 = ((vector30[0] / width), (vector30[1] / width))
self._vectors = (vector01, vector12, vector23, vector30)
self._unit_vectors = (uv01, uv12, uv23, uv30)
self.__vectors = (vector01, vector12, vector23, vector30)
self.__unit_vectors = (uv01, uv12, uv23, uv30)
def get_corners(self):
......@@ -264,6 +339,12 @@ class GeoBox():
return tuple(self._corners)
def get_cartesian_corners(self):
''' Returns a tuple containing the GeoBox object's corners
'''
return tuple(self._cart_corners)
def contains(self, lat, lon, alt):
''' Returns True if the GeoBox contains the lat/lon/alt point
@param lat: latitude being tested
......@@ -309,11 +390,11 @@ class GeoBox():
if self.contains(lat, lon, self._floor):
return 0.0
xy = self._vector_aligned_pts(lat, lon)
p0 = vector_projection(xy[0], self._vectors[0])
p1 = vector_projection(xy[1], self._vectors[1])
p2 = vector_projection(xy[2], self._vectors[2])
p3 = vector_projection(xy[3], self._vectors[3])
xy = self.__vector_aligned_pts(lat, lon)
p0 = math_utils.vector_projection(xy[0], self.__vectors[0])
p1 = math_utils.vector_projection(xy[1], self.__vectors[1])
p2 = math_utils.vector_projection(xy[2], self.__vectors[2])
p3 = math_utils.vector_projection(xy[3], self.__vectors[3])
if ((p0[1] >= 0.0) and (p0[1] <= self._length)):
return min(math.hypot(p0[0][0]-xy[0][0], p0[0][1]-xy[0][1]), \
math.hypot(p2[0][0]-xy[2][0], p2[0][1]-xy[2][1]))
......@@ -342,7 +423,13 @@ class GeoBox():
return gps_newpos(self._corners[0][0], self._corners[0][1], brng, dist)
def _vector_aligned_pts(self, lat, lon):
def center_point(self):
''' Returns the latitude/longitude of the GeoBox's center point
'''
return self._center
def __vector_aligned_pts(self, lat, lon):
''' Returns a series of cartesian coordinates corresponding to
positions relative to each of the four corners of the box
'''
......
......@@ -130,7 +130,8 @@ class HybridBehavior(behavior.Behavior):
#verify valid YPS order
if abs(rt.yawRate) <= enums.MAX_YAW_RATE and \
abs(rt.pitchRate) < enums.MAX_PITCH_RATE and \
rt.speed >= enums.MIN_FWD_SPEED and rt.speed <= enums.MAX_FWD_SPEED:
rt.speed >= enums.MIN_FW_FWD_SPEED and \
rt.speed <= enums.MAX_FW_FWD_SPEED:
self._rtPublisher.publish(rt)
else:
self.set_active(False)
......
This diff is collapsed.
#!/usr/bin/env python
#-------------------------------------------------------------------------
# PluginBehavior
# Duane Davis, 2017
#
# Defines an abstract class for ACS plugin behaviors that can be loaded
# into a behavior_manager ROS node for selectable behavior implementation
#-------------------------------------------------------------------------
# ROS imports
import rospy
# ACS imports
import ap_msgs.msg as apmsg
class PluginBehavior():
''' Abstract class for defining ACS swarm behavior controllers for
loading into the behavior_manager ROS node to provide selectable behavior
implementation. The class will provide basic functionality and an
interface to required behavior_manager objects (e.g., swarm information)
and functionality (e.g., waypoint publication).
Class member variables:
id: Unique integer identifier for this behavior
manager: BehaviorManager object to which this behavior belongs
__name: string name of this behavior
__ready: set to True when the behavior is parameterized to run
__active: set to True when the behavior is actively running
__activation_time: ROS time at which the behavior was activated
__state: container object for behavior status reporting
Class methods
set_manager: sets the manager object for this object
get_name: returns the string name of the behavior
set_ready: safely sets the ready state to True or False
is_ready: returns the behavior's current readiness state
set_active: safely activates or deactivates the behavior
is_active: returns the behavior's current active state
activation_time: returns time at which the behavior was activated
time_since_activation: returns elapsed time since activation
get_status: returns the behavior's current ready and active status
Virtual methods for inheriting class implementation
parameterize: parameterizes the behavior in preparation for activation
compute_command: runs one iteration of the behavior's control loop
safety_checks: runs required checks for continued safe execution
process_behavior_data: process SwarmBehaviorData messages
process_own_hit: perform any required "cleanup" when this UAV is hit
'''
MAX_ACTIVATE_TIME = rospy.Duration(10.0)
def __init__(self, behavior_id, behavior_name, manager=None):
''' Class initializer sets base class member variables
@param behavior_id: unique identifier for this behavior
@param behavior_name: string name for this behavior
@param manager: BehaviorManager object to which this behavior belongs
'''
if not isinstance(behavior_id, int):
raise Exception("Behavior ID must be of type int")
self.id = behavior_id
self.manager = manager
self.__name = behavior_name
self.__state = apmsg.BehaviorState()
self.__ready = False
self.__active = False
self.__activation_time = None
self.__state.behavior_id = self.id
self.__state.sequence = 0
self.__state.is_paused = False
#-------------------------------------------------------------------
# Virtual methods of this class (must be implement in child classes)
#-------------------------------------------------------------------
def parameterize(self, params):
''' Sets behavior parameters based on provided parameters
This is a "virtual" method that must be implemented by inheriting
classes. Implementations must parse the params unsigned byte
array and set behavior-specific parameters accordingly.
@param params: bitmapped byte array of behavior-specific parameters
@return True if set with valid parameters
'''
return True
def compute_command(self):
''' Runs one iteration of the behavior's control loop
Implementing classes should generate a waypoint, rate, or waypoint_id
command for return to the calling object. The parent BehaviorManager
object provides a wpt_cmd (LLA) and rate_cmd (YPS) container object
for computed waypoint or rate respectively. A waypoint_id must be
an integer corresponding to a loaded mission waypoint. Returning
None will essentially be treated as a "pass". Returning any other
object type will result in the behavior being deactivated.
@return waypoint (LLA), rate (YPS), or waypoint_id command (or None)
'''
return None
def safety_checks(self):
''' Conducts behavior-specific safety checks
@return True if the behavior passes all safety checks (False otherwise)
'''
return True
def process_own_hit(self):
''' Perform any "cleanup" required when this UAV is hit
'''
pass
def process_behavior_data(self, data_msg):
''' Processes vehicle-to-vehicle SwarmBehaviorData messages
@param data_msg: ap_msgs.SwarmBehaviorData object with message data
'''
pass
#--------------------------------------------------------
# Class-specific methods implementing class functionality
#--------------------------------------------------------
def set_manager(self, behavior_id, manager):
''' Sets the behavior's owning BehaviorManager and the ID that the
owning object will use for the behavior.
@param behavior_id: unique integer identifier for this behavior
@param manager: behavior manager object to which this behavior belongs
'''
if not isinstance(behavior_id, int):
raise Exception("Behavior ID must be of type int")
self.id = behavior_id
self.manager = manager
self.__state.behavior_id = behavior_id
def get_name(self):
''' Returns the string name of the behavior
'''
return self.__name
def get_status(self):
''' Returns an object containing the behavior's current ready and
active state.
'''
self.__state.is_ready = self.__ready
self.__state.is_active = self.__active
return self.__state
def is_ready(self):
return self.__ready
def is_active(self):
return self.__active
def activation_time(self):
''' Returns the ROS time that the behavir was activated
(or None if the behavior is inactive)
'''
if self.__active:
return self.__activation_time
return None
def time_since_activation(self, t=None):
''' Returns the length of time since the behavior was activated
(or None if the behavior is inactive)
'''
if not t:
t = rospy.Time.now()
if self.__active:
return t - self.__activation_time
return None
def set_active(self, activate):
''' Used to activate and deactivate the behavior
Will not activate an "unready" behavior or one that is already active
@param activate: Boolean value to activate or deactivate the behavior
@return active status upon method completion
'''
if activate and not self.__ready:
self.__active = False
self.manager.log_warn("Attempt to activate uninitialized behavior %d"\
%self.id)
elif activate and not self.__active:
self.__active = activate
self.__activation_time = rospy.Time.now()
self.manager.log_info("Activating behavior %d"%self.id)
elif not activate and self.__active:
self.manager.log_info("Deactivating behavior %d"%self.id)
self.set_ready(False)
return self.__active
def set_ready(self, ready):
''' Sets the behavior's ready state when new control inputs received
Publishes an updated behavior status message if the ready state changes
Will also ensure that the behavior is deactivated if the ready state
is set to false.
@param ready: Boolean value to set the behavior's ready state
@return Boolean ready-state value
'''
# Only needs to do anything if the ready state is changing
if not self.__ready and ready:
self.__ready = True
self.manager.log_info("Behavior %d ready state set to 'True'"%self.id)
elif (self.__ready or self.__active) and not ready:
self.__ready = False
self.__active = False
self.manager.log_info("Behavior %d ready state and active state set to 'False'"\
%self.id)
return self.__ready
......@@ -38,7 +38,11 @@ def quaternion_squared_magnitude(q):
# @return a unit vector in the same dimensions as the parameter
def normalize_quaternion(q):
l = quaternion_magnitude(q)
return [ q[0]/l, q[1]/l, q[2]/l, q[3]/l ]
try:
return [ q[0]/l, q[1]/l, q[2]/l, q[3]/l ]
except:
print("Exception attempting to normalize quaternion. Division by zero?")
return [0.0, 0.0, 0.0, 0.0]
# Adds two quaternions and returns the quaternion result
......
......@@ -100,7 +100,8 @@ class RateBehavior(behavior.Behavior):
#verify valid YPS order
if abs(rt.yawRate) <= enums.MAX_YAW_RATE and \
abs(rt.pitchRate) < enums.MAX_PITCH_RATE and \
rt.speed >= enums.MIN_FWD_SPEED and rt.speed <= enums.MAX_FWD_SPEED:
rt.speed >= enums.MIN_FW_FWD_SPEED and \
rt.speed <= enums.MAX_FW_FWD_SPEED:
self._rtPublisher.publish(rt)
else:
self.set_active(False)
......
#!/usr/bin/env python
import rospy
import threading
import time
#-----------------------------------------------------------------------
# Interface between two ROS architectures running simultaneously
class ROStoROSBridge(object):
''' Generic class that can be used to bridge from one ROS architecture
to another running simultaneously on the same host
'''
# Container for timed event callbacks, with timing logic
class _TimedEvent(object):
''' Container for timed event callbacks with timing logic
'''
def __init__(self, interval, callback):
self._interval = interval
self._callback = callback
self._next_time = time.time() + self._interval
def due(self, t=None):
if not t:
t = time.time()
return bool(self._next_time <= t)
def run(self, t=None):
self._callback()
if not t:
t = time.time()
self._next_time = t + self._interval
class _TypeAndObj(object):
''' Basic two-object container pairing a type and an object
'''
def __init__(self, t=None, o=None):
self.type = t
self.obj = o
def __init__(self, node_name=None):
''' Initialize class variables and register as a ROS node
'''
rospy.init_node(node_name)
# Initialize stores for handlers and ROS objects
self.msg_handlers = dict()
self.publishers = dict()
self.timed_events = []
# Utility methods
def _getArg(self, arg, ros_param, default, error_text='<unknown>'):
''' Utility method to retrieve parameter and argument values
Preference is the command-line argument value, then the ROS
parameter value, then the default.
'''
if arg:
return arg
elif rospy.has_param(ros_param):
return rospy.get_param(ros_param)
elif default:
return default
else:
raise Exception("Could not find setting for " + error_text)
def doInThread(self, main_cb, error_cb=None):
''' Run a function in its own thread with an optional error callback
@param main_cb: function to run
@param error_cb: optional function to run on an exception
'''
def wrapper():
try:
main_cb()
except Exception as ex:
if callable(error_cb): error_cb()
rospy.logwarn("THREAD ERROR: " + str(ex))
try:
t = threading.Thread(target=wrapper)
t.setDaemon(True) # Allows termination with outstanding threads
t.start()
except Exception as ex:
if callable(error_cb): error_cb()
raise Exception("doInThread: " + str(ex))
# ROS message-publication and service-call methods
def addPublisher(self, topic, m_type, queue_size=1, latched=False):
''' Create a publisher object (to one of the bridged ROS systems)
@param topic: ROS topic to which messages are to be published
@param m_type: type of ROS message to be published
@queue_size: maximum number of messages to queue
@latched: last published message is latched to the topic
'''
try:
# See if a publisher of the correct type exists, or make one
if topic not in self.publishers:
pub = rospy.Publisher(topic,
m_type,
tcp_nodelay=True,
latch=latched,
queue_size=queue_size)
tao = ROStoROSBridge._TypeAndObj(m_type, pub)
self.publishers[topic] = tao
elif self.publishers[topic].type != m_type:
raise Exception("type mismatch for " + topic)
except Exception as ex:
raise Exception("addPublisher: " + str(ex))
def publish(self, topic, msg):
''' Publish a ROS message to a topic (publisher must already exist)
'''
try:
self.publishers[topic].obj.publish(msg)
except Exception as ex:
raise Exception("publish: " + str(ex))
def callService(self, s_name, s_type, **s_fields):
''' Make a ROS service call
'''
try:
srv = rospy.ServiceProxy(s_name, s_type)
return srv(**s_fields)
except Exception as ex:
raise Exception("callService: " + str(ex))
def setParam(self, name, value):
''' Set a ROS parameter value
'''
try:
rospy.set_param(name, value)
except Exception as ex:
raise Exception("setParam: " + str(ex))
### Methods to add handlers for ROS topic callbacks and timed events
def addSubHandler(self, m_topic, m_type, m_func, log_success=False):
''' Add a handler for a ROS topic subscription
@m_topic: ROS topic to which to subscribe
@m_type: type of ROS message that will be published to the topic
@m_func: callback function for the subscription
@log_success: True if you want ROS to log message receipt
'''
def wrapper(msg):
try:
m_func(msg)
if log_success:
rospy.loginfo("SUB %s" % m_topic)
except Exception as ex:
rospy.logwarn("SUB ERROR %s: %s" % (m_topic, str(ex)))
rospy.Subscriber(m_topic, m_type, wrapper)
def addTimedHandler(self, hz, callback):
''' Add a timed event that occurs with specific periodicity
'''
def wrapper():
try:
callback()
except Exception as ex:
rospy.logwarn("TIMED ERROR: " + str(ex))
interval = 1.0 / float(hz) # NOTE: 0 Hz is illegal anyway
self.timed_events.append(ROStoROSBridge._TimedEvent(interval, wrapper))
# Primary method that runs the bridge node
def runLoop(self, loop_rate):
''' Run the bridge node as a loop to execute timed events
(everything else is handled using callbacks)
'''
rate = rospy.Rate(loop_rate)
while not rospy.is_shutdown():
try: # NOTE: This outer try is extra safety, shouldn't need it
# Handle timed events
t = time.time()
for ev in self.timed_events:
if ev.due(t):
# NOTE: technically, should use the time when the event
# actually ran, but this is slightly more efficient.
ev.run(t)
except Exception as ex:
try:
print "runLoop error" # try to get some info out
rospy.logwarn("runLoop error: " + str(ex))
except:
pass # Don't log if it causes another error
import numpy as np
import math
import utm
import pyquaternion as quat
import autopilot_bridge.msg as apbrg
from angles import normalize
def from_gps_heading(rad):
''' Converts GPS heading to local cartesian heading
'''
return normalize(math.pi/2 - normalize(rad, 0, 2*math.pi), 0, 2*math.pi)
def to_gps_heading(rad):
''' Converts local cartesian heading to GPS heading
'''
return normalize(math.pi/2 - normalize(rad, 0, 2*math.pi), 0, 2*math.pi)
class SASCState(object):
def __init__(self, geod=None, id=None):
if geod == None:
return
self.id = id
# ACS Velocities are in North(+x), East(+y), Down(+z)
self.vel = np.array([geod.twist.twist.linear.y,
geod.twist.twist.linear.x,
geod.twist.twist.linear.z])
xy = utm.from_latlon(geod.pose.pose.position.lat,
geod.pose.pose.position.lon)
self.pos = np.array([xy[0], xy[1], geod.pose.pose.position.rel_alt])
self._zone_num = xy[2]
self._zone_letter = xy[3]
self.set_wxyz_gps(geod.pose.pose.orientation.w,
geod.pose.pose.orientation.x,
geod.pose.pose.orientation.y,
geod.pose.pose.orientation.z)
def in_field_of_view(self, pos, fov_width, fov_height):
''' Returns true if the positions is in the field of view of own pose
'''
rel_vec = pos - self.pos
pure_quat = quat.Quaternion(0, rel_vec[0], rel_vec[1], rel_vec[2])
rel_pos = (self.quat.inverse * pure_quat * self.quat).vector
az = math.atan2(rel_pos[1], rel_pos[0])
norm_xy = math.sqrt(math.pow(rel_pos[0], 2.0) + math.pow(rel_pos[1], 2.0))
el = math.atan2(rel_pos[2], norm_xy)
return (abs(az) < fov_width / 2.0) and (abs(el) < fov_height / 2.0)
def set_wxyz_gps(self, w, x, y, z):
self.quat_gps = quat.Quaternion(w, x, y, z)
self.quat = self.quat_gps
# Convert GPS heading to local cartesian heading
roll = self.roll()
pitch = -self.pitch() # negate pitch for visualization purposes
yaw = self.yaw()
self.set_rpy(roll, pitch, from_gps_heading(yaw))
def local_quat(self):
return self.quat
def set_rpy(self, roll, pitch, yaw):
'''Set the internal quaternion with Euler roll, pitch, yaw
Set roll, pitch, yaw in local coordinate system. Angles are in radians
'''
sr = math.sin(roll / 2.0);
cr = math.cos(roll / 2.0);
sy = math.sin(yaw / 2.0);
cy = math.cos(yaw / 2.0);
sp = math.sin(pitch / 2.0);
cp = math.cos(pitch / 2.0);
w = cr * cp * cy + sr * sp * sy;
x = sr * cp * cy - cr * sp * sy;
y = cr * sp * cy + sr * cp * sy;
z = cr * cp * sy - sr * sp * cy;
self.quat = quat.Quaternion(w, x, y, z)
def roll(self):
'''Get state Euler roll in radians
Angles are in radians
'''
return math.atan2(2.0 * (self.quat.scalar * self.quat.vector[0] + self.quat.vector[1] * self.quat.vector[2]),
1.0 - 2.0 * (math.pow(self.quat.vector[0], 2.0) + math.pow(self.quat.vector[1], 2.0)));
def pitch(self):
'''Get state Euler pitch in radians
Angles are in radians
'''
return math.asin(2.0 * (self.quat.scalar * self.quat.vector[1] - self.quat.vector[2] * self.quat.vector[0]));
def yaw(self):
'''Get state Euler yaw in radians
Angles are in radians
'''
return math.atan2(2.0 * (self.quat.scalar * self.quat.vector[2] + self.quat.vector[0] * self.quat.vector[1]),
1.0 - 2.0 * (math.pow(self.quat.vector[1], 2.0) + math.pow(self.quat.vector[2], 2.0)));
def lla(self):
'''Get state's latitude/latitude/altitude in GPS frame
'''
ll = utm.to_latlon(self.pos[0], self.pos[1],
self._zone_num, self._zone_letter)
return (ll[0], ll[1], self.pos[2])
def local_2_lla(self, local_wp):
''' Convert a local cartesian waypoint to GPS frame waypoint
'''
ll = utm.to_latlon(local_wp[0], local_wp[1],
self._zone_num, self._zone_letter)
return (ll[0], ll[1], self.pos[2])
......@@ -11,9 +11,9 @@ import struct
from ap_lib import bitmapped_bytes
#from ap_lib import distributed_algorithms
PURSUIT_MESSAGE = 22 # Data field represents a pursuit message
PURSUIT_UPDATE_REQUEST = 23 # List of Enemies being pursued
PURSUIT_LIST = 24 #Used by swarm commander to give new swarm member the pursuit list
PURSUIT_MESSAGE = 23 # Data field represents a pursuit message
PURSUIT_UPDATE_REQUEST = 24 # List of Enemies being pursued
PURSUIT_LIST = 25 #Used by swarm commander to give new swarm member the pursuit list
class PursuitMessageParser(bitmapped_bytes.BitmappedBytes):
......
......@@ -17,6 +17,7 @@ from rospy import rostime
from ap_lib import nodeable
from ap_lib import behavior
from autopilot_bridge.msg import LLA
from autopilot_bridge.msg import Status as apStatus
import ap_lib.ap_enumerations as enums
......@@ -68,7 +69,11 @@ class WaypointBehavior(behavior.Behavior):
self._last_wp_id = 0
self.wp_msg = LLA()
self._wpPublisher = None
self._prev_publish_time = rospy.Time()
self._vehicle_type = enums.AC_UNKNOWN
def _ap_status_handler(self, msg):
self._vehicle_type = msg.vehicle_type
def runAsNode(self, rate=10.0):
''' Method that is called to run this object as an independent node.
......@@ -80,6 +85,11 @@ class WaypointBehavior(behavior.Behavior):
instantiated if the behavior is running as an independent node.
@param rate: rate (hz) of the node's timing loop
'''
self._ap_status_sub = rospy.Subscriber("autopilot/status",
apStatus,
self._ap_status_handler)
self._wpPublisher = \
rospy.Publisher("safety/payload_waypoint", LLA, \
tcp_nodelay=True, latch=True, queue_size=1)
......@@ -101,7 +111,18 @@ class WaypointBehavior(behavior.Behavior):
if wp.alt >= enums.MIN_REL_ALT and wp.alt <= enums.MAX_REL_ALT and \
abs(wp.lon) < enums.MAX_ABS_LON and \
abs(wp.lat) < enums.MAX_ABS_LAT:
self._wpPublisher.publish(wp)
#Publish slower for copter because copter's inner loop control
#doesn't work well with fast waypoint updates.
if self._vehicle_type == enums.AC_COPTER:
now = rospy.Time.now()
if (now.to_sec() - self._prev_publish_time.to_sec()) > 2:
self._wpPublisher.publish(wp)
self._prev_publish_time = rospy.Time.now()
else:
self._wpPublisher.publish(wp)
else:
self.set_active(False)
self.log_warn("Altitude control mode invalid or invalid altitude order")
......
......@@ -11,11 +11,16 @@
<arg name="range" default="100000" /> <!-- Onboard sensor maximum range -->
<arg name="gps" default="1" /> <!-- Autopilot has GPS signal; sync clock -->
<arg name="do_bag" default="1" /> <!-- Enable rosbagging -->
<arg name="do_fetch" default="1" /> <!-- Enable param/fence/rally/wp fetch fm server -->
<arg name="do_verify" default="1" /> <!-- Enable param/fence/rally/wp verification -->
<arg name="ap_dev" default="/dev/ttyUSB*" /> <!-- Device string -->
<arg name="net_dev" default="wlan0" /> <!-- Network device -->
<arg name="net_port" default="5554" /> <!-- Network port -->
<arg name="do_decon_land" default="1" /> <!-- Whether or not to attempt to deconflict landing points (and rally points) between aircraft -->
<arg name="land_dis" default="2.0"/> <!-- distance between landing (and rally) points when deconflicting (in meters)-->
<arg name="land_dir" default="114.79"/> <!-- runway heading along which to place separated landing points -->
<!-- Derived (conditional) arguments -->
<arg name="gps_arg" default="--gps-time-hack" if="$(arg gps)" />
<arg name="gps_arg" default="" unless="$(arg gps)" />
......@@ -23,15 +28,21 @@
<!-- Instance ROS parameters -->
<param name="aircraft_id" type="int" value="$(arg id)" />
<param name="aircraft_name" type="str" value="$(arg name)" />
<param name="aircraft_type" type="int" value="0" />
<param name="network_device" type="str" value="$(arg net_dev)" />
<param name="network_port" type="int" value="$(arg net_port)" />
<param name="subswarm_id" type="int" value="0" />
<param name="flight_ready" type="bool" value="false" />
<param name="rosbag_enable" type="bool" value="$(arg do_bag)" />
<param name="fetch_enable" type="bool" value="$(arg do_fetch)" />
<param name="verify_enable" type="bool" value="$(arg do_verify)" />
<param name="sensor_range" type="int" value="$(arg range)" />
<param name="swarm_team" type="int" value="$(arg team)" />
<param name="do_deconflict_land" type="bool" value="$(arg do_decon_land)" />
<param name='dis_between_land_points' type='double' value="$(arg land_dis)" />
<param name='land_point_lineup_dir' type='double' value="$(arg land_dir)" />
<!-- Core Nodes -->
<node name="autopilot" pkg="autopilot_bridge" type="mavbridge.py" output="screen" args="--device $(arg ap_dev) --baudrate 1500000 --serial-relief 1024 -m acs -m file -m fpr -m slave -m wp -m ap_msg_queue $(arg gps_arg)" />
......@@ -39,7 +50,6 @@
<node name="safety" pkg="ap_safety_monitor" type="safety.py" output="screen" >
<remap from="safety/heartbeat" to="autopilot/heartbeat_onboard" />
<remap from="safety/gnd_heartbeat" to="autopilot/heartbeat_ground" />
<remap from="safety/ap_pose" to="autopilot/acs_pose" />
</node>
<node name="network" pkg="ap_network_bridge" type="network.py" output="screen" args="--device $(arg net_dev)" >
......@@ -51,6 +61,7 @@
<remap from="network/recv_guided_goto" to="safety/payload_waypoint" />
<remap from="network/recv_waypoint_goto" to="autopilot/waypoint_goto" />
<remap from="network/recv_ap_reboot" to="autopilot/reboot" />
<remap from="network/stores_release" to="autopilot/stores_release" />
<remap from="network/send_pose" to="autopilot/acs_pose" />
<remap from="network/update_flight_status" to="autopilot/status" />
<remap from="network/slave_setup" to="autopilot/slave_setup" />
......@@ -59,14 +70,15 @@
<remap from="network/demo_servos" to="autopilot/demo_servos" />
<remap from="network/demo_motor" to="autopilot/demo_motor" />
<remap from="network/health_state" to="safety/set_health_state" />
<remap from="network/run_behavior" to="swarm_control/run_behavior" />
<remap from="network/pause_behavior" to="swarm_control/pause_behavior" />
<remap from="network/swarm_behavior" to="swarm_control/swarm_behavior" />
<remap from="network/set_swarm_state" to="swarm_control/set_swarm_state" />
<remap from="network/set_subswarm" to="swarm_control/set_subswarm" />
<remap from="network/subswarm_id" to="swarm_control/subswarm_id" />
<remap from="network/swarm_state" to="swarm_control/swarm_state" />
<remap from="network/behavior_summary" to="swarm_control/behavior_summary" />
<remap from="network/run_behavior" to="swarm_manager/run_behavior" />
<remap from="network/swarm_behavior" to="swarm_manager/swarm_behavior" />
<remap from="network/set_swarm_state" to="swarm_manager/set_swarm_state" />
<remap from="network/set_subswarm" to="swarm_manager/set_subswarm" />
<remap from="network/subswarm_id" to="swarm_manager/subswarm_id" />
<remap from="network/swarm_state" to="swarm_manager/swarm_state" />
<remap from="network/behavior_summary" to="swarm_manager/behavior_summary" />
<remap from="network/firing_reports" to="swarm_manager/firing_reports" />
<remap from="network/attack_reports" to="swarm_manager/attack_reports" />
<remap from="network/send_swarm_search_waypoint" to="swarm_searcher/send_swarm_search_waypoint" />
<remap from="network/param_setlist" to="autopilot/fpr_param_setlist" />
<remap from="network/load_param" to="autopilot/load_param" />
......@@ -80,65 +92,45 @@
<remap from="network/wp_getrange" to="autopilot/wp_getrange" />
<remap from="network/fpr_param_get" to="autopilot/fpr_param_get" />
<remap from="network/fpr_fence_getall" to="autopilot/fpr_fence_getall" />
<remap from="network/firing_reports" to="air_to_air/firing_reports" />
</node>
<node name="swarm_tracker" pkg="ap_perception" type="swarm_tracker_node.py" output="screen" args="" >
<remap from="swarm_tracker/acs_pose" to="autopilot/acs_pose" />
<remap from="swarm_tracker/recv_pose" to="network/recv_pose" />
<remap from="swarm_tracker/recv_swarm_ctl_state" to="network/recv_swarm_ctl_state" />
<remap from="swarm_tracker/swarm_state" to="swarm_control/swarm_state" />
<remap from="swarm_tracker/swarm_behavior" to="swarm_control/swarm_behavior" />
<remap from="swarm_tracker/subswarm_id" to="swarm_control/subswarm_id" />
</node>
<node name="red_tracker" pkg="ap_perception" type="red_tracker.py" output="screen" args="" >
<remap from="red_tracker/acs_pose" to="autopilot/acs_pose" />
<remap from="red_tracker/recv_red_pose" to="network/recv_red_pose" />
</node>
<node name="hit_tracker" pkg="ap_perception" type="hit_tracker.py" output="screen" args="" >
<remap from="hit_tracker/recv_swarm_data" to="network/recv_swarm_data" />
<remap from="hit_tracker/net_reset" to="network/net_reset" />
<node name="tracker" pkg="ap_perception" type="tracker.py" output="screen" args="" >
<remap from="tracker/acs_pose" to="autopilot/acs_pose" />
<remap from="tracker/recv_pose" to="network/recv_pose" />
<remap from="tracker/recv_red_pose" to="network/recv_red_pose" />
<remap from="tracker/recv_swarm_ctl_state" to="network/recv_swarm_ctl_state" />
<remap from="tracker/recv_swarm_data" to="network/recv_swarm_data" />
<remap from="tracker/net_reset" to="network/net_reset" />
<remap from="tracker/net_killed_summary" to="network/net_killed_summary" />
<remap from="tracker/net_attack_summary" to="network/net_attack_summary" />
<remap from="tracker/swarm_state" to="swarm_manager/swarm_state" />
<remap from="tracker/swarm_behavior" to="swarm_manager/swarm_behavior" />
<remap from="tracker/subswarm_id" to="swarm_manager/subswarm_id" />
</node>
<node name="task_runner" pkg="ap_tasks" type="task_runner.py" output="screen" args="" />
<node name="recv_pose_rate" pkg="ap_logging" type="recv_pose_rate.py" output="screen" args="" />
<node name="swarm_control" pkg="ap_mission_planning" type="swarm_control.py" output="screen" args="" >
<remap from="swarm_control/swarm_uav_states" to="swarm_tracker/swarm_uav_states" />
<remap from="swarm_control/wp_getlast" to="autopilot/wp_getlast" />
<remap from="swarm_control/wp_getrange" to="autopilot/wp_getrange" />
<remap from="swarm_control/status" to="autopilot/status" />
<remap from="swarm_control/waypoint_goto" to="autopilot/waypoint_goto" />
<remap from="swarm_control/mode_num" to="autopilot/mode_num" />
<remap from="swarm_control/recv_swarm_data" to="network/recv_swarm_data" />
<remap from="swarm_control/net_reset" to="network/net_reset" />
<node name="swarm_manager" pkg="ap_mission_planning" type="swarm_manager_node.py" output="screen" args="" >
<remap from="swarm_manager/swarm_uav_states" to="tracker/swarm_uav_states" />
<remap from="swarm_manager/red_uav_states" to="tracker/red_uav_states" />
<remap from="swarm_manager/hit_uavs" to="tracker/hit_uavs" />
<remap from="swarm_manager/attack_uavs" to="tracker/attack_uavs" />
<remap from="swarm_manager/payload_waypoint" to="safety/payload_waypoint" />
<remap from="swarm_manager/payload_speed_waypoint" to="safety/payload_speed_waypoint" />
<remap from="swarm_manager/payload_rate" to="safety/payload_rate" />
<remap from="swarm_manager/payload_attitude" to="safety/payload_attitude" />
<remap from="swarm_manager/waypoint_goto" to="autopilot/waypoint_goto" />
<remap from="swarm_manager/status" to="autopilot/status" />
<remap from="swarm_manager/mode_num" to="autopilot/mode_num" />
<remap from="swarm_manager/wp_getlast" to="autopilot/wp_getlast" />
<remap from="swarm_manager/wp_getrange" to="autopilot/wp_getrange" />
<remap from="swarm_manager/stores_release" to="autopilot/stores_release" />
<remap from="swarm_manager/recv_swarm_data" to="network/recv_swarm_data" />
<remap from="swarm_manager/send_swarm_behavior_data" to="network/send_swarm_behavior_data" />
<remap from="swarm_manager/send_swarm_behavior_data_to" to="network/send_swarm_behavior_data_to" />
<remap from="swarm_manager/net_reset" to="network/net_reset" />
</node>
<!-- Controller Nodes -->
<node name="independent_transit" pkg="ap_path_planning" type="independent_transit.py" output="screen" args=""/>
<node name="patrol_box" pkg="ap_path_planning" type="patrol_box.py" output="screen" args="">
<remap from="patrol_box/acs_pose" to="autopilot/acs_pose" />
</node>
<node name="simple_ground_attack" pkg="ap_path_planning" type="simple_ground_attack.py" output="screen" args=""/>
<node name="tactic_interface" pkg="ap_path_planning" type="tactic_interface.py" output="screen" args="">
<remap from="tactic_interface/send_swarm_behavior_data" to="network/send_swarm_behavior_data" />
<remap from="tactic_interface/net_reset" to="network/net_reset" />
<remap from="tactic_interface/swarm_sub_behavior" to="network/swarm_sub_behavior" />
<remap from="tactic_interface/acs_pose" to="autopilot/acs_pose" />
<remap from="tactic_interface/wp_getlast" to="autopilot/wp_getlast" />
<remap from="tactic_interface/hit_uavs" to="hit_tracker/hit_uavs" />
<remap from="tactic_interface/waypoint_goto" to="autopilot/waypoint_goto" />
<remap from="tactic_interface/firing_reports" to="air_to_air/firing_reports" />
</node>
<!-- TODO(tfoote) how to replace this gracefully? -->
<!--<include file="$(find autonomy_itar)/launch/itar.launch" />-->
</launch>