.gitignore

@@ -3,3 +3,4 @@
 lib/acs.egg-info/
 lib/build/
 lib/dist/
+repos
\ No newline at end of file

README.md

-# 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:

ap_lib/src/ap_lib/acs_messages.py

@@ -200,7 +200,7 @@ class Example(Message):
 class FlightStatus(Message):
     # Define message type parameters
     msg_type = 0x00
-    msg_fmt = '>HBBHhhHB3x16s'
+    msg_fmt = '>HBBHhhHBBBx16s'
 
     def __init__(self):
         Message.__init__(self)
@@ -230,7 +230,9 @@ class FlightStatus(Message):
         self.alt_rel = None	    # AGL (int, millimeters)
         self.mis_cur = None	    # Current mission (waypoint) index (0-65535)
         self.swarm_behavior = 0 # Swarm behavior (follow, standby, etc.)
-        # Next 3 bytes are unused
+        self.swarm_sub_behavior = 0 # Tactic ID for tactic_interface
+        self.vehicle_type = 0 #Vehicle type (AC_COPTER, AC_FIXED_WING)
+        # Next byte is unused
         self.name = None	# Friendly name of aircraft (16 chars max)
 
     def _pack(self):
@@ -266,6 +268,8 @@ class FlightStatus(Message):
                 int(self.alt_rel / 1e02),
                 int(self.mis_cur),
                 int(self.swarm_behavior),
+                int(self.swarm_sub_behavior),
+                int(self.vehicle_type),
                 _enc_str(self.name))
 
         # Pack into a byte string
@@ -303,6 +307,8 @@ class FlightStatus(Message):
         self.alt_rel = fields.pop(0) * 1e02
         self.mis_cur = fields.pop(0)
         self.swarm_behavior = fields.pop(0)
+        self.swarm_sub_behavior = fields.pop(0)
+        self.vehicle_type = fields.pop(0)
         self.name = _dec_str(fields.pop(0))
 
 class AutopilotReboot(Message):
@@ -573,6 +579,47 @@ class PrevMsgAP(Message):
         self.seq = fields[1]
         self.msg = _dec_str(fields[2])
 
+"""
+Camera Control Message class.
+
+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
+
+"""
+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)
+
+        pass
+
+    def _pack(self):
+        return ''
+
+    def _unpack(self, data):
+        pass
 
 #---------------------------------------------------
 # Ground/Air communication and connectivity messages
@@ -711,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
@@ -758,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):
@@ -774,7 +864,7 @@ class Land(Message):
         pass
 
 class LandAbort(Message):
-    msg_type = 0x63
+    msg_type = 0x64
     msg_fmt = '>h2x'
 
     def __init__(self):
@@ -791,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
@@ -861,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
@@ -884,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)
@@ -902,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
@@ -1148,6 +1290,11 @@ class GameMessage(Message):
     msg_type = 0xF0
     msg_fmt = '>HH'
 
+    START = 1
+    END = 2
+    RESET = 3
+    TIME_REMAINING = 4
+
     def __init__(self):
         Message.__init__(self)
 
@@ -1159,7 +1306,7 @@ class GameMessage(Message):
 
         #Used to complement message type
         #               For GAME START, value = game duration
-        #               For TIME REMAINING, value = minutes remaining
+        #               For TIME REMAINING, value = seconds remaining
         self.value = 0
 
     def _pack(self):
@@ -1191,24 +1338,64 @@ 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 ScoreSummary(Message):
+class AttackReport(Message):
     msg_type = 0xF3
-    msg_fmt = '>ffBBBBBBBBBB2x'
+    msg_fmt = '>B3x'
+
+    def __init__(self):
+        Message.__init__(self)
+
+        self.attacker_id = 0
+
+    def _pack(self):
+        tupl = (int(self.attacker_id), )
+        return struct.pack(type(self).msg_fmt, *tupl)
+
+    def _unpack(self, data):
+        fields = struct.unpack_from(type(self).msg_fmt, data, 0)
+        self.attacker_id = fields[0]
+
+class WinchesterReport(Message):
+    ''' Used by the Arbiter to tell a UAV it's out of ammo '''
+    msg_type = 0xF4
+    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)
@@ -1222,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)
@@ -1244,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],)
@@ -1277,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],)
 
