def step_autonomy(self, t, dt): self._target_id = -1 self._action = ["None", 0] self._pass += 1 if self._pass <= 400: self._wp = np.array([self._tlat, self._tlon, self._last_ap_wp[2]]) elif self._pass < 600 and self._pass > 400: self._wp = np.array( [self._desired_lat, self._desired_lon, self._last_ap_wp[2]]) else: self._pass = 0 # Fire at any reds that are within firing parameters for target_id, red in self._reds.iteritems(): if (red.game_status == enums.GAME_ACTIVE_DEFENSE or red.game_status == enums.GAME_ACTIVE_OFFENSE): if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ red.state.pose.pose.position.lat, red.state.pose.pose.position.lon, red.state.pose.pose.position.rel_alt): self._target_id = target_id self._action = ["Fire", self._target_id] return True return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = None # Search for the closest target every frame min_dist = np.inf for uav in self._reds.values(): if uav.vehicle_id not in self._shot: # print uav.vehicle_id, d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id #print self._blues.values() # If a target has been identified, move towards it if self._target_id != None: #print self._reds[self._target_id] target_pos = self._reds[self._target_id].state.pose.pose.position # s = ", ".join(str(e) for e in self._shot) # print "Reds Shot: " # print s #print "ID: %d Target ID: %d Dist: %d" % (self._id, self._target_id, min_dist) own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon tgt_lat = target_pos.lat tgt_lon = target_pos.lon # Set the waypoint past the current target, so we don't go into a # loiter mode bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) self._wp = np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] #print "=====>ID: %d shot at Target %d" % (self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = None # Search for the closest target every frame min_dist = np.inf for uav in self._reds.values(): if uav.vehicle_id not in self._shot and (uav.game_status == enums.GAME_ACTIVE_DEFENSE or uav.game_status == enums.GAME_ACTIVE_OFFENSE): d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id # If a target has been identified, move towards it if self._target_id != None: target_pos = self._reds[self._target_id].state.pose.pose.position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon tgt_lat = target_pos.lat tgt_lon = target_pos.lon lat = tgt_lat lon = tgt_lon if self._vehicle_type == enums.AC_FIXED_WING: # When using fixed wing, Set the waypoint past the current # target, so we don't go into a loiter mode bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) self._wp = np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint # Maintain altitude self._wp[2] = self._last_ap_wp[2] return True
def target_hitable(self, target_id): sp = self._own_pose.pose.pose.position so = self._own_pose.pose.pose.orientation quat = (so.x, so.y, so.z, so.w) if target_id in self._reds: target_pos = self._reds[target_id].state.pose.pose.position return gps.hitable(sp.lat, sp.lon, sp.alt, quat, self._max_range, self._fov_width, self._fov_height, target_pos.lat, target_pos.lon, target_pos.alt) else: return False
def hitable(self, target_id): '''Returns True if the target_id is hitable ''' own_lla = self.state.lla() try: tgt_lla = self.local_reds[target_id].lla() except KeyError: print('ID not in local_reds: %d' % target_id) return false return gps.hitable(own_lla[0], own_lla[1], own_lla[2], \ (self.state.quat_gps.vector[0], \ self.state.quat_gps.vector[1], \ self.state.quat_gps.vector[2], \ self.state.quat_gps.scalar ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lla[0], tgt_lla[1], tgt_lla[2])
def step_autonomy(self, t, dt): status = super(GotoDefend, self).step_autonomy(t, dt) # Fire at any reds that are within firing parameters for target_id, red in self._reds.iteritems(): if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ red.state.pose.pose.position.lat, red.state.pose.pose.position.lon, red.state.pose.pose.position.rel_alt): self._action = ["Fire", target_id] return status
def step_autonomy(self, t, dt): status = super(GotoAttack, self).step_autonomy(t, dt) self._action = ["None", 0] # Fire at any reds that are within firing parameters for target_id, red in self._reds.iteritems(): if (red.game_status == enums.GAME_ACTIVE_DEFENSE or red.game_status == enums.GAME_ACTIVE_OFFENSE): if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ red.state.pose.pose.position.lat, red.state.pose.pose.position.lon, red.state.pose.pose.position.rel_alt): self._action = ["Fire", target_id] return status
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = -1 ############################################################ # If self._target_id is not None and in the shot group # then reset to None, otherwise set to last_target ############################################################ min_dist = 500 #self._target_dist # np.inf #print 'ID: {} min_dist {}'.format(self._id, min_dist) for uav in self._reds.values(): if uav.vehicle_id not in self._shot and ( uav.game_status == enums.GAME_ACTIVE_DEFENSE or uav.game_status == enums.GAME_ACTIVE_OFFENSE): uav_lat = uav.state.pose.pose.position.lat uav_lon = uav.state.pose.pose.position.lon uav_alt = uav.state.pose.pose.position.rel_alt d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav_lat, uav_lon) if d < min_dist: min_dist = d self._target_dist = d self._target_id = uav.vehicle_id if self._target_id in self._shot: self._target_id = -1 self._target_last_id = -1 self._target_dist = np.inf self._target_lat = None self._target_lon = None self._target_heading = None self._target_speed = None # If a target has been identified, move towards it if self._target_id > 0: #print "ID: %d Target: %d Last Target: %d" % (self._id, self._target_id, self._target_last_id) own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt # added to keep from changing alt if self._last_lat is not None: self._speed = gps.gps_distance(own_lat, own_lon,\ self._last_lat, self._last_lon) / 0.1 self._heading = gps.normalize_angle( gps.gps_bearing(self._last_lat, self._last_lon, own_lat, own_lon)) * RAD2DEG self._last_lat = own_lat self._last_lon = own_lon target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) #print 'UAV: {} tgt_lat: {} tgt_lon: {} last_lat: {} last_lon: {}'.format(self._id, tgt_lat, tgt_lon,\ #self._target_last_lat, self._target_last_lon) if (self._target_id == self._target_last_id) and (self._target_last_lat is not None): self._target_speed = gps.gps_distance(tgt_lat, tgt_lon,\ self._target_last_lat, self._target_last_lon) / 0.1 self._target_heading = gps.normalize_angle( gps.gps_bearing(self._target_last_lat, self._target_last_lon, tgt_lat, tgt_lon)) * RAD2DEG #print "ID: %d Speed: %f Head: %f Target: %d Dist: %f Speed: %f Head: %f" % (self._id, self._speed, self._heading, self._target_id, self._target_dist, self._target_speed, self._target_heading) #print "Lat: %f Lon: %f Head: %f T_lat: %f T_lon %f T_head: %f" % (own_lat, own_lon, self._heading, \ #tgt_lat, tgt_lon, self._target_heading) else: self._target_dist = np.inf self._target_lat = None self._target_lon = None self._target_heading = None self._target_speed = None self._target_last_id = self._target_id # set last target id to current id self._target_last_lat = tgt_lat self._target_last_lon = tgt_lon ############################################################################# # Calc intercept point # Reference: http://jaran.de/goodbits/2011/07/17/calculating-an-intercept-course-to-a-target-with-constant-direction-and-velocity-in-a-2-dimensional-plane/ # Translated from Java to Python ############################################################################# t = None if self._target_heading is not None: s = self._speed ox = tgt_lat - own_lat oy = tgt_lon - own_lon vx = self._target_speed * math.cos(self._target_heading) # sin vy = self._target_speed * math.sin(self._target_heading) # cos h1 = vx**2 + vy**2 - s**2 h2 = ox * vx + oy * vy if h1 == 0: t = -(ox**2 + oy**2) / (2 * h2) else: minusPHalf = -h2 / h1 disc = minusPHalf**2 - (ox**2 + oy**2) / h1 if disc < 0: t = None else: root = math.sqrt(disc) t1 = minusPHalf + root t2 = minusPHalf - root tmin = t1 if t1 < t2 else t2 tmax = t1 if t1 > t2 else t2 t = tmin if tmin > 0 else tmax if t < 0: # or d < self._max_range: # close enough so go right to him (switch to greedy shooter) t = None else: t *= 1.0 #1.53 if t is not None: int_lat = tgt_lat + vx * t int_lon = tgt_lon + vy * t int_dist = gps.gps_distance(own_lat, own_lon, int_lat, int_lon) bearing = gps.gps_bearing(own_lat, own_lon, int_lat, int_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, int_dist + 100) #print "ID: %d using intercept bearing %f to Lat: %f Lon: %f Dist: %f to UAV: %d" % (self._id, bearing, lat, lon, int_dist, self._target_id) # I_lat: %f I_lon: %f t: %f else: bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, dist + 100) # range was 1000 #print "ID: %d using direct bearing %f to Lat: %f Lon: %f to UAV: %d" % (self._id, bearing, lat, lon, self._target_id) # Set the waypoint past the current target, so we don't go into a loiter mode self._wp = np.array([lat, lon, own_alt]) # don't use tgt_alt # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", self._target_id] print "GS Int=============================>ID: %d shot at Target %d" % ( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._target_id = None self._target_last_id = -1 self._target_dist = np.inf self._target_lat = None self._target_lon = None self._target_heading = None self._target_speed = None self._wp = self._safe_waypoint #print 'ID: {} has no target!'.format(self._id) return True
def step_autonomy(self, t, dt): # Reset the action every loop self._action = ["None", 0] self._target_id = None own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.alt # Search for the closest target min_dist = np.inf for uav in self._reds.values(): if uav.vehicle_id not in self._shot: d = gps.gps_distance(own_lat, \ own_lon, \ uav.state.pose.pose.position.lat, \ uav.state.pose.pose.position.lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id # If a target has been identified, move towards it if self._target_id != None: target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon distance_to_target = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) # Set bearing to look at the enemy vehicle bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) # Set waypoint to move forward and strafe sideways once the enemy is within range self._evasion_range = self._weapons_range * 2 self._engagement_range = self._weapons_range * 2 / 3 if distance_to_target <= self._engagement_range: lat, lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(bearing - self._fov_width / 2), 0.1) elif distance_to_target <= self._evasion_range: # Check to see if sidestep right waypoint is outside the battlebox # (only need to check distance_to_target/2 since enemy is closing) lat_right, lon_right = gps.gps_newpos(own_lat, own_lon, \ gps.normalize(bearing + (math.pi / 3)), distance_to_target/2) if self._bboxes[0].contains(lat_right, lon_right, own_alt): lat, lon = gps.gps_newpos(own_lat, own_lon, \ gps.normalize(bearing + (math.pi / 3)), distance_to_target) else: # If right sidestep is outside the battlebox, sidestep to the left lat, lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(bearing - (math.pi / 3)), distance_to_target) else: lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) self._wp = np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w), \ self._weapons_range, self._fov_width, self._fov_height, \ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] #self._change = False self._target_range = np.inf self._target_score = np.inf del self._tmp_list[:] # clear the search lists del self._target_list[:] # Search for the closest target every frame min_dist = MIN_DIST # range to start looking for enemy min_score = MIN_SCORE for uav in self._reds.values(): if (uav.vehicle_id not in self._shot): target_pos = self._reds[ uav.vehicle_id].state.pose.pose.position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt own_orientation = self._own_pose.pose.pose.orientation tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt # get distance and bearing to potential enemy d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) # Convert absolute angles to relative (shooter to target) pose_quat = (own_orientation.x, own_orientation.y, own_orientation.z, own_orientation.w) rpy = qmath.quat_to_euler(pose_quat) rel_bearing = math.fabs( gps.normalize_angle(bearing - rpy[2])) * RAD2DEG #print 'UAV {} to enemy {}: Dist: {} Bearing {}'.format(self._id, uav.vehicle_id), d, rel_bearing) # is the target UAV in front of me? score = d * rel_bearing # lower is better if uav.vehicle_id not in self._tmp_list: self._tmp_list.append((uav.vehicle_id, d, rel_bearing)) #print 'UAV {} to enemy {}: Dist: {} Bearing {} Score {}'.format(self._id, uav.vehicle_id, d, rel_bearing, score) self._target_list = sorted(self._tmp_list, key=lambda tgts: tgts[1], reverse=True) target_list_len = len(self._target_list) - 1 for uav in self._target_list: if uav[0] in self._shot: self._target_list.remove(uav) if target_list_len < 0: self._target_id = None else: self._target_id = self._target_list[target_list_len][0] self._target_dist = self._target_list[target_list_len][1] self._target_bearing = self._target_list[target_list_len][2] #print 'UAV {} target list = {}'.format(self._id, self._target_list) # If a target has been identified, move towards it if self._target_id != None: target_pos = self._reds[self._target_id].state.pose.pose.position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt own_orientation = self._own_pose.pose.pose.orientation tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) msg = apmsg.MsgStat() msg.id = self._target_id msg.count = int(self._target_dist) msg.latency = self._target_bearing self.pubs['score'].publish(msg) #print '{} targeting UAV {} at distance {} bearing {}'.format(self._id, self._target_id, self._target_dist, self._target_bearing) # Set the waypoint past the current target, so we don't go into a # loiter mode lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 10) self._wp = np.array([ lat, lon, own_alt ]) # don't climb or dive to target (was target_pos.rel_alt) # Determine if the target is within firing parameters if gps.hitable(own_lat, own_lon, own_alt, \ (own_orientation.x, \ own_orientation.y, \ own_orientation.z, \ own_orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", self._target_id] print "=============> Wing %d shot at Target %d" % ( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint print 'UAV {} has no target ID!'.format(self._id) self._wp = self._safe_waypoint self._change = False return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = None # Search for the closest target every frame min_dist = self._min_dist # np.inf own_pos = self._own_pose.pose.pose.position own_orientation = self._own_pose.pose.pose.orientation own_lat = own_pos.lat own_lon = own_pos.lon own_alt = own_pos.rel_alt for uav in self._reds.values(): if uav.vehicle_id not in self._shot and ( uav.game_status == enums.GAME_ACTIVE_DEFENSE or uav.game_status == enums.GAME_ACTIVE_OFFENSE): target_pos = self._reds[ uav.vehicle_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) if d <= self._max_range: if gps.hitable(own_lat, own_lon, own_alt, \ (own_orientation.x, own_orientation.y, \ own_orientation.z, own_orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", uav.vehicle_id] print 'GSA========(in loop)===========> ID: {} shot at UAV: {}'.format( self._id, uav.vehicle_id) #self._action = ["None", 0] if d < min_dist: min_dist = d self._target_id = uav.vehicle_id if self._target_id in self._shot: self._target_id = None if self._last_target_id == self._target_id: total_count = self._max_count * ( 1 + self._chase_safe) / self._chase_safe if d < self._target_dist: # how close do we have to be before I call chasing self._target_count += 1 if self._target_count < self._max_count: pass #print 'ID: {} chasing UAV: {} for {} iterations'.format(self._id, self._target_id, self._target_count) elif self._target_count < total_count: pass #print 'ID: {} going to safe wp for {} iterations'.format(self._id, total_count - self._target_count) else: self._target_count = 0 self._target_id = None # If a target has been identified, move towards it if self._target_id != None and self._target_count < self._max_count: self._last_target_id = self._target_id target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt lat = tgt_lat lon = tgt_lon if self._vehicle_type == enums.AC_FIXED_WING: # When using fixed wing, Set the waypoint past the current # target, so we don't go into a loiter mode bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) self._wp = np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(own_lat, own_lon, own_alt, \ (own_orientation.x, own_orientation.y, \ own_orientation.z, own_orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", self._target_id] print 'GSA============================> ID: {} shot at UAV: {}'.format( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint # Maintain altitude self._wp[2] = self._last_ap_wp[2] return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = self._last_target # Search for the closest target every frame if you don't already have one min_dist = np.inf # self._min_dist if self._got_target is False: for uav in self._reds.values(): if uav.vehicle_id not in self._shot: d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id self._last_target = self._target_id if d < MIN_DIST: self._got_target = True else: self._got_target = False else: self._got_target = False self._last_target = None for uav in self._reds.values(): if uav.vehicle_id not in self._shot: if self._target_id == uav.vehicle_id: self._last_target = self._target_id self._got_target = True # If a target has been identified, move towards it if self._target_id != None: target_pos = self._reds[self._target_id].state.pose.pose.position # print "*************************************" print "ID: %d Target ID: %d\n" % (self._id, self._target_id) own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt # Set the waypoint past the current target, so we don't go into a # loiter mode bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) self._wp = np.array([lat, lon, tgt_alt ]) # np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] self._last_target = None self._got_target = False else: # Start at the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = None self._target_range = np.inf self._target_score = np.inf del self._t_id[:] # clear the search lists del self._t_score[:] # Search for the closest target every frame min_dist = MIN_DIST # range to start looking for enemy min_score = MIN_SCORE for uav in self._reds.values(): if (uav.vehicle_id not in self._shot): target_pos = self._reds[ uav.vehicle_id].state.pose.pose.position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt own_orientation = self._own_pose.pose.pose.orientation tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt # get distance and bearing to potential enemy d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) # Convert absolute angles to relative (shooter to target) pose_quat = (own_orientation.x, own_orientation.y, own_orientation.z, own_orientation.w) rpy = qmath.quat_to_euler(pose_quat) rel_bearing = math.fabs(gps.normalize_angle(bearing - rpy[2])) #print 'UAV {} to enemy {}: Dist: {} Bearing {}'.format(self._id, uav.vehicle_id), d, rel_bearing) # is the target UAV in front of me? score = d * rel_bearing # lower is better print 'UAV {} to enemy {}: Dist: {} Bearing {} Score {}'.format( self._id, uav.vehicle_id, d, rel_bearing, score) if score < min_score: min_score = score #self._target_score = score #self._target_id = uav.vehicle_id #self._target_range = d if uav.vehicle_id not in self._t_id: self._t_id.append(uav.vehicle_id) self._t_score.append(score) # whoops! was d! self._target_id = uav.vehicle_id self._target_score = score self._target_range = d #print 'UAV {} to enemy {}: Dist: {} Bearing {} Score {}'.format(self._id, uav.vehicle_id, d, rel_bearing, score) # If a target has been identified, move towards it if self._target_id != None: msg = apmsg.MsgStat() msg.id = self._id msg.count = self._target_id msg.latency = self._target_score self.pubs['score'].publish(msg) #print '{} targeting UAV {} with a score of of {}'.format(self._id, self._target_id, self._target_score) target_pos = self._reds[self._target_id].state.pose.pose.position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt own_orientation = self._own_pose.pose.pose.orientation tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt # Set the waypoint past the current target, so we don't go into a # loiter mode bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) self._wp = np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(own_lat, own_lon, own_alt, \ (own_orientation.x, \ own_orientation.y, \ own_orientation.z, \ own_orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", self._target_id] print "=============> Wing %d shot at Target %d" % ( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] # Search for the closest target every frame for uav in self._reds.values(): if uav.vehicle_id not in self._shot: # Calculate target and own position and orientation target_pos = self._reds[uav.vehicle_id].state.pose.pose.position target_orientation = self._reds[uav.vehicle_id].state.pose.pose.orientation tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt if self._bboxes[0].contains(tgt_lat, tgt_lon, tgt_alt): # added to only consider those in battlebox own_pos = self._own_pose.pose.pose.position own_lat = own_pos.lat own_lon = own_pos.lon own_alt = own_pos.rel_alt own_orientation = self._own_pose.pose.pose.orientation # Calculate bearing to target and from target to self bearing = gps.normalize(gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)) tgt_bearing = gps.normalize(gps.gps_bearing(tgt_lat, tgt_lon, own_lat, own_lon)) # Calculate heading/yaw of self and target own_rpy = qmath.quat_to_euler((own_orientation.x,\ own_orientation.y, \ own_orientation.z, \ own_orientation.w)) own_heading = gps.normalize(own_rpy[2]) tgt_rpy = qmath.quat_to_euler((target_orientation.x,\ target_orientation.y, \ target_orientation.z, \ target_orientation.w)) tgt_heading = gps.normalize(tgt_rpy[2]) # Calculate distance to target dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) bearing_adj = 0.0 # Calculate offset between bearing and target heading (measures degree of "head on") # ---------------------------------------------------------------------------- heading_diff = own_heading - tgt_heading tgt_bearing_diff = gps.normalize_pi( tgt_heading - tgt_bearing ) if math.fabs(heading_diff) > math.pi/2: # moving towards each other if tgt_bearing > (tgt_heading - self._fov_width_radians/2) or tgt_bearing < (tgt_heading + self._fov_width_radians/2): # we are in the "fan" if dist > 2*self._max_range and math.fabs(tgt_bearing_diff) < self._fov_width_radians/2: bearing_adj = math.copysign(ADJ_ANGLE, tgt_bearing_diff) #print 'ID: {} toward UAV: {}: adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj)) elif dist < self._max_range and math.fabs(tgt_bearing_diff) > self._fov_width_radians*3: # along side each other bearing_adj = math.copysign(3*ADJ_ANGLE, tgt_bearing_diff) #print 'ID: {} along side UAV: {}: adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj)) else: # heading in same general direction if math.fabs(tgt_bearing_diff) < math.pi/2: # I am in front of target if dist < 2*self._max_range and math.fabs(tgt_bearing_diff) < self._fov_width_radians/2: bearing_adj = -math.copysign(ADJ_ANGLE, tgt_bearing_diff) #print 'ID: {} away from UAV: {} adjusting bearing by {}'.format(self._id, uav.vehicle_id, math.degrees(bearing_adj)) # ---------------------------------------------------------------------------- lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj, 1000) self._wp = np.array([lat, lon, own_alt]) # keep own alt # Determine if the target is within firing parameters if gps.hitable(own_lat, own_lon, own_alt, \ (own_orientation.x, \ own_orientation.y, \ own_orientation.z, \ own_orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", uav.vehicle_id] print "EA=======================>ID: %d shot at Target %d" % (self._id, uav.vehicle_id) return True
def step_autonomy(self, t, dt): self._action = ["None", 0] if self._target_id in self._shot: self._target_id = -1 #dont excede max fly dist #break and check targets sdist = self.dist_calc(self._own_pose.pose.pose.position.lat, self._own_pose.pose.pose.position.lon) if sdist > 100: print "Exceded fly zone. Returning to holding pattern" sdist = self.dist_calc(self._own_pose.pose.pose.position.lat, self._own_pose.pose.pose.position.lon) self._target_id = -1 self._wp = np.array([self._desired_lat, self._desired_lon, self._last_ap_wp[2]]) return True #hunt current target if self._target_id != -1 and self._pursue <= 50: print "Continue hunting %d for %d more cycles" % (self._target_id, 50-self._pursue) self._pursue += 1 #check if target outside max dist target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon tdist = self.dist_calc(tgt_lat, tgt_lon) #if target outside airspace, clear and check for other targets if tdist > 150: print "%d left airspace; breaking contact" % self._target_id self._target_id = -1 self._pursue = 0 #target still within pursue distance, check if firing solution available else: own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) self._wp = np.array([lat, lon, own_alt]) #if firing solution, take shot if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): print "RESERVE===========================> %d Firing on %d" % (self._id, self._target_id) self._action = ["Fire", self._target_id] self._pursue = 0 return True #pursue exceded, reset and find new target if self._pursue > 50: self._pursue = 0 print "Pursue exceeded: reset target" self._target_id = -1 #check if any aircraft in airspace if self._target_id == -1: print "Checking for hostiles in airspace" target = -1 minDist = 1000 #find closest hostile within attack range for target_id, red in self._reds.iteritems(): if (red.game_status == enums.GAME_ACTIVE_DEFENSE or red.game_status == enums.GAME_ACTIVE_OFFENSE): target_pos = self._reds[target_id].state.pose.pose.position if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._target_id = target_id self._action = ["Fire", self._target_id] print "RESERVE===========================> %d Firing on %d" % (self._id, self._target_id) return True dist = self.dist_calc(red.state.pose.pose.position.lat, red.state.pose.pose.position.lon) prox = self.self_dist(red.state.pose.pose.position.lat, red.state.pose.pose.position.lon) if dist < 150 and target_id not in self._shot and prox < minDist: target = target_id minDist = dist self._target_id = target self._pursue = 0 #if this code is hit, no hostile aircraft in airspace, and still in max range #return home and end step self._wp = np.array([self._desired_lat, self._desired_lon, self._last_ap_wp[2]]) return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] ############################################################ # If self._target_id is not None and in the shot group # then reset to None, otherwise set to last_target ############################################################ min_dist = self._target_dist for uav in self._reds.values(): if uav.vehicle_id not in self._shot: uav_lat = uav.state.pose.pose.position.lat uav_lon = uav.state.pose.pose.position.lon uav_alt = uav.state.pose.pose.position.rel_alt d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav_lat, uav_lon) if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ 9999, 180, 180, uav_lat, uav_lon, uav_alt): if d < min_dist: min_dist = d self._target_id = uav.vehicle_id if self._target_id in self._shot: self._target_id = None self._target_dist = np.inf self._target_count = 0 self._target_last_lat = None self._target_last_lon = None self._target_heading = None self._target_speed = None # If a target has been identified, move towards it if self._target_id != None: own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon if self._last_lat is not None: self._speed = gps.gps_distance(own_lat, own_lon,\ self._last_lat, self._last_lon) / 0.1 self._heading = gps.gps_bearing(self._last_lat, self._last_lon, own_lat, own_lon) self._last_lat = own_lat self._last_lon = own_lon target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ tgt_lat,\ tgt_lon) if self._target_last_lat is not None: self._target_speed = gps.gps_distance(tgt_lat, tgt_lon,\ self._target_last_lat, self._target_last_lon) / 0.1 self._target_heading = gps.gps_bearing(self._target_last_lat, self._target_last_lon, tgt_lat, tgt_lon) #print "ID: %d Target: %d" % (self._id, self._target_id) #print "Lat: %f Lon: %f Head: %f T_lat: %f T_lon %f T_head: %f" % (own_lat, own_lon, self._heading, \ #tgt_lat, tgt_lon, self._target_heading) self._target_last_lat = tgt_lat self._target_last_lon = tgt_lon ############################################################################# # Calc intercept point # swapped lat, lon, sin, cos to see if it changed t = None if self._target_heading is not None: s = self._speed ox = tgt_lat - own_lat oy = tgt_lon - own_lon vx = self._target_speed * math.cos(self._target_heading) # sin vy = self._target_speed * math.sin(self._target_heading) # cos h1 = vx**2 + vy**2 - s**2 h2 = ox * vx + oy * vy if h1 == 0: t = -(ox**2 + oy**2) / (2 * h2) else: minusPHalf = -h2 / h1 disc = minusPHalf**2 - (ox**2 + oy**2) / h1 if disc < 0: t = None else: root = math.sqrt(disc) t1 = minusPHalf + root t2 = minusPHalf - root tmin = t1 if t1 < t2 else t2 tmax = t1 if t1 > t2 else t2 t = tmin if tmin > 0 else tmax if t < 0: t = None #else: #t *= 1.65 if t is not None: t_adj = 0.0 # 003 # add equiv of 1 sec int_lat = tgt_lat + vx * (t + t_adj) int_lon = tgt_lon + vy * (t + t_adj) int_dist = gps.gps_distance(own_lat, own_lon, int_lat, int_lon) bearing = gps.gps_bearing(own_lat, own_lon, int_lat, int_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, int_dist) #print "ID: %d using intercept bearing %f to Lat: %f Lon: %f Dist: %f Time: %f" % (self._id, bearing, lat, lon, int_dist, t*10000) # I_lat: %f I_lon: %f t: %f else: bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, dist + 250) # range was 1000 #print "ID: %d using direct bearing %f to Lat: %f Lon: %f Dist: %f" % (self._id, bearing, lat, lon, dist) # Set the waypoint past the current target, so we don't go into a # loiter mode #if tgt_alt < 200: # we're getting ground collisions #tgt_alt = 200 self._wp = np.array([lat, lon, tgt_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", self._target_id] print "GS Int=============================>ID: %d shot at Target %d" % ( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = None # Search for the closest target every frame min_dist = np.inf for uav in self._reds.values(): if uav.vehicle_id not in self._shot: d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon, \ uav.state.pose.pose.position.rel_alt): self._action = ["Fire", uav.vehicle_id] print "RT=========(loop)========>ID: %d shot at Target %d" % ( self._id, uav.vehicle_id) '''if self._last_target_id == self._target_id: if d < 100: # how close do we have to be before I call chasing self._target_count += 1 if self._target_count > 100: self._target_count = 300 self._target_id = None elif self._target_count > 0: self._target_count -= 1 self._target_id = None ''' # If a target has been identified, move towards it if self._target_id != None: self._last_target_id = self._target_id # Calculate target and own position and orientation target_pos = self._reds[self._target_id].state.pose.pose.position target_orientation = self._reds[ self._target_id].state.pose.pose.orientation tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt own_pos = self._own_pose.pose.pose.position own_lat = own_pos.lat own_lon = own_pos.lon own_alt = own_pos.rel_alt own_orientation = self._own_pose.pose.pose.orientation # Calculate bearing to target and from target to self bearing = gps.normalize( gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon)) tgt_bearing = gps.normalize( gps.gps_bearing(tgt_lat, tgt_lon, own_lat, own_lon)) # Calculate heading/yaw of self and target own_rpy = qmath.quat_to_euler((own_orientation.x,\ own_orientation.y, \ own_orientation.z, \ own_orientation.w)) own_heading = gps.normalize(own_rpy[2]) tgt_rpy = qmath.quat_to_euler((target_orientation.x,\ target_orientation.y, \ target_orientation.z, \ target_orientation.w)) tgt_heading = gps.normalize(tgt_rpy[2]) # Calculate distance to target dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) bearing_adj = 0.0 #print 'ID: {} targeting UAV: {} at bearing {} and distance {}'.format(self._id, self._target_id, math.degrees(bearing), dist) #print 'UAV: {} own bearing: {}'.format(self._target_id, math.degrees(tgt_heading)) # Calculate offset between bearing and target heading (measures degree of "head on") # This section needs more work # ---------------------------------------------------------------------------- heading_diff = own_heading - tgt_heading tgt_bearing_diff = gps.normalize_pi(tgt_heading - tgt_bearing) if math.fabs(heading_diff) > math.pi / 2: # moving towards each other if tgt_bearing > ( tgt_heading - self._fov_width_radians / 2 ) or tgt_bearing < (tgt_heading + self._fov_width_radians / 2): # we are in the "fan" if dist > self._max_range and math.fabs( tgt_bearing_diff) < self._fov_width_radians / 2: bearing_adj = math.copysign(ADJ_ANGLE, tgt_bearing_diff) print 'ID: {} toward UAV: {}: adjusting bearing by {}'.format( self._id, self._target_id, math.degrees(bearing_adj)) else: # heading in same general direction if math.fabs(tgt_bearing_diff) < math.pi / 2: # I am in front of target if dist < 2 * self._max_range and math.fabs( tgt_bearing_diff) < self._fov_width_radians / 2: bearing_adj = -math.copysign(ADJ_ANGLE, tgt_bearing_diff) print 'ID: {} away from UAV: {} adjusting bearing by {}'.format( self._id, self._target_id, math.degrees(bearing_adj)) # ---------------------------------------------------------------------------- lat, lon = gps.gps_newpos(own_lat, own_lon, bearing + bearing_adj, 1000) self._wp = np.array([lat, lon, own_alt]) # keep own alt # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] print "RT=======================>ID: %d shot at Target %d" % ( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = self._last_target # Search for the closest target every frame if you don't already have one min_dist = np.inf # self._min_dist if self._got_target is False: for uav in self._reds.values(): if uav.vehicle_id not in self._shot: d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if self._tgt_find_count >= 100: self._tracked = [] self._tgt_find_count = 0 print "=======================ID %d reseting targets\n" % ( self._id) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id if self._target_id not in self._tracked: self._last_target = self._target_id if d < MIN_DIST: self._got_target = True print "ID: %d locked onto Target ID: %d\n" % ( self._id, self._target_id) self._tracked.append(self._target_id) break else: print "ID: %d Found Last Target %d\n" % ( self._id, self._tgt_find_count) self._tgt_find_count += 1 self._got_target = False else: self._got_target = False self._target_id = None for uav in self._reds.values(): if uav.vehicle_id not in self._shot: if self._last_target == uav.vehicle_id: d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if d < MIN_DIST: self._target_id = self._last_target self._got_target = True print "ID: %d tracking Target ID: %d\n" % ( self._id, self._target_id) else: print "ID: %d lost Target ID: %d\n" % ( self._id, self._last_target) self._got_target = False #self._target_id = None #self._last_target = None # If a target has been identified, move towards it if self._target_id != None: self._cur_track += 1 target_pos = self._reds[self._target_id].state.pose.pose.position # print "*************************************" #print "ID: %d Target ID: %d\n" % (self._id, self._target_id) own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) # Set the waypoint past and above the current target, so we don't go into a # loiter mode, and can attack from above bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) if self._cur_track >= 100: print "ID: %d swapping target\n" % (self._id) self._cur_track = 0 attack_alt = 250 self._got_target = False lat, lon = gps.gps_newpos(tgt_lat, tgt_lon, bearing, 2000) else: lat, lon = gps.gps_newpos(tgt_lat, tgt_lon, bearing, 1000) if dist < 500: attack_alt = tgt_alt else: if tgt_alt > self._reach_alt: self._reach_alt = tgt_alt + 100 if self._descent == 1: attack_alt = own_alt - 50 if own_alt <= 500: self._descent = 0 else: if own_alt >= 1000: attack_alt = self._reach_alt / 2 self._descent = 1 elif own_alt < self._reach_alt: attack_alt = own_alt + 100 else: attack_alt = own_alt - 50 self._wp = np.array([ lat + (.00002 * self._id), lon + (.00002 * self._id), attack_alt ]) # np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] print "GS Single==========================>ID: %d shot at Target %d" % ( self._id, self._target_id) #self._last_target = None #self._got_target = False else: # Start at the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Get our current position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt # was just alt # Reset the action every loop self._action = ["None", 0] self._last_target_id = self._target_id self._last_target_bearing = self._target_bearing self._target_id = None self._target_bearing = 0.0 bearing_adj = -0.7 # default adjust for CCW circling enemy # Search for the closest target min_dist = np.inf for uav in self._reds.values(): if uav.vehicle_id not in self._shot and ( uav.game_status == enums.GAME_ACTIVE_DEFENSE or uav.game_status == enums.GAME_ACTIVE_OFFENSE): target_pos = self._reds[ uav.vehicle_id].state.pose.pose.position d = gps.gps_distance(own_lat, \ own_lon, \ uav.state.pose.pose.position.lat, \ uav.state.pose.pose.position.lon) if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w), \ self._weapons_range, self._fov_width, self._fov_height, \ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", uav.vehicle_id] print "Quad=======(loop)=======> ID: %d shot at enemy %d" % ( self._id, uav.vehicle_id) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id # If a target has been identified, move towards it if self._target_id != None: target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon distance_to_target = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) # Set bearing to look at the enemy vehicle bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) self._target_bearing = bearing if self._target_id == self._last_target_id: bearing_diff = self._target_bearing - self._last_target_bearing if bearing_diff < 0.0: bearing_adj = math.sin(-UAV_SPEED / distance_to_target) else: bearing_adj = math.sin(UAV_SPEED / distance_to_target) # Set waypoint to move forward and strafe sideways once the enemy is within range self._evasion_range = self._weapons_range * 3 self._engagement_range = self._weapons_range * 2 / 3 if distance_to_target <= self._engagement_range: lat, lon = gps.gps_newpos(own_lat, own_lon, gps.normalize(bearing + bearing_adj), 0.1) # was - self._fov_width/2 #print "UAV %d in engagement range with enemy %d bearing %f distance %f using %f bearing adj" % (self._id, self._target_id, math.degrees(bearing), distance_to_target, bearing_adj) elif distance_to_target <= self._evasion_range: # Check to see if sidestep right waypoint is outside the battlebox # (only need to check distance_to_target/2 since enemy is closing) evade_sign = -2 * (self._id % 2) + 1 #print "UAV %d in evasion range with enemy %d" % (self._id, self._target_id) lat, lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(bearing - evade_sign * (math.pi / 3)), distance_to_target) if not self._bboxes[0].contains(lat, lon, own_alt): lat, lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(bearing + evade_sign * (math.pi / 3)), distance_to_target) #print "UAV %d sidestepping left" % self._id else: lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) # was 1000 self._wp = np.array([lat, lon, own_alt ]) # don't change alt, was target_pos.rel_alt # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w), \ self._weapons_range, self._fov_width, self._fov_height, \ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] print "Quad====================> ID: %d shot at enemy %d" % ( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = None # Search for the closest target every frame min_dist = 500 # np.inf for uav in self._reds.values(): if uav.vehicle_id not in self._shot: d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id # If a target has been identified, move towards it if self._target_id != None: target_pos = self._reds[self._target_id].state.pose.pose.position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon tgt_lat = target_pos.lat tgt_lon = target_pos.lon # Set the waypoint past the current target, so we don't go into a # loiter mode bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) self._wp = np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] elif t < 10: # Start at the safe waypoint self._wp = self._safe_waypoint elif t > (self._last_update + 1): # Slowly increment the waypoint's altitude if self._own_pose.pose.pose.position.alt > 3000: self._wp[2] -= 5 elif self._own_pose.pose.pose.position.alt < 1000: self._wp[2] += 5 else: self._wp[2] += random.randint(-2, 10) self._last_update = t return True
def receive_msg_stat(self, msg): #self._reported.add(msg.id) if self._id != msg.id: self._taken.add(msg.count) #print '{} received message from {}: Targeting {} at distance {}'.format(self._id, msg.id, msg.count, msg.latency) if msg.count == self._target_id: if msg.latency < self._target_dist: #self._best_dist: target_pos = self._reds[msg.count].state.pose.pose.position blue_lat = self._blues[msg.id].state.pose.pose.position.lat blue_lon = self._blues[msg.id].state.pose.pose.position.lon blue_alt = self._blues[ msg.id].state.pose.pose.position.rel_alt blue_orientation = self._blues[ msg.id].state.pose.pose.orientation tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt if gps.hitable(blue_lat, blue_lon, blue_alt, \ (blue_orientation.x, \ blue_orientation.y, \ blue_orientation.z, \ blue_orientation.w ), \ np.inf, 45.0, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): #self._best_dist = msg.latency #self._best_id = msg.id print '{} is closer than {} to UAV {} and is within 45 deg'.format( msg.id, self._id, msg.count) l = len(self._t_id) if l > 1: self._t_id.pop() self._t_dist.pop() self._target_id = self._t_id[l - 2] self._target_dist = self._t_dist[l - 2] #self._taken.add(msg.count) print '{} dropped UAV {} due to {} being better candidate at distance {}'.format( self._id, msg.count, msg.id, msg.latency) print '{} now targeting UAV {} at a distance of {}'.format( self._id, self._target_id, self._target_dist) else: self._target_id = None self._target_dist = np.inf #self._taken.discard(msg.count) else: self._taken.discard(msg.count) '''if self._best_dist < self._target_dist: l = len(self._t_id) if l > 1: self._t_id.pop() self._t_dist.pop() self._target_id = self._t_id[l-2] self._target_dist = self._t_dist[l-2] #self._taken.add(msg.count) print '{} dropped UAV {} due to {} being better candidate at distance {}'.format(self._id, msg.count, self._best_id, self._best_dist) print '{} now targeting UAV {} at a distance of {}'.format(self._id, self._target_id, self._target_dist) else: self._target_id = None self._target_dist = np.inf #self._taken.discard(msg.count) else: self._taken.discard(msg.count) ''' '''if self._reported == set(self._blues) or len(self._blues) == 1:
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] ############################################################ # If self._target_id is not None and in the shot group # then reset to None, otherwise set to last_target ############################################################ min_dist = self._target_dist for uav in self._reds.values(): if uav.vehicle_id not in self._reds_shot: uav_lat = uav.state.pose.pose.position.lat uav_lon = uav.state.pose.pose.position.lon d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav_lat,\ uav_lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id if self._target_id in self._reds_shot: self._target_id = None self._target_dist = np.inf self._target_count = 0 self._target_last_lat = None self._target_last_lon = None self._target_heading = None self._target_speed = None # If a target has been identified, move towards it if self._target_id != None: own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon if self._last_lat is not None: self._speed = gps.gps_distance(own_lat, own_lon,\ self._last_lat, self._last_lon) / 0.1 self._heading = gps.gps_bearing(self._last_lat, self._last_lon, own_lat, own_lon) self._last_lat = own_lat self._last_lon = own_lon target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ tgt_lat,\ tgt_lon) if self._target_last_lat is not None: self._target_speed = gps.gps_distance(tgt_lat, tgt_lon,\ self._target_last_lat, self._target_last_lon) / 0.1 self._target_heading = gps.gps_bearing(self._target_last_lat, self._target_last_lon, tgt_lat, tgt_lon) #print "ID: %d Target: %d" % (self._id, self._target_id) #print "Lat: %f Lon: %f Head: %f T_lat: %f T_lon %f T_head: %f" % (own_lat, own_lon, self._heading, \ #tgt_lat, tgt_lon, self._target_heading) self._target_last_lat = tgt_lat self._target_last_lon = tgt_lon ############################################################################# # Calc intercept point # Reference: http://jaran.de/goodbits/2011/07/17/calculating-an-intercept-course-to-a-target-with-constant-direction-and-velocity-in-a-2-dimensional-plane/ # Translated from Java to Python ############################################################################# t = None if self._target_heading is not None: s = self._speed ox = tgt_lat - own_lat oy = tgt_lon - own_lon vx = self._target_speed * math.cos(self._target_heading) # sin vy = self._target_speed * math.sin(self._target_heading) # cos h1 = vx**2 + vy**2 - s**2 h2 = ox * vx + oy * vy if h1 == 0: t = -(ox**2 + oy**2) / (2 * h2) else: minusPHalf = -h2 / h1 disc = minusPHalf**2 - (ox**2 + oy**2) / h1 if disc < 0: t = None else: root = math.sqrt(disc) t1 = minusPHalf + root t2 = minusPHalf - root tmin = t1 if t1 < t2 else t2 tmax = t1 if t1 > t2 else t2 t = tmin if tmin > 0 else tmax if t < 0: t = None if t is not None: t_adj = 0 # add extra time for tweaking int_lat = tgt_lat + vx * (t + t_adj) int_lon = tgt_lon + vy * (t + t_adj) int_dist = gps.gps_distance(own_lat, own_lon, int_lat, int_lon) bearing = gps.gps_bearing(own_lat, own_lon, int_lat, int_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, int_dist + 1000) #print "Using intercept Lat: %f Lon: %f Time: %f" % (lat, lon, t) # I_lat: %f I_lon: %f t: %f else: bearing = gps.gps_bearing( own_lat, own_lon, tgt_lat, tgt_lon ) # use standard TutorialGreedyShooter method of new waypoint lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) # range was 1000 #print "Using bearing %f to Lat: %f Lon: %f" % (bearing, lat, lon) # Set the waypoint past the current target, so we don't go into a loiter mode self._wp = np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", self._target_id] #print "GS Int=============================>ID: %d shot at Target %d" % (self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] if self._target_id in RedGreedyShooter._shot: RedGreedyShooter._reds_taken.remove(self._target_id) self._target_id = None else: self._target_id = self._last_target min_dist = np.inf for uav in self._reds.values(): uav_lat = uav.state.pose.pose.position.lat uav_lon = uav.state.pose.pose.position.lon d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav_lat,\ uav_lon) if uav.vehicle_id not in RedGreedyShooter._shot: if uav_lat == self._reds_last_lat[uav.vehicle_id]: # check to see if target moved since last time RedGreedyShooter._shot.add(uav.vehicle_id) # add to dead list print "===============================>Enemy ID: %d added to dead list" % (uav.vehicle_id) if uav.vehicle_id in RedGreedyShooter._reds_taken: RedGreedyShooter._reds_taken.remove(uav.vehicle_id) else: self._reds_last_lat[uav.vehicle_id] = uav_lat # yes, update position if uav.vehicle_id not in RedGreedyShooter._reds_taken: if d < min_dist: min_dist = d self._target_id = uav.vehicle_id self._last_target = self._target_id # If a target has been identified, move towards it if self._target_id != None: #if self._target_id is not self._last_target: #RedGreedyShooter._reds_taken.remove(self._last_target) RedGreedyShooter._reds_taken.add(self._target_id) target_pos = self._reds[self._target_id].state.pose.pose.position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ tgt_lat,\ tgt_lon) print "ID: %d Target ID: %d Dist: %d" % (self._id, self._target_id, d) s = ", ".join(str(e) for e in RedGreedyShooter._reds_taken) print "Reds Taken: " print s # Set the waypoint past the current target, so we don't go into a # loiter mode bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 200) # range was 1000 self._wp = np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", self._target_id] print "=====>ID: %d shot at Target %d" % (self._id, self._target_id) '''if tgt_lat != self._reds_last_lat[self._target_id]: # check to see if target moved since last time self._reds_last_lat[self._target_id] = tgt_lat # yes, update position else: RedGreedyShooter._shot.add(self._target_id) # add to dead list print "=====>Enemy ID: %d added to dead list" % (self._target_id) RedGreedyShooter._reds_taken.remove(self._target_id) self._last_target = None s = ", ".join(str(e) for e in RedGreedyShooter._shot) print "Reds Shot: " print s ''' # RedGreedyShooter._reds_taken.remove(self._target_id) # release enemy ID from set else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Get our current position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt # was just alt # Reset the action every loop self._action = ["None", 0] self._target_id = None # Search for the closest target min_dist = MIN_DIST del t_id[:] del t_dist[:] for uav in self._reds.values(): if (uav.vehicle_id not in self._shot) and (uav.vehicle_id not in self._taken): d = gps.gps_distance(own_lat, \ own_lon, \ uav.state.pose.pose.position.lat, \ uav.state.pose.pose.position.lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id self._bearing_adj = -(self._fov_width / 2) self._target_range = d # If a target has been identified, move towards it if self._target_id != None: msg = apmsg.MsgStat() msg.id = self._id msg.count = self._target_id msg.latency = self._target_range self.pubs['taken'].publish(msg) print '{} targeting UAV {} at a distance of {}'.format( self._id, self._target_id, self._target_range) print '{} has taken list of {}'.format(self._id, self._taken) target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon distance_to_target = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) # Set bearing to look at the enemy vehicle bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) # Set waypoint to move forward and strafe sideways once the enemy is within range self._evasion_range = self._weapons_range * 2 self._engagement_range = self._weapons_range if distance_to_target <= self._engagement_range: self._bearing_adj += 2.5 if self._bearing_adj > self._fov_width: self._bearing_adj = -self._fov_width bearing += math.radians( self._bearing_adj ) # bearing_adj in degrees; convert to radians lat, lon = gps.gps_newpos(own_lat, own_lon, gps.normalize(bearing), 0.1) # was - self._fov_width/2 #print "UAV %d in engagement range with enemy %d bearing %f" % (self._id, self._target_id, math.degrees(bearing)) elif distance_to_target <= self._evasion_range: # Check to see if sidestep right waypoint is outside the battlebox # (only need to check distance_to_target/2 since enemy is closing) #print "UAV %d in evasion range with enemy %d" % (self._id, self._target_id) lat_right, lon_right = gps.gps_newpos(own_lat, own_lon, \ gps.normalize(bearing + (math.pi / 3)), distance_to_target/2) if self._bboxes[0].contains(lat_right, lon_right, own_alt): lat, lon = gps.gps_newpos(own_lat, own_lon, \ gps.normalize(bearing + (math.pi / 3)), distance_to_target) #print "UAV %d sidestepping right" % self._id else: # If right sidestep is outside the battlebox, sidestep to the left lat, lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(bearing - (math.pi / 3)), distance_to_target) #print "UAV %d sidestepping left" % self._id else: lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 100) # was 1000 self._wp = np.array([lat, lon, target_pos.rel_alt ]) # +random.randint(-10,10)] # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w), \ self._weapons_range, self._fov_width, self._fov_height, \ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] print "=================>ID: %d shot at Target %d" % ( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint self._taken.clear( ) # clear the taken list in case it's full of old data return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = None self._target_range = np.inf # print set(self._blues) # Search for the closest target every frame min_dist = MIN_DIST # range to start looking for enemy for uav in self._reds.values(): if (uav.vehicle_id not in self._shot) and (uav.vehicle_id not in self._taken): target_pos = self._reds[ uav.vehicle_id].state.pose.pose.position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt own_orientation = self._own_pose.pose.pose.orientation tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt d = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) # is the target UAV in front of me? if gps.hitable(own_lat, own_lon, own_alt, \ (own_orientation.x, \ own_orientation.y, \ own_orientation.z, \ own_orientation.w ), \ np.inf, 180.0, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): if d < min_dist: min_dist = d self._target_id = uav.vehicle_id self._target_range = d if (self._target_id in self._shot) or (self._target_id in self._taken): self._target_id = None self._target_range = np.inf # If a target has been identified, move towards it if self._target_id != None: msg = apmsg.MsgStat() msg.id = self._id msg.count = self._target_id msg.latency = self._target_range self.pubs['taken'].publish(msg) print '{} targeting UAV {} at a distance of {}'.format( self._id, self._target_id, self._target_range) print '{} has taken list of {}'.format(self._id, self._taken) target_pos = self._reds[self._target_id].state.pose.pose.position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = self._own_pose.pose.pose.position.rel_alt own_orientation = self._own_pose.pose.pose.orientation tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt # Set the waypoint past the current target, so we don't go into a # loiter mode bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 100) self._wp = np.array([lat, lon, target_pos.rel_alt]) # Determine if the target is within firing parameters if gps.hitable(own_lat, own_lon, own_alt, \ (own_orientation.x, \ own_orientation.y, \ own_orientation.z, \ own_orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", self._target_id] print "=================>ID: %d shot at Target %d" % ( self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint self._taken.clear( ) # clear the taken list in case it's full of old data return True
def step_autonomy(self, t, dt): #swarmCount = 0 # Reset the action and target ID every loop self._action = ["None", 0] #self._target_id = self._last_target if self._team is None: if self._id in self._blues: self._team = 'blue' else: self._team = 'red' (self._goal_lat, self._goal_lon, _) = enums.GOAL_POSITS[self._team] print "lat %f lon %f" % (self._goal_lat, self._goal_lon) #go to waypoints for storm formation if self._got_target is False and self._goal_lat is not None: #self._in_swarm = True if self._id%5 == 0: storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + math.pi/2.0, 0) self._wp = np.array([storm_lat, storm_lon, 500]) #self._safe_waypoint_reserve 35.720785, -120.765860 print "ID: %d sent RESERVE %f, %f\n" % (self._id, storm_lat, storm_lon) if self._id%5 == 1: storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT), 100) self._wp = np.array([storm_lat, storm_lon, 500]) #self._wp = np.array([125, 375, 250])#self._safe_waypoint_nw 35.720785, -120.765860 print "ID: %d sent NW %f, %f\n" % (self._id, storm_lat, storm_lon) if self._id%5 == 2: storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + math.pi, 100) self._wp = np.array([storm_lat, storm_lon, 500]) #self._wp = np.array([125, 125, 300])#self._safe_waypoint_sw 35.719981, -120.769101 print "ID: %d sent SW %f, %f\n" % (self._id, storm_lat, storm_lon) if self._id%5 == 3: storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + math.pi/4.0, 100) self._wp = np.array([storm_lat, storm_lon, 500]) #self._wp = np.array([375, 375, 200])#self._safe_waypoint_ne 35.720785, -120.765860 print "ID: %d sent NE %f, %f\n" % (self._id, storm_lat, storm_lon) if self._id%5 == 4: storm_lat, storm_lon = gps.gps_newpos(self._goal_lat, self._goal_lon, math.radians(enums.BATTLE_CUBE_ORIENT) + 3.0*math.pi/4.0, 100) self._wp = np.array([storm_lat, storm_lon, 500]) #self._wp = np.array([375, 125, 350])#self._safe_waypoint_se 35.719981, -120.769101 print "ID: %d sent SE %f, %f\n" % (self._id, storm_lat, storm_lon) # Search for the closest target every frame if you don't already have one min_dist = self._target_dist # self._min_dist if self._got_target is False: for uav in self._reds.values(): if uav.vehicle_id not in self._shot: d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id self._last_target = self._target_id if d < self._max_range: self._got_target = True self._in_swarm = False print "UAV %d within range of Target %d\n" % (self._id, self._target_id) break else: self._got_target = False self._target_id = None for uav in self._reds.values(): if uav.vehicle_id not in self._shot: if self._last_target == uav.vehicle_id: d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if d < self._max_range: self._target_id = self._last_target if self._got_target is False: # only print once print "UAV %d within range of Target %d\n" % (self._id, self._target_id) self._got_target = True else: # was a match, but d > MIN_DIST, reset all print "UAV %d out of range of Target %d\n" % (self._id, self._last_target) self._got_target = False self._target_id = None self._last_target = None # If a target has been identified, move towards it if self._target_id != None: target_pos = self._reds[self._target_id].state.pose.pose.position #print "*************************************" #print "ID: %d Target ID: %d\n" % (self._id, self._target_id) own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt # Set the waypoint past the current target, so we don't go into a # loiter mode if self._got_target is True: bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) self._wp = np.array([lat, lon, tgt_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] #self._action = ["None", 0] print "Storm Shooter==========================>UAV %d shot at Target %d" % (self._id, self._target_id) #self._last_target = None #self._got_target = False #else: # Start at the safe waypoint #self._wp = self._safe_waypoint return True
def step_autonomy(self, t, dt): # Reset the action and target ID every loop self._action = ["None", 0] msg = apmsg.MsgStat() msg.id = self._id ############################################################ # If self._target_id is not None and in the shot group # then reset to None, otherwise set to last_target ############################################################ min_dist = self._target_dist for uav in self._reds.values(): if uav.vehicle_id not in self._shot: if uav.vehicle_id not in self._taken: uav_lat = uav.state.pose.pose.position.lat uav_lon = uav.state.pose.pose.position.lon uav_alt = uav.state.pose.pose.position.rel_alt d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav_lat, uav_lon) if d < min_dist: min_dist = d self._target_id = uav.vehicle_id self._target_range = d if self._target_id in self._shot: self._target_id = None self._target_dist = np.inf self._target_count = 0 self._target_last_lat = None self._target_last_lon = None self._target_heading = None self._target_speed = None # If a target has been identified, move towards it if self._target_id != None: msg.count = self._target_id msg.latency = self._target_range self.pubs['my_topic'].publish(msg) print '{} targeting UAV {} at a distance of {}'.format( self._id, msg.count, msg.latency) print '{} has taken list of {}'.format(self._id, self._taken) own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon if self._last_lat is not None: self._speed = gps.gps_distance(own_lat, own_lon,\ self._last_lat, self._last_lon) / 0.1 self._heading = gps.gps_bearing(self._last_lat, self._last_lon, own_lat, own_lon) self._last_lat = own_lat self._last_lon = own_lon target_pos = self._reds[self._target_id].state.pose.pose.position tgt_lat = target_pos.lat tgt_lon = target_pos.lon tgt_alt = target_pos.rel_alt d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ tgt_lat, tgt_lon) if self._target_last_lat is not None: self._target_speed = gps.gps_distance(tgt_lat, tgt_lon,\ self._target_last_lat, self._target_last_lon) / 0.1 self._target_heading = gps.gps_bearing(self._target_last_lat, self._target_last_lon, tgt_lat, tgt_lon) #print "ID: %d Target: %d" % (self._id, self._target_id) #print "Lat: %f Lon: %f Head: %f T_lat: %f T_lon %f T_head: %f" % (own_lat, own_lon, self._heading, \ #tgt_lat, tgt_lon, self._target_heading) self._target_last_lat = tgt_lat self._target_last_lon = tgt_lon ############################################################################# # Calc intercept point # Reference: http://jaran.de/goodbits/2011/07/17/calculating-an-intercept-course-to-a-target-with-constant-direction-and-velocity-in-a-2-dimensional-plane/ # Translated from Java to Python ############################################################################# t = None if self._target_heading is not None: s = self._speed ox = tgt_lat - own_lat oy = tgt_lon - own_lon vx = self._target_speed * math.cos(self._target_heading) # sin vy = self._target_speed * math.sin(self._target_heading) # cos h1 = vx**2 + vy**2 - s**2 h2 = ox * vx + oy * vy if h1 == 0: t = -(ox**2 + oy**2) / (2 * h2) else: minusPHalf = -h2 / h1 disc = minusPHalf**2 - (ox**2 + oy**2) / h1 if disc < 0: t = None else: root = math.sqrt(disc) t1 = minusPHalf + root t2 = minusPHalf - root tmin = t1 if t1 < t2 else t2 tmax = t1 if t1 > t2 else t2 t = tmin if tmin > 0 else tmax if t < 0 or d < self._max_range: # close enough so go right to him (switch to greedy shooter) t = None else: t *= 1.53 if t is not None: int_lat = tgt_lat + vx * t int_lon = tgt_lon + vy * t int_dist = gps.gps_distance(own_lat, own_lon, int_lat, int_lon) bearing = gps.gps_bearing(own_lat, own_lon, int_lat, int_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, int_dist) #print "ID: %d using intercept bearing %f to Lat: %f Lon: %f Dist: %f Time: %f" % (self._id, bearing, lat, lon, int_dist, t*10000) # I_lat: %f I_lon: %f t: %f else: bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) dist = gps.gps_distance(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, dist + 250) # range was 1000 #print "ID: %d using direct bearing %f to Lat: %f Lon: %f Dist: %f" % (self._id, bearing, lat, lon, dist) # Set the waypoint past the current target, so we don't go into a loiter mode self._wp = np.array([lat, lon, tgt_alt]) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height,\ tgt_lat, tgt_lon, tgt_alt): self._action = ["Fire", self._target_id] #print "GS Int=============================>ID: %d shot at Target %d" % (self._id, self._target_id) else: # If a target hasn't been identified, return to the safe waypoint self._wp = self._safe_waypoint self._taken.clear( ) # clear the taken list in case it's full of old data return True
def run_behavior(self): ''' Executes one iteration of the behavior First state is order determination. After the order has been determined intercept waypoints will be generated for each iteration ''' if self._behavior_state == GreedyShooter.HUNT_TGT: self._target_id = self._identify_target() if self._target_id == None: self._behavior_state = GreedyShooter.NO_TGTS else: self._behavior_state = GreedyShooter.TRACK_TGT elif self._behavior_state == GreedyShooter.TRACK_TGT: intercept = self._track_target() if intercept: if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, self._own_pose.pose.pose.position.alt, ( self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height, \ self._target_lat, self._target_lon, \ self._target_alt): self._envelope_counter = 1 self._behavior_state = GreedyShooter.SHOOT_TGT self.log_dbug("Target UAV in firing envelope") else: self.log_info("Lost contact with target") self._behavior_state = GreedyShooter.HUNT_TGT elif self._behavior_state == GreedyShooter.SHOOT_TGT: intercept = self._track_target() if intercept: if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, self._own_pose.pose.pose.position.alt, ( self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._max_range, self._fov_width, self._fov_height, \ self._target_lat, self._target_lon, \ self._target_alt): self._envelope_counter += 1 self._shot_time = rospy.Time.now() if self._envelope_counter >= GreedyShooter.RQD_IN_ENVELOPE: self._make_firing_reports() self._reds_shot.add(self._target_id) self._behavior_state = GreedyShooter.HUNT_TGT else: self.log_dbug("Target UAV out of firing envelope") self._behavior_state = GreedyShooter.TRACK_TGT else: self.log_info("Lost contact with target") self._behavior_state = GreedyShooter.HUNT_TGT elif self._behavior_state == GreedyShooter.NO_TGTS: stdby_wp_cmd = stdmsg.UInt16() stdby_wp_cmd.data = enums.SWARM_STANDBY_WP self._behavior_state = GreedyShooter.TERMINAL_STDBY self._to_wp_publisher.publish(stdby_wp_cmd) self.log_info("No targets available to greedy shooter--standby") elif self._behavior_state == GreedyShooter.TERMINAL_STDBY: pass # Don't actually do anything--just wait else: # shouldn't get here, but just in case self.set_ready_state(False) self.log_warn("Invalid greedy shooter state--disabling")
def step_autonomy(self, t, dt): # Reset the action every loop self._action = ["None", 0] self._target_id = None own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon own_alt = 400 if self._state == States.new_start: self.ordered_corners = self.closestCorners() lat, lon = self.fixed_wing_waypoint(own_lat, own_lon, self.ordered_corners[0][0], self.ordered_corners[0][1]) if gps.gps_distance(own_lat, own_lon, self.ordered_corners[0][0], self.ordered_corners[0][1]) < 50: #print("Starting patrol!") self.ordered_corners = self.closestCorners() self._state = States.patrol self.check_for_supply_route() elif self._state == States.patrol: lat, lon = self.fixed_wing_waypoint( own_lat, own_lon, self.ordered_corners[2][0], self.ordered_corners[2][1]) #At the time of ordering, #3rd closes waypoint is next in path, #1st closest is waypoint that you're currently at, #2nd closest is waypoint you visited previously if gps.gps_distance(own_lat, own_lon, self.ordered_corners[2][0], self.ordered_corners[2][1]) < 25: #print("Reached Waypoint at %f,%f!" % (lat, lon)) self.ordered_corners = self.closestCorners() self.check_for_supply_route() elif self._state == States.kill_supply: lat, lon = self.fixed_wing_waypoint(own_lat, own_lon, self.ordered_corners[1][0], self.ordered_corners[1][1]) if gps.gps_distance(own_lat, own_lon, self.ordered_corners[1][0], self.ordered_corners[1][1]) < 50: #print("Reached supply point at %f,%f!" % (lat, lon)) self.ordered_corners = self.closestCorners() ###################################################################### #If a target is in range, pursue and fire # Reset the action and target ID every loop self._action = ["None", 0] self._target_id = None # Search for the closest target every frame min_dist = self._weapons_range + 30 for uav in self._reds.values(): if uav.vehicle_id not in self._shot: d = gps.gps_distance(self._own_pose.pose.pose.position.lat,\ self._own_pose.pose.pose.position.lon,\ uav.state.pose.pose.position.lat,\ uav.state.pose.pose.position.lon) if d < min_dist: #We also want to constrain our pursuits to enemies in front of us if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w), \ min_dist, self._fov_width*2, self._fov_height, \ uav.state.pose.pose.position.lat, \ uav.state.pose.pose.position.lon, \ uav.state.pose.pose.position.alt): min_dist = d self._target_id = uav.vehicle_id # If a target has been identified, move towards it if self._target_id != None: #print("Target Aquired!") target_pos = self._reds[self._target_id].state.pose.pose.position own_lat = self._own_pose.pose.pose.position.lat own_lon = self._own_pose.pose.pose.position.lon tgt_lat = target_pos.lat tgt_lon = target_pos.lon lat = tgt_lat lon = tgt_lon if self._vehicle_type == enums.AC_FIXED_WING: # When using fixed wing, Set the waypoint past the current # target, so we don't go into a loiter mode bearing = gps.gps_bearing(own_lat, own_lon, tgt_lat, tgt_lon) lat, lon = gps.gps_newpos(own_lat, own_lon, bearing, 1000) # Determine if the target is within firing parameters if gps.hitable(self._own_pose.pose.pose.position.lat, \ self._own_pose.pose.pose.position.lon, \ self._own_pose.pose.pose.position.rel_alt, \ (self._own_pose.pose.pose.orientation.x, \ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w ), \ self._weapons_range, self._fov_width, self._fov_height,\ target_pos.lat, target_pos.lon, target_pos.rel_alt): self._action = ["Fire", self._target_id] ################################################################################# #Safety to make sure you don't fly out of bounds rpy = qmath.quat_to_euler((self._own_pose.pose.pose.orientation.x,\ self._own_pose.pose.pose.orientation.y, \ self._own_pose.pose.pose.orientation.z, \ self._own_pose.pose.pose.orientation.w)) own_bearing = rpy[2] left_lat, left_lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(own_bearing - (math.pi / 2)), 20) right_lat, right_lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(own_bearing + (math.pi / 2)), 20) front_lat, front_lon = gps.gps_newpos( own_lat, own_lon, gps.normalize(own_bearing + (math.pi / 2)), 50) if not self._bboxes[0].contains(left_lat, left_lon, own_alt) or \ not self._bboxes[0].contains(right_lat, right_lon, own_alt) or \ not self._bboxes[0].contains(front_lat, front_lon, own_alt): lat, lon = self.box_center self._wp = np.array([lat, lon, own_alt]) return True
def handle_fire(self, fire, shooter_snap, target_snap, shooter_score, target_score): # Make sure neither shooter nor target are "hit" already # NOTE shooter won't know if target was hit, so might change this if shooter_score.already_hit(): self.log_message(self.LOG_INFO, sfire(fire, 'Shooter already hit')) return if target_score.already_hit(): self.log_message(self.LOG_INFO, sfire(fire, 'Target already hit')) return # Get poses at time of firing for shooter and target shooter_pose = shooter_snap.get_uav_pose_at(fire.id, fire.time) target_pose = target_snap.get_uav_pose_at(fire.target, fire.time) if shooter_pose is None: self.log_message(self.LOG_INFO, sfire(fire, 'No shooter pose')) return if target_pose is None: self.log_message(self.LOG_INFO, sfire(fire, 'No target pose')) return # See if hit occurred hit = gps_utils.hitable(shooter_pose.lat, shooter_pose.lon, shooter_pose.alt, (shooter_pose.q_x, shooter_pose.q_y, shooter_pose.q_z, shooter_pose.q_w), self.__weapon_range, self.__weapon_FOV_w, self.__weapon_FOV_h, target_pose.lat, target_pose.lon, target_pose.alt) if not hit: return # Hit occurred, handle consequences # Report the hit self.log_message(self.LOG_INFO, sfire(fire, 'HIT!')) # Send network message to GCSs, red & blue UAVs, and other modules parser = bitmapped_bytes.UShortParser() parser.value = fire.target msg = acs_messages.SwarmBehaviorData() msg.msg_src = self.state.net_id msg.msg_sub = 0 msg.msg_seq = 0 msg.msg_ack = 0 msg.msg_secs = 0 msg.msg_nsecs = 0 msg.data_type = bitmapped_bytes.HIT_REPORT msg.data = parser.pack() msg.msg_fl_rel = False for i in range(0, 4): # Better odds that it gets through self.state.blue_network.send_message_to(Socket.ID_BCAST_ALL, msg) self.state.red_network.send_message_to(Socket.ID_BCAST_ALL, msg) for module in self.state.modules.values(): module.on_blue_message_received(msg) module.on_red_message_received(msg) # Denote successful hit for shooter shooter_score.record_fire(fire) # Denote that target has been hit target_score.set_hit_time(fire.time)