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 = '>HBBHhhHBB2x16s'
 
     def __init__(self):
         Message.__init__(self)
@@ -230,7 +230,8 @@ 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
+        # Next 2 bytes are unused
         self.name = None	# Friendly name of aircraft (16 chars max)
 
     def _pack(self):
@@ -266,6 +267,7 @@ class FlightStatus(Message):
                 int(self.alt_rel / 1e02),
                 int(self.mis_cur),
                 int(self.swarm_behavior),
+                int(self.swarm_sub_behavior),
                 _enc_str(self.name))
 
         # Pack into a byte string
@@ -303,6 +305,7 @@ 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.name = _dec_str(fields.pop(0))
 
 class AutopilotReboot(Message):

ap_lib/src/ap_lib/behavior_parser.py

+#!/usr/bin/python3
+# -*- coding: utf-8 -*-
+
+import sys
+import os
+import collections
+import xml.etree.ElementTree as ET
+
+class BehaviorParser():
+
+    def __init__(self):
+        self.behaviors = collections.OrderedDict()
+        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('Tactic Interface File Doesn\'t Exist.')
+            print('Missing env variable: SCRIMMAGE_TACTIC_INTERFACE_FILE')
+            return
+        
+        tree = ET.parse(tactic_int_file)
+        root = tree.getroot()
+
+        tactic_id = 0
+        for child in root:
+            self.behaviors[tactic_id] = {'name' : child.attrib['name'], 
+                                         'module' : child.attrib['module'],
+                                         'class' : child.attrib['class'],
+                                         'id' : tactic_id}
+            tactic_id += 1
+
+        tree = ET.parse(tactic_int_file)
+        root = tree.getroot()
+
+    def behavior_names(self):        
+        names_list = []
+        for key, value in self.behaviors.items():
+            names_list.append(value['name'])
+        return names_list
+
+if __name__ == '__main__':
+    bp = BehaviorParser()    
+    print(bp)
+    sys.exit()
+

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/sasc_scrimmage.py

+
+class Publisher(object):
+    def __init__(self, topic):
+        self.msg_list = []
+        self.topic = topic
+
+    def publish(self, msg):
+        self.msg_list.append(msg)
+
+class Subscriber(object):
+    def __init__(self, topic, callback):
+        self.msg_list = []
+        self.topic = topic
+        self.callback = callback
+
+class Tactic(object):
+
+    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)

ap_master/launch/master.launch

@@ -14,8 +14,6 @@
   <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="tactic_module_default" default="increasing_altitude" /> <!-- Tactic module -->
-  <arg name="tactic_name_default" default="IncreasingAltitude" /> <!-- Tactic class name -->
 
   <!-- Derived (conditional) arguments -->
   <arg name="gps_arg" default="--gps-time-hack" if="$(arg gps)" />
@@ -31,8 +29,6 @@
   <param name="rosbag_enable" type="bool" value="$(arg do_bag)" />
   <param name="verify_enable" type="bool" value="$(arg do_verify)" />
   <param name="sensor_range" type="int" value="$(arg range)" />
-  <param name="tactic_module" type="str" value="$(arg tactic_module_default)" />
-  <param name="tactic_name" type="str" value="$(arg tactic_name_default)" />
 
   <!-- Core Nodes -->
 
@@ -129,11 +125,12 @@
   <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" />
+    <remap from="tactic_interface/firing_reports" to="air_to_air/firing_reports" />    
   </node>
 
   <!-- TODO(tfoote) how to replace this gracefully? -->

ap_network_bridge/src/network.py

@@ -42,6 +42,7 @@ def timed_status(bridge):
     message.ok_pwr = False
     message.swarm_state = timed_status.swarm_state
     message.swarm_behavior = timed_status.swarm_behavior
+    message.swarm_sub_behavior = timed_status.swarm_sub_behavior
     message.fence_state = 2
     message.batt_rem = 0
     message.batt_vcc = 0
@@ -102,6 +103,7 @@ timed_status.c_status = None        # Controller status
 timed_status.f_status = None        # Flight status
 timed_status.swarm_state = 0        # Current swarm state
 timed_status.swarm_behavior = 0     # Currently active swarm behavior
+timed_status.swarm_sub_behavior = 0 # Currently active swarm_sub_behavior
 timed_status.calpress_done = False  # Airspeed calibration done?
 timed_status.calgyros_done = True   # Gyros calibration done?
 