@@ -1292,8 +1489,16 @@ 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) + (padding * 'x')
+              (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)
 
@@ -1301,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_on_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_on_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)
@@ -1331,9 +1537,14 @@ 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_on_offense += [next]
+            self.blues_attack += [next]
             offset += type(self).msg_fmt_uav_sz
 
         for uav in range(0, self.num_red_in_area):
@@ -1356,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_on_offense += [next]
+            self.reds_attack += [next]
             offset += type(self).msg_fmt_uav_sz
 

ap_lib/src/ap_lib/acs_socket.py

@@ -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)
 
 

ap_lib/src/ap_lib/attack_behavior.py

+#!/usr/bin/env python
+
+#-------------------------------------------------------------------------
+# AttackBehavior
+# Duane Davis, 2017
+#
+# Defines an abstract class for wrapping ACS behaviors that conduct
+# attacks by landing at the adversary's ground station.  The class inherits
+# from HybridBehavior, so it can use either waypoint control or rate-based
+# control to set up for the attack run.  The conductAttack method will
+# shift the UAV to the landing waypoint associated with the attack and will
+# inhibit further rate or waypoint commands.
+#--------------------------------------------------------------------------
+
+# ROS imports
+import rospy
+from rospy import rostime
+import std_msgs.msg as stdmsg
+
+# ACS imports
+from ap_lib import nodeable
+from ap_lib import hybrid_behavior
+import ap_lib.ap_enumerations as enums
+
+
+class AttackBehavior(hybrid_behavior.HybridBehavior):
+    ''' Abstract class for wrapping ACS behaviors that conduct attacks
+    by landing at the adversary's ground station.  The class inherits
+    from HybridBehavior, so it can use either waypoint control or rate-
+    based control to set up for the attack run.  The conductAttack
+    method will shift the UAV to the landing waypoint associated with
+     the attack and will inhibit further rate or waypoint commands.
+    NOTE: Currently, only the turn-rate portion of the rate-based control
+          behavior will have any effect (speed is fixed, and pitch rate 
+          will have no effect)
+
+    Class member variables:
+      attack_started: gets set to True when the attack has commenced
+      _ldg_wp_publisher: publisher object to send the UAV to the attack wpt
+
+    Inherited from HybridBehavior
+      _rtPublisher: publisher object for publishing waypoint commands
+      rt_msg: YPS object containing waypoint order
+      _wpPublisher: publisher object for publishing waypoint commands
+      wp_msg: LLA object containing waypoint order
+      _last_wp_id: Index (ID) of the last (infinite loiter) waypoint
+
+    Inherited from Behavior
+      behaviorID: identifier (int) for this particular behavior
+      _subswarm_id: ID of the subswarm to which this UAV is assigned
+      _swarm: container for state info for all swarm UAVs
+      _swarm_keys: key values (IDs) of all swarm UAVs
+      _subswarm_keys: key values (IDs) of all subswarm UAVs
+      _ap_intent: most recently ordered autopilot waypoint
+      _lock: reentrant lock to enforce thread-safe swarm dictionary access
+      is_ready: set to True when the behavior has been initialized
+      is_active: set to True when the behavior is running 
+      is_paused: set to True when an active behavior is paused
+      _uses_wp_control: set to True if the behavior drives by waypoint
+      _statusPublisher: publisher object for behavior status
+      _statusStamp: timestamp of the last status message publication
+      _sequence: sequence number of the next status message
+
+    Inherited from Nodeable:
+      nodeName:  Name of the node to start or node in which the object is
+      timer: ROS rate object that controls the timing loop
+      DBUG_PRINT: set true to force screen debug messages (default FALSE)
+      INFO_PRINT: set to true to force screen info messages (default FALSE)
+      WARN_PRINT: set false to force screen warning messages (default FALSE)
+
+    Class member functions
+      publishWaypoint: "safely" publishes a waypoint position
+    '''
+
+    def __init__(self, nodename, ctrlrID):
+        ''' Class initializer sets up the publisher for the rate topic
+        @param nodename: name of the node that the object is contained in
+        @param ctrlrID: identifier (int) for this particular behavior
+        '''
+        hybrid_behavior.HybridBehavior.__init__(self, nodename, ctrlrID)
+        self.attack_started = False
+
+
+    def runAsNode(self, rate=10.0):
+        ''' Method that is called to run this object as an independent node.
+        Registers as a ROS node, calls the setup functions, and runs a
+        timing loop for the object's required processing.  This method
+        overrides (then calls) the parent-class method to provide for the
+        creation and registration of subscriptions, publishers, services,
+        and callbacks that are common to all behaviors but should only be
+        instantiated if the behavior is running as an independent node.
+        @param rate: rate (hz) of the node's timing loop
+        '''
+        self._ldg_wp_publisher = \
+            rospy.Publisher("autopilot/waypoint_goto", stdmsg.UInt16, \
+                            tcp_nodelay=True, latch=False, queue_size=1)
+        super(AttackBehavior, self).runAsNode(rate)
+
+
+    #------------------------------------
+    # Overrides of HybridBehavior methods
+    #------------------------------------
+
+    def publishWaypoint(self, wp):
+        ''' Publishes a computed waypoint to the autopilot topic.
+        This method enforces an altitude check to make sure the published 
+        waypoint is not below the minimum or above the maximum safe altitude.
+        Additional safety checks can be implemented here as required.
+        @param wp: computed waypoint (LLA) (must use rel_alt!)
+        '''
+        if not self.attack_started:
+            super(AttackBehavior, self).publishWaypoint(wp)
+
+
+    def publishRate(self, rt):
+        ''' Publishes a computed rate to the autopilot topic.
+        This method enforces an altitude check to make sure the published 
+        waypoint is not below the minimum or above the maximum safe altitude.
+        Additional safety checks can be implemented here as required.
+        @param wp: computed waypoint (LLA) (must use rel_alt!)
+        '''
+        if not self.attack_started:
+            super(AttackBehavior, self).publishRate(rt)
+
+    #-----------------------
+    # Class-specific methods
+    #-----------------------
+
+    def conductAttack(self):
+        msg = stdmsg.UInt16()
+        msg.data = enums.ATTACK_WP
+        self._ldg_wp_publisher.publish(msg)
+        self.attack_started = True

