def init(self, params):
self._id = int(params['id'])
self._last_lat = None
self._last_lon = None
self._speed = None
self._heading = None
self._target_id = None
self._target_count = 0
self._target_dist = np.inf
self._target_last_lat = None
self._target_last_lon = None
self._target_heading = None
self._target_speed = None
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._reds_shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'GreedyShooterInt'
self._state = "HUNT" # states are HUNT, MANEUVER, ATTACK
def __init__(self, ownID, subswarm, initState, baseAlt=0.0):
''' Class initializer sets initial values for member variables
@param ownID: ID of the aircraft for this state vector
@param subswarm: ID of the subswarm to which this aircraft belongs
@param initState: Geodometry object with the initial state information
@param swarmState
@param baseAlt: MSL altitude from which rel_alt is calculated
'''
self.ID = ownID
self.subSwarmID = subswarm
self.swarmState = 0
self.swarmBehavior = 0
self.state = initState
self.state.header.seq = 0
self.state.header.frame_id = 'base_footprint'
if not initState.pose.pose.position.using_rel_alt:
self.state.pose.pose.position.rel_alt = \
initState.pose.pose.position.alt - baseAlt
self.state.pose.pose.position.using_alt = True
self.state.pose.pose.position.using_rel_alt = True
self.drPose = apbrg.Geodometry()
self.drPose.header.seq = 0
self.state.header.frame_id = 'base_footprint'
self.drPose.pose.pose.orientation.w = 1.0
self.drPose.pose.pose.position.using_alt = True
self.drPose.pose.pose.position.using_rel_alt = True
self._stateMsg = SwarmVehicleState()
self._stateMsg.vehicle_id = self.ID
self._stateMsg.subswarm_id = self.subSwarmID
self._stateMsg.state.pose.pose.position.using_alt = True
self._stateMsg.state.pose.pose.position.using_rel_alt = True
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._weapons_range = enums.MAX_RANGE_DEF
self._tracking_range = self._weapons_range * 1.5
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._bboxes = enums.get_battleboxes()
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'Pincer'
self._state = States.new_start
try:
self._vehicle_type = int(params['vehicle_type'])
except KeyError:
self._vehicle_type = enums.AC_UNKNOWN
self.box_center = self.interpolate(self._bboxes[1][0][0],
self._bboxes[1][0][1],
self._bboxes[1][1][0],
self._bboxes[1][1][1], 0.5)
self._safe_corners = self.set_safe_corners()
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._speed = None
self._heading = None
self._target_count = 0
self._last_target = None
self._target_dist = np.inf
self._target_heading = None
self._target_speed = None
self._wp = np.array([0, 0, 0])
self._goal = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
#self._safe_waypoint_reserve = np.array([250, 125, 250])
#self._safe_waypoint_nw = np.array([125, 375, 250])
#self._safe_waypoint_sw = np.array([125, 125, 300])
#self._safe_waypoint_ne = np.array([375, 375, 200])
#self._safe_waypoint_se = np.array([375, 125, 350])
self._action = ["None", 0]
self._name = 'StormShooter'
self._got_target = False
#self._in_swarm = False
self._team = None
#self._min_dist = np.inf
self._goal_lat = None
self._goal_lon = None
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
#self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._vehicle_type = int(params['vehicle_type'])
self._spud = int(params['Survey Number'])
self.home = expanduser("~")
self.radeyeDir = self.home + "/scrimmage/usma/plugins/autonomy/python/"
if (self._spud == 1):
self._enumList = usma_enums.WP_LOC_S1
elif (self._spud == 0):
self._enumList = usma_enums.WP_LOC_SX
elif (self._spud == 4):
self._enumList = usma_enums.WP_LOC_S4
elif (self._spud == 5):
self._enumList = usma_enums.WP_LOC_S5
elif (self._spud == 6):
self._enumList = usma_enums.WP_LOC_RiverCourt
self._name = 'GreedyGoto'
#self._location = int(params['location'])
#self._desired_lat = float(self._enumList[self._location][0])
#self._desired_lon = float(self._enumList[self._location][1])
# Initialize Variables for Waypoint Assignment
self._subswarm_id = 0
self._id_in_subswarm = []
self._first_tick = True
self._subswarm_num_to_assign = []
self._subswarm_wp_assignment = dict()
self._wp_assigned = []
for i in range(0, len(self._enumList)):
self._wp_assigned.append(False)
#self.altitude = PointStamped()
#self.subs['altitude'] = self.createSubscriber('altitude', PointStamped, self.receive_altitude)
# Initialize Variables for Sequencing between Waypoints
self._wp_id_list = [] # List of WPs to be assigned to this UAS
for i in range(0, len(self._enumList)):
self._wp_id_list.append(i) # Place holder for other logic
self._wp_id_list_id = 0 # Index within assigned list of WPs
self._loc = self._wp_id_list[self._wp_id_list_id]
self._desired_lat = float(self._enumList[self._loc][0])
self._desired_lon = float(self._enumList[self._loc][1])
self._desired_alt = self._last_ap_wp[2]
self._original_alt = SAFE_ALT
self._time_at_wp = 0
self._time_start = 0
self._at_wp = False
self._base_alt = int(params['Base Altitude']) #sam g new
self._alt_change = int(params['Altitude Change']) #sam g new
self.callbackSetup() #BB
def _updateOwnPose(self, stateMsg):
''' Updates swarm info for this UAV when a new pose is published
@param poseMsg: Geodometry object with the new pose
TODO: add covariances when they are added to the msg
'''
try:
if not self.ownID in self._swarm:
try:
self._lock.acquire()
self._swarm[self.ownID] = \
SwarmElement(self.ownID, self.subSwarmID, apbrg.Geodometry())
except Exception as ex:
self.log_warn("Self update callback error: " + str(ex))
finally:
self._lock.release()
element = self._swarm[self.ownID]
element.updateState(stateMsg, self.subSwarmID, self.swarmState, \
self.swarmBehavior)
element.subSwarmID = self.subSwarmID
newBaseAlt = element.state.pose.pose.position.alt - \
element.state.pose.pose.position.rel_alt
if abs(newBaseAlt - self._baseAlt) > 0.001:
self._baseAlt = newBaseAlt
except Exception as ex:
self.log_warn("Self update callback error: " + str(ex))
def init(self, params):
self._id = int(params['id'])
self._target_id = None # -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._target_dist = np.inf # added to keep track of current target range
self._target_bearing = np.inf
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._tmp_list = list() # sets up lists of found enemy
self._sorted_list = list() # sorted list
self._target_list = list()
self._change = False
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'GreedyShooterMsg'
# This is the new messaging logic (from the documentation)
self.pubs['score'] = self.createPublisher('score', apmsg.MsgStat, 1)
self.subs['score'] = self.createSubscriber('score', apmsg.MsgStat,
self.receive_msg_stat)
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._target_range = np.inf # added to keep track of current target range
self._wp = np.array([0, 0, 0])
self._weapons_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._bboxes = enums.get_battleboxes()
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._taken = set(
) # this is the set that this UAV will use to keep track of "taken" enemy
self._t_id = list() # sets up lists of found enemy
self._t_range = list()
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'USMAQuadMsg'
self._bearing_adj = 0
# This is the new messaging logic (from the documentation)
self.pubs['taken'] = self.createPublisher('taken', apmsg.MsgStat, 1)
self.subs['taken'] = self.createSubscriber('taken', apmsg.MsgStat,
self.receive_msg_stat)
def init(self, params):
self._id = int(params['id'])
self._team_id = int(params['team_id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'LocalBehavior'
self._battleboxes = gps.get_battleboxes()
self._battlebox = self._battleboxes[0]
self._vehicle_type = int(params['vehicle_type'])
self._local_corners = None
self._run_conversions = True # only modified by scrimmage
if self._team_id == 1:
self._own_base = enums.GOAL_POSITS['blue']
else:
self._own_base = enums.GOAL_POSITS['red']
if self._team_id == 1:
self._enemy_base = enums.GOAL_POSITS['red']
else:
self._enemy_base = enums.GOAL_POSITS['blue']
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._vehicle_type = int(params['vehicle_type'])
self._name = 'GotoMult'
self._wp_dist = float(params['wp_dist']) # sets the distance you have to get to desired wp before going to next
self._loc = []
self._loc.append(int(params['loc1'])) # always contains a valid location number
self._loc.append(int(params['loc2'])) # may contain a location number, or other meaning
self._loc.append(int(params['loc3'])) # e.g. -1 means repeat path from loc1
self._loc.append(int(params['loc4'])) # e.g. 99 means orbit at last location forever
self._desired_lat = float(usma_enums.WP_LOC[self._loc[0]][0])
self._desired_lon = float(usma_enums.WP_LOC[self._loc[0]][1])
self._step = 0 # start at loc1 or loc[0]
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'SetVOParams'
self._hysteresis = float(params['Hysteresis'])
self._object_radius = float(params['Object Radius'])
self._t_cpa_thresh = float(params['T CPA Thresh'])
self._mode_2D = bool(params['Mode 2D'])
topic = 'velocity_obstacles_params'
try:
self.pubs[topic] = \
self.createPublisher(topic, apmsg.VelocityObstacles, 1)
except AttributeError:
pass
msg = apmsg.VelocityObstacles()
msg.hysteresis = self._hysteresis
msg.object_radius = self._object_radius
msg.t_cpa_thresh = self._t_cpa_thresh
msg.mode_2D = self._mode_2D
self.pubs[topic].publish(msg)
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._vehicle_type = int(params['vehicle_type'])
self._spud = int(params['Survey Number'])
self.home = expanduser("~")
self.radeyeDir = self.home + "/scrimmage/usma/plugins/autonomy/python/"
if (self._spud == 1):
self._enumList = usma_enums.WP_LOC_RiverCourt
elif (self._spud == 2):
self._enumList = usma_enums.WP_LOC_Range11
self._name = 'GreedyGoto'
self._radType = 0
# Initialize Variables for Waypoint Assignment
self._subswarm_id = 0
self._id_in_subswarm = []
self._first_tick = True
self._subswarm_num_to_assign = []
self._subswarm_wp_assignment = dict()
self._wp_assigned = []
for i in range(0, len(self._enumList)):
self._wp_assigned.append(False)
# Initialize Variables for Sequencing between Waypoints
self._wp_id_list = [] # List of WPs to be assigned to this UAS
for i in range(0, len(self._enumList)):
self._wp_id_list.append(i) # Place holder for other logic
self._wp_id_list_id = 0 # Index within assigned list of WPs
self._loc = self._wp_id_list[self._wp_id_list_id]
self._desired_lat = float(self._enumList[self._loc][0])
self._desired_lon = float(self._enumList[self._loc][1])
self._desired_alt = self._last_ap_wp[2]
self._original_alt = SAFE_ALT
self._time_at_wp = 0
self._time_start = 0
self._at_wp = False
self._base_alt = int(params['Base Altitude'])
self._alt_change = int(params['Altitude Change'])
self._altitude = 0.0
self._radiation = 0.0
topic='/autopilot/acs_pose'
outstring = "THIS IS MODIFIED TO FIND error logs"
print >>sys.stderr, "TESTINGTESTING1 "+ outstring
rospy.Subscriber('/tactic_interface/altitude', PointStamped, self.altitude_cb)
rospy.Subscriber('/tactic_interface/radiation', PointStamped, self.radeye_cb)
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'TutorialGreedyShooter'
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._weapons_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._bboxes = enums.get_battleboxes()
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'Vortex'
def init(self, params):
self._id = int(params['id'])
self._target_count = 0
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'EvadeOrAttack'
self._fov_width_radians = math.radians(enums.HFOV_DEF)
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._vehicle_type = int(params['vehicle_type'])
self._spud = int(params['Survey Number'])
self.home = expanduser("~")
self.radeyeDir = self.home + "/scrimmage/usma/plugins/autonomy/python/"
if (self._spud == 1):
self._enumList = usma_enums.WP_LOC_S1
elif (self._spud == 0):
self._enumList = usma_enums.WP_LOC_SX
elif (self._spud == 4):
self._enumList = usma_enums.WP_LOC_S4
elif (self._spud == 5):
self._enumList = usma_enums.WP_LOC_S5
elif (self._spud == 6):
self._enumList = usma_enums.WP_LOC_RiverCourt
self._name = 'InitialPass'
#self._location = int(params['location'])
#self._desired_lat = float(self._enumList[self._location][0])
#self._desired_lon = float(self._enumList[self._location][1])
# Initialize Variables for Waypoint Assignment
self._subswarm_id = 0
self._id_in_subswarm = []
self._first_tick = True
self._subswarm_num_to_assign = []
self._subswarm_wp_assignment = dict()
self._wp_assigned = []
for i in range(0, len(self._enumList)):
self._wp_assigned.append(False)
# Initialize Variables for Waypoint Assignment
self._subswarm_id = 0
self._id_in_subswarm = [] # number of vehicles?
self._first_tick = True
self._subswarm_num_to_assign = []
self._subswarm_wp_assignment = dict()
self._wp_assigned = []
for i in range(0, len(usma_enums.WP_LOC)):
self._wp_assigned.append(False)
def init_variables(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
#self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._vehicle_type = int(params["vehicle_type"])
self._name = "ObjectDetResponse"
#self._location = int(params['location'])
#self._desired_lat = float(usma_enums.WP_LOC[self._location][0])
#self._desired_lon = float(usma_enums.WP_LOC[self._location][1])
# Initialize Variables for Waypoint Assignment
self._subswarm_id = 0
self._subswarm_ids_list = [
] # This is actually a list of all the vehicle IDs in your subswarm
self._first_tick = True
self._subswarm_num_to_assign = []
self._subswarm_wp_assignment = dict()
#self._wp_assigned = []
#for i in range(0, len(usma_enums.WP_LOC)):
# self._wp_assigned.append(False)
# Initialize Variables for Sequencing between Waypoints
self._wp_id_list = [] # List of WPs to be assigned to this UAS
for i in range(0, len(usma_enums.WP_LOC)):
self._wp_id_list.append(i) # Place holder for other logic
self._wp_id_list_id = 0 # Index within assigned list of WPs
self._loc = self._wp_id_list[self._wp_id_list_id]
self._desired_lat = float(usma_enums.WP_LOC[self._loc][0])
self._desired_lon = float(usma_enums.WP_LOC[self._loc][1])
self._desired_alt = SAFE_ALT #self._last_ap_wp[2]
self._original_alt = SAFE_ALT
self._time_at_wp = 0
self._time_start = 0
self._at_wp = False
print "Variables assigned"
def init(self, params):
self._id = int(params['id'])
self._target_id = None
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
try:
self._safe_waypoint = self._parent._safe_waypoint
except AttributeError:
self._safe_waypoint = np.array([0, 0, 0])
self._wp = self._safe_waypoint
try:
self._vehicle_type = int(params['vehicle_type'])
except KeyError:
self._vehicle_type = enums.AC_UNKNOWN
self._action = ["None", 0]
self._name = 'SmartShooter'
self.behavior_state = States.hunt_tgt
self.pursuit_status = False
self.shot_pending = False
self.overtime = False
self.reds_pursued = []
self.wp_calc = \
pputils.PNInterceptCalculator(
self, self._id, self._reds, max_time_late=self.MAX_TIME_LATE
)
try:
topic = 'network/send_swarm_behavior_data'
# we dont need to create a subscriber here because this is taken care of in
self.pubs[topic] = self._parent._behavior_data_publisher
except AttributeError:
self.pubs[topic] = \
self.createPublisher(topic, apmsg.BehaviorParameters, 1)
self.subs[topic] = \
self.createSubscriber(
topic, apmsg.BehaviorParameters, self._process_swarm_data_msg
)
def init(self, params):
if self.lanes == False:
self.lanes = createLanes.createLanes(
) #should come out to 10 lanes
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
#self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._vehicle_type = int(params['vehicle_type'])
self._name = 'InitialPass'
#self._location = int(params['location']) #waypoint ID?
self._desired_lat = float(self.lanes[0][0][0])
self._desired_lon = float(self.lanes[0][0][1])
self._speed = 10
# Initialize Variables for Waypoint Assignment
self._subswarm_id = 0
self._id_in_subswarm = [] # number of vehicles?
self._first_tick = True
self._subswarm_num_to_assign = []
self._subswarm_wp_assignment = dict()
self._wp_assigned = []
for i in range(0, len(usma_enums.WP_LOC)):
self._wp_assigned.append(False)
# Initialize Variables for Sequencing between Waypoints
self._wp_id_list = [] # List of WPs to be assigned to this UAS
for i in range(0, len(self.lanes) * 2):
self._wp_id_list.append([41.390863, -73.952947])
self._wp_id_list_id = 0 # Index within assigned list of WPs
self._loc = self._wp_id_list[self._wp_id_list_id]
#self._desired_lat = float(usma_enums.WP_LOC[self._loc][0])
#self._desired_lon = float(usma_enums.WP_LOC[self._loc][1])
self._desired_alt = SAFE_ALT
self._original_alt = SAFE_ALT
self._at_wp = False
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
#self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._vehicle_type = int(params['vehicle_type'])
self._name = 'GreedyGoto'
#self._location = int(params['location'])
#self._desired_lat = float(usma_enums.WP_LOC[self._location][0])
#self._desired_lon = float(usma_enums.WP_LOC[self._location][1])
# Initialize Variables for Waypoint Assignment
self._subswarm_id = 0
self._id_in_subswarm = []
self._first_tick = True
self._subswarm_num_to_assign = []
self._subswarm_wp_assignment = dict()
self._wp_assigned = []
for i in range(0, len(usma_enums.WP_LOC)):
self._wp_assigned.append(False)
# Initialize Variables for Sequencing between Waypoints
self._wp_id_list = [] # List of WPs to be assigned to this UAS
for i in range(0, len(usma_enums.WP_LOC)):
self._wp_id_list.append(i) # Place holder for other logic
self._wp_id_list_id = 0 # Index within assigned list of WPs
self._loc = self._wp_id_list[self._wp_id_list_id]
self._desired_lat = float(usma_enums.WP_LOC[self._loc][0])
self._desired_lon = float(usma_enums.WP_LOC[self._loc][1])
self._desired_alt = self._last_ap_wp[2]
self._original_alt = SAFE_ALT
self._time_at_wp = 0
self._time_start = 0
self._at_wp = False
self._base_alt = int(params['Base Altitude']) #sam g new
self._alt_change = int(params['Altitude Change']) #sam g new
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._vehicle_type = int(params['vehicle_type'])
self._name = 'GotoOrbit'
self._desired_lat = float(params['lat'])
self._desired_lon = float(params['lon'])
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._last_target = None
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'GreedyShooterSingle'
self._last_update = 0
self._got_target = False
self._min_dist = np.inf
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = None
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'HelloGreedyShooter'
self._last_update = 0
self._start_time = float(params['start_time'])
self._start_climbing_time = float(params['Start Time'])
self._alt_step = float(params['Altitude Step'])
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
#self._red_state = apmsg.RedVehicleState()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'BrickTest'
self.subs['my_topic'] = \
self.createSubscriber('my_topic', apmsg.SwarmVehicleState, \
self.receive_msg_stat)
def init(self, params):
self._id = int(params['id'])
#def __init__(self, nodename):
''' Class initializer initializes class variables.
@param nodename: name of the ROS node in which this object exists
'''
#WaypointBehavior.__init__(self, nodename, enums.PN_INTERCEPTOR)
#self._own_uav_id = rospy.get_param("aircraft_id")
self._target_id = 0
self._state = USMAInterceptor.RUN_IN
self._state_initialized = False
self._t = None
self._dt = 0.0
self._xp = (0.0, 0.0, 0.0)
self._vp = (0.0, 0.0, 0.0)
self._xt = (0.0, 0.0, 0.0)
self._vt = (0.0, 0.0, 0.0)
self._range = 0.0
self._psi_p3 = 0.0
self._theta = 0.0
self._theta2 = 0.0
self._theta3 = 0.0
self._theta_dot = 0.0
self._theta_dot_last = 0.0
self._VT = (0.0, 0.0)
self._theta_VT_init = 0.0
self._theta_dot_VT_init = 0.0
self._psi_p_VT_init = 0.0
self._VT_turn_type = USMAInterceptor.DIRECT_TURN
self._s_turn_state = USMAInterceptor.S1
self._psi_p_f = 0.0
self._gain = 0.0
self._bias = 0.0
self._E = 1e9
self._E_trend = [1e9] * 10
self._bias_initialized = False # to facilitate a log entry
self._swarm = dict()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._own_pose = brgmsg.Geodometry()
self._wp = numpy.array([0, 0, 0])
def init(self, params):
self._id = int(params['id'])
self._team_id = int(params['team_id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'HelloGreedyShooter'
try:
self._vehicle_type = int(params['vehicle_type'])
except KeyError:
self._vehicle_type = enums.AC_FIXED_WING
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._vehicle_type = int(params['vehicle_type'])
self._name = 'Raid'
self._home = int(params['home'])
self._desired_lat = float(usma_enums.WP_LOC[self._home][0])
self._desired_lon = float(usma_enums.WP_LOC[self._home][1])
self._tloc = int(params['target'])
self._tlat = float(usma_enums.WP_LOC[self._tloc][0])
self._tlon = float(usma_enums.WP_LOC[self._tloc][1])
self._pass = 0
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._last_target = None
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._action = ["None", 0]
self._name = 'PythonGreedyAltitude'
self._last_update = 0
self._got_target = False
self._min_dist = np.inf
self._reach_alt = 0
self._descent = 0
self._tgt_find_count = 0
self._tracked = []
self._cur_track = 0
def init(self, params):
self._id = int(params['id'])
self._target_id = -1
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
self._reds = dict()
self._shot = set()
self.wp_state = 0
self.wp_change_time = -np.inf
self.straight_time = 30
self.bank_time = 60
self.cube_offset = 100
self.geobox = gps.GeoBox(
enums.BATTLE_CUBE_SW_LAT, enums.BATTLE_CUBE_SW_LON,
enums.BATTLE_CUBE_LENGTH, enums.BATTLE_CUBE_WIDTH,
enums.BATTLE_CUBE_ORIENT)
self.curr_corner = 3
try:
self._safe_waypoint = self._parent._safe_waypoint
except AttributeError:
self._safe_waypoint = np.array([0, 0, 0])
self._wp = self._safe_waypoint
pos = self._own_pose.pose.pose.position
self._safe_waypoint = np.array([pos.lat, pos.lon, pos.alt])
self._action = ["None", 0]
self._name = 'LinearFormation'
self.behavior_state = States.setup
try:
self.wp_calc = \
pputils.InterceptCalculator(
self._parent, self._id, self._parent._swarm
)
except AttributeError:
self.wp_calc = \
pputils.InterceptCalculator(self, self._id, dict())
parser = bytes.LinearFormationOrderParser()
self.formation_params = {
'dist': parser.distance,
'ang': parser.angle,
'stacked': parser.stack_formation,
'lead': False,
'start_time': -np.nan
}
topic = 'network/send_swarm_behavior_data'
try:
self.lock = self._parent._swarm_lock
self.pubs[topic] = self._parent._behaviorDataPublisher
except AttributeError:
self.lock = self.lock = threading.RLock()
self.pubs[topic] = \
self.createPublisher(topic, apmsg.BehaviorParameters, 1)
self.subs[topic] = \
self.createSubscriber(
topic, apmsg.BehaviorParameters,
self._process_swarm_data_msg
)
self.sorter = dist.EagerConsensusSort(set(), set(), self.lock, self._id)
self.sorter.set_msg_publisher(self.pubs[topic])
self.sorter.reset(self._id)
def init(self, params):
self._id = int(params['id'])
self._file_id = params['id']
self._target_id = -1
self._wp = np.array([0, 0, 0])
self._max_range = enums.MAX_RANGE_DEF
self._fov_width = enums.HFOV_DEF
self._fov_height = enums.VFOV_DEF
self._own_pose = apbrg.Geodometry()
self._blues = dict()
#self._reds = dict()
self._shot = set()
self._safe_waypoint = np.array([0, 0, 0])
self._last_ap_wp = np.array([0, 0, 0])
self._action = ["None", 0]
self._vehicle_type = int(params['vehicle_type'])
self._name = 'Fly'
#self._location = int(params['location'])
#self._desired_lat = float(usma_enums.WP_LOC[self._location][0])
#self._desired_lon = float(usma_enums.WP_LOC[self._location][1])
# Initialize Variables for Waypoint Assignment
self._subswarm_id = 0
self._id_in_subswarm = []
self._first_tick = True
self._subswarm_num_to_assign = []c
self._subswarm_wp_assignment = dict()
self._wp_assigned = []
for i in range(0, len(usma_enums.WP_LOC)):
self._wp_assigned.append(False)
# Initialize Variables for Sequencing between Waypoints
self._wp_id_list = [] # List of WPs to be assigned to this UAS
for i in range(0, len(usma_enums.WP_LOC)):
self._wp_id_list.append(i) # Place holder for other logic
self._wp_id_list_id = 0 # Index within assigned list of WPs
self._loc = self._wp_id_list[self._wp_id_list_id]
self._desired_lat = float(usma_enums.WP_LOC[self._loc][0])
self._desired_lon = float(usma_enums.WP_LOC[self._loc][1])
self._finished_list = []
self._time_at_wp = 0
self._time_start = 0
self._at_wp = False
######## this is the new messaging logic
self.pubs['wp_tracker'] = \
self.createPublisher('wp_tracker', apmsg.WPTracker, 1)
self.subs['wp_tracker'] = \
self.createSubscriber('wp_tracker', apmsg.WPTracker, \
self.receive_msg_stat)
msg = apmsg.SwarmSearchWaypointList()
msg.header = std_msg.msg.Header()
msg.header.seq = 5
msg.header.stamp = rospy.Time.now
msg.header.frame_id = "Peter"
msg.waypoints = self._finished_list
self.pubs['wp_tracker'].publish(msg)
print '{} sending message' \
.format(self._finished_list)