@@ -128,6 +130,10 @@ def sub_swarm_behavior(msg, bridge):
     # Just update the swarm_behavior; timed event will do the send
     timed_status.swarm_behavior = msg.data
 
+def sub_swarm_sub_behavior(msg, bridge):
+    # Just update the swarm_sub_behavior; timed event will do the send
+    timed_status.swarm_sub_behavior = msg.data
+
 def sub_pose(msg, bridge):
     message = messages.Pose()
     message.msg_dst = Socket.ID_BCAST_ALL
@@ -768,6 +774,8 @@ if __name__ == '__main__':
                              std_msgs.msg.UInt8, sub_subswarm_id)
         bridge.addSubHandler('swarm_behavior',
                              std_msgs.msg.UInt8, sub_swarm_behavior)
+        bridge.addSubHandler('swarm_sub_behavior',
+                             std_msgs.msg.UInt8, sub_swarm_sub_behavior)
         bridge.addSubHandler('behavior_summary',
                              ap_msg.BehaviorGroupStateStamped,
                              sub_behavior_summary)

ap_path_planning/src/ap_path_planning/tactic_interface.py

 #!/usr/bin/env python
 
-# Hunter behavior directing each UAV to individually track and shoot
-# "red" UAVs (closest one targeted first)
-#
-# Duane Davis 2015
-
 # Standard python imports
 import math
-
 import numpy as np
+import imp
+import sys
 
 # ROS imports
 import rospy
@@ -21,12 +17,12 @@ import autopilot_bridge.srv as bridgesrv
 import ap_msgs.msg as apmsg
 import ap_lib.ap_enumerations as enums
 import ap_lib.bitmapped_bytes as bytes
+import ap_lib.parameter_helper as parameterBytes
 import ap_lib.gps_utils as gps
 import ap_lib.quaternion_math as quat
 from ap_lib.behavior import *
 from ap_lib.waypoint_behavior import *
-import imp
-import sys
+from ap_lib.behavior_parser import BehaviorParser
 
 class TacticInterface(WaypointBehavior):
     ''' Generic Swarm Behavior
@@ -92,7 +88,7 @@ class TacticInterface(WaypointBehavior):
     '''
     # Class-specific enumerations and constants
     