ap_lib/src/ap_lib/behavior.py

@@ -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()
 

ap_lib/src/ap_lib/behavior_parser.py

+#!/usr/bin/python3
+# -*- coding: utf-8 -*-
+
+import sys
+import os
+import imp
+import collections
+import xml.etree.ElementTree as ET
+import struct
+#from future.utils import iteritems
+
+class BehaviorParser():
+
+    def __init__(self):
+        self.behaviors = collections.OrderedDict()
+        self.fmt_lookup = {'char':'c', 'signed char': 'b', 'unsigned char': 'B', 'int':'i', 'bool':'?', 'float':'f'}
+        self.init()
+
+    def __repr__(self):
+        return "BehaviorParser()"
+
+    def __str__(self):
+        return str(self.behaviors)
+
+    def init(self):
+        # Get tactic_interface behaviors
+        tactic_int_file = os.environ['SCRIMMAGE_TACTIC_INTERFACE_FILE']
+        if tactic_int_file == None:
+            print('===============================================')
+            print('ERROR: Tactic Interface File Doesn\'t Exist.')
+            print('ERROR: Missing env variable: SCRIMMAGE_TACTIC_INTERFACE_FILE')
+
+        if not os.path.isfile(tactic_int_file):
+            print('===============================================')
+            print('ERROR: Tactic Interface File Doesn\'t Exist.')
+            print('File doesn\'t exist: %s' % tactic_int_file)
+            
+        tree = ET.parse(tactic_int_file)        
+        root = tree.getroot()
+
+        # Loop over all "behavior" tags and save all attributes of each tag
+        tactic_id = 0
+        for behavior in root:
+            self.behaviors[tactic_id] = behavior.attrib
+            self.behaviors[tactic_id]['params'] = [] #empty list for params
+
+            # Loop over all "param" tags under each "behavior" tag and save in
+            # dict
+            for param in behavior:                
+                self.behaviors[tactic_id]['params'].append(param.attrib)
+
+            tactic_id += 1
+
+    def behavior_names(self):
+        names_list = []
+        for key, value in self.behaviors.items():
+            names_list.append(value['name'])
+        return names_list
+
+    def params(self, id):
+        return self.behaviors[id]['params']
+
+    def pack(self, id, params):        
+        # prepend the params with the tactic_id
+        fmt_str = ">B"        
+        value_list = [id]
+        for param in self.behaviors[id]['params']:
+            value_list.append(params[param['name']])
+            fmt_str += self.fmt_lookup[param['type']]            
+        return struct.pack(fmt_str, *value_list)
+
+    def unpack(self, data):    
+        # Unpack the first unsigned char to get the tactic_id
+        id = struct.unpack("B", data[:1])[0]        
+        
+        fmt_str = ">B"        
+        for param in self.behaviors[id]['params']:
+            fmt_str += self.fmt_lookup[param['type']]
+        
+        value_list = struct.unpack_from(fmt_str, data)        
+
+        # First element in value list is the tactic_id
+        params_dict = {'tactic_id': value_list[0]}
+        index = 1
+        for param in self.behaviors[id]['params']:
+            params_dict[param['name']] = value_list[index]
+            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)
+    if not bp.test_load_behaviors():
+        sys.exit(-1)
+    else:
+        sys.exit(0)

