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)