-    def __init__(self, nodename, own_id, tactic_module_name, tactic_name, \
+    def __init__(self, nodename, own_id, \
                  max_range=enums.MAX_RANGE_DEF, fov_width=enums.HFOV_DEF, \
                  fov_height=enums.VFOV_DEF):
         ''' Class initializer initializes class variables.
@@ -119,42 +115,56 @@ class TacticInterface(WaypointBehavior):
         self._firing_report_publisher = None
         self._behavior_data_publisher = None
         self._to_wp_publisher = None
+        self._swarm_sub_behavior_publisher = None
         self._wp_getlast_proxy = None
         self._tactic = None
         self._prev_run_time = None
-        self._sasc_scrimmage = None
+        self._behavior_parser = None
+        self._tactic_parser = None
         
         self.DBUG_PRINT = True
         self.INFO_PRINT = True
         self.WARN_PRINT = True
-
-        # Shutdown cleanly if we can't import sasc_scrimmage, this means that
-        # scrimmage isn't installed.
+        
+        # Parse the behaviors XML file
+        self._bp = BehaviorParser() 
+        self.log_info(self._bp)        
+        
+    #--------------------------------------------------------------------------
+    # Load a Tactic listed in the behaviors.xml file based on a tactic_id
+    #--------------------------------------------------------------------------
+    def load_tactic(self, tactic_id):
         try:
-            fp, pathname, description = imp.find_module('sasc_scrimmage')
-            self._sasc_scrimmage = imp.load_module('sasc_scrimmage', fp, \
-                                                 pathname, description)
-        except ImportError:
-            self.log_info('sasc_scrimmage not installed, exiting gracefully.')
-            sys.exit()
-
+            tactic = self._bp.behaviors[tactic_id]
+        except KeyError:
+            self._log_error('Tactic ID doesn\t exist: %d' % tactic_id)
+            return False
+        
         # Load the custom python tactic module
-        fp, pathname, description = imp.find_module(tactic_module_name)
+        fp, pathname, description = imp.find_module(tactic['module'])
         if fp == None:
-            self.log_error('Cannot find tactic module: ' + tactic_module_name)
+            self.log_error('Cannot find tactic module: ' + tactic['module'])
+            return False
         else:
-            self.log_info('Found ' + tactic_module_name + ' at: ' + pathname)
-            tactic_module = imp.load_module(tactic_module_name, fp, pathname, \
+            self.log_info('Found ' + tactic['module'] + ' at: ' + pathname)
+            tactic_module = imp.load_module(tactic['module'], fp, pathname, \
                                             description)
             
             params = {'id': self._own_uav_id, 'team_id': 1}
 
-            tactic_class = getattr(tactic_module, tactic_name)
+            tactic_class = getattr(tactic_module, tactic['class'])
             self._tactic = tactic_class();
             self._tactic.__parent = self
             self._tactic.pubs = dict()
             self._tactic.init(params)
             self.log_info('Tactic Name: ' + self._tactic._name)
+            self.log_info('Successfully Loaded: ' + tactic['name'])            
+        
+        # Publish the swarm_sub_behavior tactic_id
+        msg = stdmsg.UInt8()
+        msg.data = tactic_id
+        self._swarm_sub_behavior_publisher.publish(msg)
+        return True
             
     #-------------------------------------------------
     # Implementation of parent class virtual functions
@@ -173,6 +183,9 @@ class TacticInterface(WaypointBehavior):
         self._to_wp_publisher = \
             self.createPublisher("waypoint_goto", stdmsg.UInt16, 1)
 
+        self._swarm_sub_behavior_publisher = \
+            self.createPublisher("swarm_sub_behavior", stdmsg.UInt8, 1)
+
 
     def callbackSetup(self):
         ''' Sets up required subscriptions and callbacks
@@ -200,9 +213,14 @@ class TacticInterface(WaypointBehavior):
         self.subscribe_to_red_states()
         self.subscribe_to_swarm()
         
+        self._tactic_parser = parameterBytes.TacticInterfaceParser()
+        self._tactic_parser.unpack(params)
+        
+        if not self.load_tactic(self._tactic_parser.tactic_id):
+            return False
+        
         self._last_wp_id = int(rospy.get_param("last_mission_wp_id"))
-        self.set_ready_state(True)
-        self.log_info("initializing tactic interface")
+        self.set_ready_state(True)        
         try:
             last_wp = self._wp_getlast_proxy().points[0]
             self._stdby_lat = last_wp.x
@@ -210,6 +228,9 @@ class TacticInterface(WaypointBehavior):
 
             self._tactic._safe_waypoint = np.array([last_wp.x, last_wp.y, 
                                                     last_wp.z])
+            print('SAFE WAYPOINT')
+            print(self._tactic._safe_waypoint)
+            
         except Exception as e:
             self.log_warn("unable to fetch last waypoint to initialize tactic interface")
             return False
@@ -234,16 +255,16 @@ class TacticInterface(WaypointBehavior):
                 self._tactic._reds = self._reds
                 self._tactic._reds_shot = self._reds_shot
                 self._tactic._blues = self._swarm
-                valid_waypoint = self._tactic.step_autonomy(rospy.Time.now(), 
+                valid_waypoint = self._tactic.step_autonomy(rospy.Time.now().to_sec(), 
                                                             dt)
                 action = self._tactic._action
                 
         if valid_waypoint:
-            waypoint = self._tactic._wp
+            waypoint = self._tactic._wp            
             self.wp_msg.lat = waypoint[0]
             self.wp_msg.lon = waypoint[1]
             self.wp_msg.alt = waypoint[2]
-            self.publishWaypoint(self.wp_msg)      
+            self.publishWaypoint(self.wp_msg)
             
         if action[0].lower() == 'fire':
             self._shot_time = rospy.Time.now()
@@ -346,9 +367,6 @@ class TacticInterface(WaypointBehavior):
 
 if __name__ == '__main__':
     ownAC = int(rospy.get_param("aircraft_id"))
-    tactic_module_name = rospy.get_param("tactic_module")
-    tactic_name = rospy.get_param("tactic_name")
-    tactic_interface = TacticInterface("tactic_interface", ownAC, 
-                                       tactic_module_name, tactic_name)
+    tactic_interface = TacticInterface("tactic_interface", ownAC)
     tactic_interface.runAsNode(10.0)