ap_lib/src/ap_lib/distributed_algorithms.py

@@ -17,7 +17,7 @@ import rospy
 # ACS imports
 import ap_msgs.msg as apmsg
 import ap_lib.bitmapped_bytes as bytes
-
+import socket
 
 class LossyDataExchange(object):
     ''' Abstract class used in support of reliable information exchange
@@ -48,17 +48,22 @@ class LossyDataExchange(object):
 
     MIN_COMMS_DELAY = rospy.Duration(0.25) # Throttle comms to limit congestion
 
-    def __init__(self, subswarm_keys, crashed_list, lock, own_id=1):
+    def __init__(self, subswarm_keys, crashed_list, lock, own_id=None):
         ''' Initializer for the class sets up class variables
         @param subswarm_keys: Set object containing active subswarm IDs
         @param crashed_list: Set object containing IDs of possible crashed UAVs
         @param msg_publisher: ROS publisher to the swarm_data_msg topic
         @param lock: reentrant lock that will be used to enforce thread safety
         '''
-        if rospy.has_param("aircraft_id"):
-            self._own_id = int(rospy.get_param("aircraft_id"))
-        else:
-            self._own_id = own_id
+        try:
+            if rospy.has_param("aircraft_id"):
+                self._own_id = int(rospy.get_param("aircraft_id"))
+        except socket.error:
+            if own_id is not None: 
+                self._own_id = own_id
+            else: 
+                rospy.logerr("cannot retrieve aircraft_id from parameter server")
+
         self._short_list_parser = bytes.UShortListParser()
         self._short_list_parser.source_id = self._own_id
         self._subswarm = subswarm_keys
@@ -68,8 +73,8 @@ class LossyDataExchange(object):
         self._data = dict()
         self._to_send = set()
         self._behavior_msg = apmsg.BehaviorParameters()
-        self._info_bcast_time = rospy.Time(0.0)
-        self._rqst_bcast_time = rospy.Time(0.0)
+        self._info_bcast_time = 0.0
+        self._rqst_bcast_time = 0.0
         self.rounds = 0
         self.xmit_msgs = 0
         self.xmit_bytes = 0
@@ -100,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
@@ -190,13 +197,14 @@ class EagerConsensusSort(LossyDataExchange):
                         self._to_send.add(self._data[uav])
 
 
-    def send_requested(self):
+    def send_requested(self, t=None):
         ''' Sends any available data that has been requested by other UAVs
         This method should be called once per loop even after decided
         so that UAVs that have not decided yet will still get the info
         '''
-        t = rospy.Time.now()
-        if (t - self._info_bcast_time) < LossyDataExchange.MIN_COMMS_DELAY:
+        if t is None:
+            t = rospy.Time.now().to_sec()
+        if (t - self._info_bcast_time) < LossyDataExchange.MIN_COMMS_DELAY.to_sec():
             return
         with self._lock:
             self._value_pair_parser.pairs = list(self._to_send)
@@ -210,7 +218,7 @@ class EagerConsensusSort(LossyDataExchange):
                 self._info_bcast_time = t
 
 
-    def decide_sort(self):
+    def decide_sort(self, t=None):
         ''' Sorts the list if all data is available or requests missing data
         @return the sorted id-value list or None if more data is still needed
         '''
@@ -222,9 +230,10 @@ class EagerConsensusSort(LossyDataExchange):
                     missing_ids.append(uav)
 
             if len(missing_ids) > 0:
-                t = rospy.Time.now()
+                if t is None:
+                    t = rospy.Time.now().to_sec()
                 if (t - self._rqst_bcast_time) >= \
-                   LossyDataExchange.MIN_COMMS_DELAY:
+                   LossyDataExchange.MIN_COMMS_DELAY.to_sec():
                     self.rounds += 1
                     self._short_list_parser.number_list = missing_ids
                     self._behavior_msg.id = bytes.USHORT_LIST

ap_lib/src/ap_lib/gps_utils.py

@@ -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)
 
@@ -108,65 +112,32 @@ def normalize_angle(angle, radians = True):
 #FOV_width and FOV_height specified in degrees.
 def hitable(lat, lon, alt, pose_quat, max_range, FOV_width, FOV_height,
             targ_lat, targ_lon, targ_alt):
-    # Compute lateral distance to leader (using lat/lon tools)
-    distance = gps_distance( lat, lon, targ_lat, targ_lon )
 
-    if (distance > max_range or distance <= 0):
-        return False
-
-    # Compute altitude difference to target
-    #  - negative sign to indicate FROM me TO target
-    alt_diff = -(alt - targ_alt)
-    #with this implemenation: alt_diff / distance must be within the domain
-    #of the arcsin function [-1, 1]:
-    if (math.fabs(alt_diff) > distance):
-        return False
-
-    # Compute planar bearing from target
-    # - measured in bearing angle (NED), radians
-    abs_bearing = gps_bearing(lat, lon, targ_lat, targ_lon )
+    # Compute absolute target distance, bearing, and elevation
+    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(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)
-    yaw = rpy[2]
-
-    #transform yaw to desired coord space
-    if yaw < math.radians(180.0):
-        yaw += math.radians(360.0)
-
-    # Transform to account for current heading
-    bearing = abs_bearing - yaw
-    if bearing < 0.0:
-        bearing += math.radians(360.0)
-
-    # Check if meets FOV conditions
-    #  -- within azimuthal scope (+/-FOV_half_width degrees)
-    #  -- within elevation scope (+/-FOV_half_width degrees)
-    FOV_half_width = math.radians(FOV_width / 2.0)
-    if math.fabs(bearing) > FOV_half_width:
-        return False
-
-    # Compute and compare elevation
-    FOV_half_height = math.radians(FOV_height / 2.0)
-    phi = math.asin(alt_diff / distance)
-    pitch = rpy[1]
-    
-    # Account for my pitch
-    if alt_diff > 0.0:
-        phi -= pitch
-    else:
-        phi += pitch
-
-    if math.fabs(phi) > FOV_half_height:
-        return False
-
-    #all checks passed, target is 'shootable'
-    return True
+    rel_bearing = normalize_pi(bearing - rpy[2])
+    rel_elevation = normalize_pi(elevation - rpy[1])
+
+    # Determine if relative parameters are within weapon envelope
+    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
 
 
 # A couple of general-purpose mathematical utility functions
 
 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 )
     '''
@@ -177,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 )
     '''
@@ -187,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
     '''
@@ -196,32 +169,120 @@ 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)
+    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)
+    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)
+    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)
+    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
     '''
@@ -249,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]), \
@@ -267,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):
@@ -277,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
@@ -284,7 +352,17 @@ class GeoBox():
         @param alt: altitude (meters MSL) being tested
         @return True if the point is within the GeoBox's boundaries
         '''
-        if (self._floor > alt) or (self._ceiling < alt): return False
+        return self.altitude_contains(alt) and \
+               self.geography_contains(lat, lon)
+
+
+    def geography_contains(self, lat, lon):
+        ''' Returns True if the GeoBox contains the lat/lon point
+        (only considers geography, so GeoBox floor and ceiling don't matter)
+        @param lat: latitude being tested
+        @param lon: longitude being tested
+        @return True if the point is within the GeoBox's boundaries
+        '''
         xy = cartesian_offset(self._corners[0][0], self._corners[0][1], lat, lon)
         ba = self._cart_corners[1]
         da = self._cart_corners[3]
@@ -296,6 +374,15 @@ class GeoBox():
         return True
 
 
+    def altitude_contains(self, alt):
+        ''' Returns True if the altitude is within the minimum and maximum
+        altitudes of the GeoBox (the georaphic boundaries are not considered)
+        @param alt: altitude (meters MSL) being tested
+        @return True if the altitude is between the GeoBox min & max altitudes
+        '''
+        return (self._floor <= alt) and (self._ceiling >= alt)
+
+
     def distance(self, lat, lon):
         ''' Returns the distance of the point from the GeoBox object
         A value of 0.0 is returned if the lat/lon is contained within the box
@@ -303,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]))
@@ -336,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
         '''

ap_lib/src/ap_lib/hybrid_behavior.py

+#!/usr/bin/env python
+
+#-------------------------------------------------------------------------
+# HybridBehavior
+# Duane Davis, 2016
+#
+# Defines an abstract class for wrapping ACS behaviors that work by
+# publishing rate commands or waypoints to ROS topics that can be
+# incorporated into other classes or run as independent nodes
+#-------------------------------------------------------------------------
+
+# ROS imports
+import rospy
+from rospy import rostime
+
+# ACS imports
+from ap_lib import nodeable
+from ap_lib import behavior
+from autopilot_bridge.msg import YPS
+from autopilot_bridge.msg import LLA
+import ap_lib.ap_enumerations as enums
+
+
+class HybridBehavior(behavior.Behavior):
+    ''' Abstract class for wrapping a control-order-issuing ACS ROS object
+    Control is implemented through the generation of rate commands (yaw rate,
+    pitch rate, forward speed) or waypoints.  Instantiated objects will be
+    provided a rate publisher that publishes computed rates to the appropriate
+    and a waypoint publisher that publishes computed waypoints to the
+    appropriate topic.
+    NOTE: Currently, only the turn-rate portion of the rate-based control
+          behavior will have any effect (speed is fixed, and pitch rate 
+          will have no effect)
+
+    Class member variables:
+      _rtPublisher: publisher object for publishing waypoint commands
+      rt_msg: YPS object containing waypoint order
+      _wpPublisher: publisher object for publishing waypoint commands
+      wp_msg: LLA object containing waypoint order
+      _last_wp_id: Index (ID) of the last (infinite loiter) waypoint
+
+    Inherited from Behavior
+      behaviorID: identifier (int) for this particular behavior
+      _subswarm_id: ID of the subswarm to which this UAV is assigned
+      _swarm: container for state info for all swarm UAVs
+      _swarm_keys: key values (IDs) of all swarm UAVs
+      _subswarm_keys: key values (IDs) of all subswarm UAVs
+      _ap_intent: most recently ordered autopilot waypoint
+      _lock: reentrant lock to enforce thread-safe swarm dictionary access
+      is_ready: set to True when the behavior has been initialized
+      is_active: set to True when the behavior is running 
+      is_paused: set to True when an active behavior is paused
+      _uses_wp_control: set to True if the behavior drives by waypoint
+      _statusPublisher: publisher object for behavior status
+      _statusStamp: timestamp of the last status message publication
+      _sequence: sequence number of the next status message
+
+    Inherited from Nodeable:
+      nodeName:  Name of the node to start or node in which the object is
+      timer: ROS rate object that controls the timing loop
+      DBUG_PRINT: set true to force screen debug messages (default FALSE)
+      INFO_PRINT: set to true to force screen info messages (default FALSE)
+      WARN_PRINT: set false to force screen warning messages (default FALSE)
+
+    Class member functions
+      publishWaypoint: "safely" publishes a waypoint position
+    '''
+
+    def __init__(self, nodename, ctrlrID):
+        ''' Class initializer sets up the publisher for the rate topic
+        @param nodename: name of the node that the object is contained in
+        @param ctrlrID: identifier (int) for this particular behavior
+        '''
+        behavior.Behavior.__init__(self, nodename, ctrlrID)
+        self._uses_wp_control = True
+        self._last_wp_id = 0
+        self.rt_msg = YPS()
+        self._rtPublisher = None
+        self.wp_msg = LLA()
+        self._wpPublisher = None
+
+
+    def runAsNode(self, rate=10.0):
+        ''' Method that is called to run this object as an independent node.
+        Registers as a ROS node, calls the setup functions, and runs a
+        timing loop for the object's required processing.  This method
+        overrides (then calls) the parent-class method to provide for the
+        creation and registration of subscriptions, publishers, services,
+        and callbacks that are common to all behaviors but should only be
+        instantiated if the behavior is running as an independent node.
+        @param rate: rate (hz) of the node's timing loop
+        '''
+        self._rtPublisher = \
+            rospy.Publisher("safety/payload_rate", YPS, \
+                            tcp_nodelay=True, latch=True, queue_size=1)
+        self._wpPublisher = \
+            rospy.Publisher("safety/payload_waypoint", LLA, \
+                            tcp_nodelay=True, latch=True, queue_size=1)
+        super(HybridBehavior, self).runAsNode(rate)
+
+
+    #---------------------------------------------------------
+    # Class-specific methods implementing class functionality
+    #---------------------------------------------------------
+
+    def publishWaypoint(self, wp):
+        ''' Publishes a computed waypoint to the autopilot topic.
+        This method enforces an altitude check to make sure the published 
+        waypoint is not below the minimum or above the maximum safe altitude.
+        Additional safety checks can be implemented here as required.
+        @param wp: computed waypoint (LLA) (must use rel_alt!)
+        '''
+        #verify valid LLA order
+        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)
+        else:
+            self.set_active(False)
+            self.log_warn("Altitude control mode invalid or invalid altitude order")
+
+
+    def publishRate(self, rt):
+        ''' Publishes a computed rate to the autopilot topic.
+        This method enforces an altitude check to make sure the published 
+        waypoint is not below the minimum or above the maximum safe altitude.
+        Additional safety checks can be implemented here as required.
+        @param wp: computed waypoint (LLA) (must use rel_alt!)
+        '''
+        #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_FW_FWD_SPEED and \
+           rt.speed <= enums.MAX_FW_FWD_SPEED:
+            self._rtPublisher.publish(rt)
+        else:
+            self.set_active(False)
+            self.log_warn("Rate control mode invalid or invalid rate order")

ap_lib/src/ap_lib/parameter_helper.py

@@ -14,6 +14,30 @@ from ap_lib import bitmapped_bytes
 CONSTRAINT_PARAMETERS = 23     # Data field represents a Constrained Shooter parameter message
 WINGMAN_PARAMETERS = 24        # Data field represents a Wingman Parameter message
 
+class TacticInterfaceParser(bitmapped_bytes.BitmappedBytes):
+    ''' Parser for constrained shooter parameter data
+    '''
+    fmt = ">ff"
+
+    def __init__(self):
+        ''' Initializes parameters with default values
+        '''
+        self.tactic_id = 0
+        self.extra = 0
+
+
+    def pack(self):
+        ''' Serializes parameter values into a bitmapped byte array
+        @return bitmapped bytes as a string
+        '''
+        return struct.pack(type(self).fmt, self.tactic_id, self.extra)
+
+
+    def unpack(self, bytes):
+        ''' Sets parameter values from a bitmapped byte array
+        @param bytes: bitmapped byte array
+        '''
+        self.tactic_id, self.extra = struct.unpack_from(type(self).fmt, bytes, 0)
 
 class ConstrainedShooterParser(bitmapped_bytes.BitmappedBytes):
     ''' Parser for constrained shooter parameter data

ap_lib/src/ap_lib/plugin_behavior.py

+#!/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
+

ap_lib/src/ap_lib/quaternion_math.py

@@ -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
@@ -77,3 +81,18 @@ def quat_to_euler(q):
     yaw = math.atan2(2.0*(q1*q4 + q2*q3), 1 - 2.0*(q3*q3 + q4*q4))
 
     return [ roll, pitch, yaw ]
+
+# Generates a quaternion from a set of Euler angles
+# @param roll (radians)
+# @param pitch (radians)
+# @param yaw (radians)
+# @return a quaternion (list) in the form (x, y, z, w)
+def euler_to_quat(roll, pitch, yaw):
+    cr, sr = ( math.cos(roll/2.0), math.sin(roll/2.0) )
+    cp, sp = ( math.cos(pitch/2.0), math.sin(pitch/2.0) )
+    cy, sy = ( math.cos(yaw/2.0), math.sin(yaw/2.0) )
+    x = sr * cp * cy - cr * sp * sy
+    y = cr * sp * cy + sr * cp * sy
+    z = cr * cp * sy - sr * sp * cy
+    w = cr * cp * cy + sr * sp * sy
+    return [ x, y, z, w ]

ap_lib/src/ap_lib/rate_behavior.py

@@ -23,12 +23,16 @@ import ap_lib.ap_enumerations as enums
 class RateBehavior(behavior.Behavior):
     ''' Abstract class for wrapping a control-order-issuing ACS ROS object
     Control is implemented through the generation of rate commands.
-    Instantiated objects will provide a rate publisher that publishes
+    Instantiated objects will be provided a rate publisher that publishes
     computed rates to the appropriate topic.
+    NOTE: Currently, only the turn-rate portion of the rate-based control
+          behavior will have any effect (speed is fixed, and pitch rate 
+          will have no effect)
 
     Class member variables:
       _rtPublisher: publisher object for publishing rate commands
       rt_msg: YPS object containing Yaw Rate, Pitch Rate, Speed order
+      _last_wp_id: Index (ID) of the last (infinite loiter) waypoint
 
     Inherited from Behavior
       behaviorID: identifier (int) for this particular behavior
@@ -63,7 +67,7 @@ class RateBehavior(behavior.Behavior):
         @param ctrlrID: identifier (int) for this particular behavior
         '''
         behavior.Behavior.__init__(self, nodename, ctrlrID)
-        self._uses_wp_control = False
+        self._uses_wp_control = True   # Note--might need to change this later
         self.rt_msg = YPS()
         self._rtPublisher = None
 
@@ -89,8 +93,16 @@ class RateBehavior(behavior.Behavior):
 
     def publishRate(self, rt):
         ''' Publishes a rate command to the safety topic.
-        WARNING: NO Safety checks are performed here!  Safety Node 
-	MUST validate commands before forwarding to autopilot.
+        WARNING: Only max/min rate/speed safety checks are performed here!
+        Safety Node MUST validate commands before forwarding to autopilot.
         @param rt: computed rates Yaw Rate, Pitch Rate, airSpeed
         '''
-	self._rtPublisher.publish(rt)
+        #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_FW_FWD_SPEED and \
+           rt.speed <= enums.MAX_FW_FWD_SPEED:
+            self._rtPublisher.publish(rt)
+        else:
+            self.set_active(False)
+            self.log_warn("Rate control mode invalid or invalid rate order")

ap_lib/src/ap_lib/ros_to_ros_bridge.py

+#!/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
+

ap_lib/src/ap_lib/sasc_scrimmage.py

+import copy
+import enum
+
+class Publisher(object):
+    def __init__(self, topic):
+        self.msg_list = []
+        self.topic = topic
+
+    def publish(self, msg):
+        self.msg_list.append(copy.deepcopy(msg))
+
+class Subscriber(object):
+    def __init__(self, topic, callback):
+        self.msg_list = []
+        self.topic = topic
+        self.callback = callback
+
+class LogLevels(enum.IntEnum):
+    fatal = 1
+    error = 2
+    info = 3
+    warn = 4
+    dbug = 5
+
+class Tactic(object):
+
+    def __init__(self):
+        self.log_level = LogLevels.info
+
+    def createPublisher(self, topicName, msgType, queueSize=1, latched=True):
+        try:
+            return self.__parent.createPublisher(topicName, msgType, queueSize=1, latched=True)
+        except AttributeError:
+            return Publisher(topicName)
+
+    def createSubscriber(self, topicName, msgType, msgHandler):
+        try:
+            return self.__parent.createSubscriber(topicName, msgType, msgHandler)
+        except AttributeError:
+            return Subscriber(topicName, msgHandler)
+
+    def log_info(self, msg):
+        try:
+            self.__parent.log_info(msg)
+        except AttributeError:
+            print msg
+            if self.log_level >= LogLevels.info:
+                print msg
+
+    def log_warn(self, msg):
+        try:
+            self.__parent.log_info(msg)
+        except AttributeError:
+            if self.log_level >= LogLevels.warn:
+                print msg
+
+    def log_dbug(self, msg):
+        try:
+            self.__parent.log_dbug(msg)
+        except AttributeError:
+            if self.log_level >= LogLevels.dbug:
+                print msg
+
+    def _process_swarm_data_msg(self, msg):
+        pass

ap_lib/src/ap_lib/sasc_state.py

+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])