class CommsEngine : def __init__(self) : self._event_api = EventAPI() self._event_api.start() self._network_name = None self._world = None self._event_api.subscribe(CommunicationSendEvent, self._check_is_network) def get_world(self) : return self._world def set_world(self, world) : self._world = world def get_node_from_agent(self, agent_uid) : return self.get_world().get_agent_mapping()[agent_uid] def get_event_api(self) : return self._event_api def get_network_name(self) : return self._network_name def set_network_name(self, network_name) : self._network_name = network_name def _check_is_network(self, event) : if event.get_network() == self._network_name : self._on_send(event) def _on_send(self, event) : pass
class SdtInterface : def __init__(self, ip, port, sdt_port) : # Socket to local SDT instance self._sdt_port = sdt_port self._sdt_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # Socket to remote simulation event channel self._remote_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self._remote_sock.connect((ip, port)) self._event_api = EventAPI(self._remote_sock) t = self._event_api.start() self._event_api.subscribe(LinkEvent, self._on_link) self._event_api.subscribe(EntityMoveEvent, self._on_move) t.join() def _send(self, msg) : print 'To SDT:', msg self._sdt_sock.sendto(msg, ('127.0.0.1', self._sdt_port)) def _on_radar(self, event) : lat, lon, agl = event.get_radar_loc() self._send('node %s position %s,%s,%s' % (event.get_sensor_id(), lon, lat, agl)) def _on_link(self, event) : if event.get_up() : thickness = 3#max(1, int(8 * (1-(event.get_pathloss() / -100)))) self._send('link %s,%s,%s line %s,%s' % (event.get_uid1(), event.get_uid2(), '802.11', 'red', thickness)) else : self._send('delete link,%s,%s,%s' % (event.get_uid1(), event.get_uid2(), '802.11')) def _on_move(self, event) : self._send('node %s position %s,%s,%s' % (event.get_uid(), event.get_long(), event.get_lat(), int(event.get_agl() * 1000)))
class StartupDaemon : def __init__(self, phys_id) : self._event_api = EventAPI() self._event_api.start() self._phys_id = phys_id self._running_pids = set([]) self._running = True def _on_startup(self, event) : self.terminate_all() self._event_api.unsubscribe_all(StartupEvent) self._event_api.subscribe(StartSimulationEvent, self._on_sim_start) self._event_api.publish(AckStartupEvent(self._phys_id)) self._world = event.get_world() def terminate_all(self) : for p in self._running_pids : p.kill() self._running_pids.clear() def _start_entity_process(self, entity) : print 'Starting subprocess for uid %s' % entity.get_uid() p = subprocess.Popen(('python entity.py %s %s' % (entity.pickle(), self._world.pickle())).split(' ')) self._running_pids.add(p) def _on_sim_start(self, event) : local_entities = event.get_mapping()[self._phys_id] for e in local_entities : self._start_entity_process(e) def start(self) : self._event_api.subscribe(StartupEvent, self._on_startup) self._event_api.subscribe(StopSimulationEvent, self._restart) self._event_api.get_thread().join() # while self._running : # pass def _restart(self, event) : self.terminate_all() self._event_api.clear_subscriptions() self._running_pids = set([]) print 'Got stop event. State cleared.' self._event_api.subscribe(StartupEvent, self._on_startup) self._event_api.subscribe(StopSimulationEvent, self._restart) def stop(self) : self._event_api.stop() self.terminate_all() self._running = False
class TcpForward: def __init__(self, port): self._api = EventAPI() self._api.start() self._api.subscribe(All, self._on_event) self._clients = set([]) self._tcp_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self._tcp_server.bind(("", port)) self._tcp_server.listen(1) def start(self): t = Thread(target=self._acceptor) t.start() return t def _acceptor(self): while True: conn, addr = self._tcp_server.accept() self._clients.add(conn) Thread(target=self._listener, args=(conn,)).start() def _listener(self, conn): while True: length = conn.recv(4) if len(length) < 4: return None length = struct.unpack(">L", length)[0] packet = conn.recv(length) while len(packet) < length: packet += conn.recv(length - len(packet)) self._api.push_raw(packet) def _on_event(self, event): discards = set([]) for client in self._clients: try: raw = event.pickle() client.sendall(struct.pack(">L", len(raw))) client.sendall(raw) except: print "error sending:", sys.exc_info()[0] discards.add(client) for discard in discards: self._clients.discard(discard)
class Simulation : def __init__(self, world_inst) : self._event_api = EventAPI() self._event_api.start() self._startup_acks = set([]) self._world = world_inst def get_world(self) : return self._world def _on_ack_startup(self, event) : print 'Got ack startup:', event.get_daemon_id() self._startup_acks.add(event.get_daemon_id()) def start(self, wait) : self._event_api.subscribe(AckStartupEvent, self._on_ack_startup) # Do *NOT* change self._world after this point. It is pickled here and sent to other phy nodes self._event_api.publish(StartupEvent(self._world)) time.sleep(wait) if len(self._startup_acks) > 0 : print 'Got %s startup acks. Starting simulation.' % (len(self._startup_acks),) self._event_api.unsubscribe_all(AckStartupEvent) #TODO: Fix the division entities_per_phy_node = int(len(self._world.get_entities()) / float(len(self._startup_acks))) print ' allocating %s entities per node' % entities_per_phy_node mapping = {} to_allocate = list(self._world.get_entities()) for i, phy in enumerate(list(self._startup_acks)) : alloc = to_allocate[i*entities_per_phy_node:(i+1)*entities_per_phy_node] print i, alloc if len(alloc) == 0 : break mapping[phy] = alloc self._world.initialize() self._event_api.publish(StartSimulationEvent(mapping)) else : print 'Got no startup acks. Quitting...' def stop(self) : self._event_api.publish(StopSimulationEvent()) self._event_api.stop()
class CorrelationAgent(Agent): def __init__(self, uid, threat_dist, max_corr_dist, iface_ais) : Agent.__init__(self, uid) self._iface_ais = iface_ais # max distance from AIS data point to sensor data point, to consider a sensor data point a threat to AIS point self._threat_dist = threat_dist # max distance from an AIS ship and sensor pt, and be correlated to each other self._max_corr_dist = max_corr_dist self._correlation = {} self._all_sonar_data = [] self._ais_data = {} self._radar_data = {} # keyed by bearing self._sonar1_data = {} # keyed by bearing self._sonar2_data = {} # keyed by bearing self._event_api = None self._sensor_history = [] self._old_correlation = {} self._sonar_land_data = {} self._sonar_land_data_is_ready = False self._threats = {} #self._tanker_id = 5 def run(self): print 'Running correlation agent...' self.lock = Lock() self._event_api = EventAPI() self._event_api.subscribe(RadarEvent, self._new_radar1_data) #self._event_api.subscribe(SonarEvent, self._new_sonar_data) self._event_api.start() self.get_owner_node().get_interface(self._iface_ais).set_recv_callback(self._new_ais_data) #while True: # self.refresh() # time.sleep(0.5) def refresh(self): self.lock.acquire() self._correlate() self._detect_threats() self.lock.release() def _correlate(self): #self.lock.acquire() '''recreate list of most recent sensor data, as it may have been updated''' # possibly move this action to functions where individual data points are added self._update_sensor_data() #print '*****************CORRELATING****' #for pt in self._all_sonar_data: # print pt #print '\n\n' #for ais_id in self._ais_data: # print self._ais_data[ais_id], ais_id #print '*************************' #self.lock.acquire() '''clear all correlations for now This way, there will be no chance of having an AIS ship point still correlated to an outdated sensor point (which may no longer even exist)''' # copy old correlation into new # used to check for duplicates. need this b/c correlation needs to start as a cleared dictionary # (later we need to be able to check if all AIS ships were paired with a sensor point, before we try to find closest point to each unpaired AIS ship self._old_correlation.clear() for ais_id in self._correlation: dist, s_pt = self._correlation[ais_id] self._old_correlation[ais_id] = [dist, s_pt] self._correlation.clear() '''for each sensor data item ("current sensor item"), see which AIS data point is closest. If distance from the current sensor item is less than or equal to the most recent smallest distance from a sensor point to this AIS data point, then set update the most recent smallest sensor point to be the current sensor item.''' #for sensor_pt in self._all_sonar_data: ''' ------------------------------------------------------------ We now, at first, ONLY correlate with RADAR sensor points. See below for sonar sensor point correlation ------------------------------------------------------------''' for sensor_pt in self._radar_data.values(): if sensor_pt is None: continue #print '\nCorrelating ', sensor_pt, '...' ''' distance to AIS ship should be negligent ''' closest_dist = self._max_corr_dist #self._ais_threshold #self._ais_threshold #99999 closest_ais_id = None for ais_id in self._ais_data: ais_pt = self._ais_data[ais_id] dist = haver_distance( sensor_pt[0], sensor_pt[1], ais_pt[0], ais_pt[1] ) #print 'dist = ', dist if dist < closest_dist: # Only match if we find an AIS point closer than we have found before closest_dist = dist closest_ais_id = ais_id #print 'closest_ais_id = ', closest_ais_id, 'closest_dist = ', closest_dist #print 'Found a new closest distance: ', closest_dist #print 'closest_ais_id = ', closest_ais_id if closest_ais_id is None: continue ''' Now that we know which AIS data point is closest to this sensor_pt...''' if self._correlation.has_key(closest_ais_id): ''' There was already a sensor point that was matched as closest to this AIS point. Let's see if current sensor point is closer (or equidistant). If so, update the correlation''' if closest_dist <= self._correlation[closest_ais_id][0]: self._correlation[closest_ais_id] = [closest_dist, (sensor_pt)] #print 'OVERWRITE: Paired this sensor pt to AIS ', closest_ais_id, 'dist=', closest_dist, '*' else: ''' No sensor point had yet been paired with this AIS point, and we know this is the closest AIS point we've found to this sensor so far. Update.''' self._correlation[closest_ais_id] = [closest_dist, (sensor_pt)] #print 'Paired this sensor pt to AIS ', closest_ais_id, 'dist=', closest_dist for ais_id in self._correlation: ais_pt = self._ais_data[ais_id] dist, s_pt = self._correlation[ais_id] #print 'Correlated AIS id', ais_id, 'at', self._ais_data[ais_id], 'with', s_pt, '. DIST=', dist #self._event_api.publish( CorrelationEvent(ais_pt[0], ais_pt[1], s_pt[0], s_pt[1], ais_id)) self._publish_correlation(ais_id, ais_pt, dist, s_pt) ''' update all blank AIS matches''' # first, create list of unmatched sensor points available_sensor_pts = [] #for pt in self._all_sonar_data: for pt in self._radar_data.values(): if pt is None: continue if pt not in self._correlation.values(): available_sensor_pts.append(pt) for ais_id in self._ais_data: if not ais_id in self._correlation: #print 'About to correlate AIS ID', ais_id, ', which so far was unmatched.' ais_pt = self._ais_data[ais_id] closest_dist, closest_pt = self._get_closest_pt(ais_pt, available_sensor_pts) if closest_pt is None: #print 'No radar point correlated with AIS ID', ais_id self._correlation[ais_id] = [0, ais_pt] self._publish_correlation(ais_id, ais_pt, 0, ais_pt) else: if closest_dist <= self._max_corr_dist: #print 'Correlating with closest radar pt ', closest_pt, ' dist = ', closest_dist self._correlation[ais_id] = [closest_dist, closest_pt] available_sensor_pts.remove(closest_pt) self._publish_correlation(ais_id, ais_pt, closest_dist, closest_pt) else: #if ais_id == self._tanker_id: # print '\tNo radar point correlated with AIS ID', ais_id, '. closest_d = ', closest_dist self._correlation[ais_id] = [0, ais_pt] self._publish_correlation(ais_id, ais_pt, 0, ais_pt) ''' ------------------------------------------------------ Now, correlate any of the sonar sensor data points ------------------------------------------------------''' available_sonar_pts = self._all_sonar_data[:] for ais_id in self._ais_data: # Need new key so as not to duplicate key in self._correlation for radar point correlations sonar_ais_key = str(ais_id) + 's' ais_pt = self._ais_data[ais_id] closest_dist, closest_pt = self._get_closest_pt(ais_pt, available_sonar_pts) if closest_pt is None: ''' No closest point. Update to publish as correlation with self.''' self._correlation[sonar_ais_key] = [0, ais_pt] self._publish_correlation(ais_id, ais_pt, 0, ais_pt, sonar_ais_key) #print 'No SONAR point correlated with AIS ID', ais_id else: ''' Found a closest point, not necessarily within self._max_corr_dist''' if closest_dist <= self._max_corr_dist: self._correlation[sonar_ais_key] = [closest_dist, closest_pt] available_sonar_pts.remove(closest_pt) self._publish_correlation(ais_id, ais_pt, closest_dist, closest_pt, sonar_ais_key) #print 'Correlating with closest SONAR pt ', closest_pt, ' dist = ', closest_dist else: ''' Closest point is too far away. Publish as correlation with self.''' self._correlation[sonar_ais_key] = [0, ais_pt] self._publish_correlation(ais_id, ais_pt, 0, ais_pt, sonar_ais_key) #print 'No SONAR point correlated with AIS ID', ais_id, '. closest_d = ', closest_dist # self._old_correlation is used later, don't clear yet def _publish_correlation(self, ais_id, ais_pt, dist, s_pt, ais_key=None): if ais_key is None: ais_key = ais_id publish_event = False if ais_key in self._old_correlation: old_dist, old_pt = self._old_correlation[ais_key] if not old_pt == s_pt: publish_event = True else: publish_event = True if publish_event: self._event_api.publish( CorrelationEvent(ais_pt[0], ais_pt[1], s_pt[0], s_pt[1], ais_id)) #if ais_id != self._tanker_id: # return #print 'Correlated AIS id', ais_key, 'at', ais_pt, 'with', s_pt, '. DIST=', dist def _get_closest_pt(self, loc, pts): closest_dist = 99999 closest_pt = None for pt in pts: dist = haver_distance(pt[0], pt[1], loc[0], loc[1]) if dist < closest_dist: closest_dist = dist closest_pt = pt return [closest_dist, closest_pt] def _publish_threat(self, ais_id, sensor_pt, d): if not ais_id in self._threats: self._threats[ais_id] = [] if not sensor_pt in self._threats[ais_id]: #print 'len of old correlation = ', len(self._old_correlation) #print '\nold correlation = ', self._old_correlation ''' Prevent throwing a threat if sensor_pt was the second most recently correlated pt with this AIS ship Do this for sonar and radar pts''' if ais_id in self._old_correlation: if sensor_pt == self._old_correlation[ais_id][1]: #print 'Not throwing threat because this is a previously correlated radar sensor pt. *' return #else: # print 'old hist: ', sensor_pt, '!=', self._old_correlation[ais_id][1] sonar_ais_key = str(ais_id) + 's' if sonar_ais_key in self._old_correlation: if sensor_pt == self._old_correlation[sonar_ais_key][1]: #print 'Not throwing threat because this is a previously correlated sonar sensor pt. *' return # End checking if sensor pt was just previously correlated #if ais_id == self._tanker_id: # print 'DETECTED THREAT at ', sensor_pt, ' to AIS ID', ais_id, 'at', self._ais_data[ais_id], ' d=', d print 'DETECTED THREAT at ', sensor_pt, ' to AIS ID', ais_id, 'at', self._ais_data[ais_id], ' d=', d self._event_api.publish( ProximityThreatEvent(sensor_pt[0], sensor_pt[1], ais_id)) self._threats[ais_id].append(sensor_pt) # DEBUGGING STUFF # Nothing gets modified in the rest of this function #if ais_id != self._tanker_id: # skip all but tanker # return ''' if sensor_pt in self._radar_data.values(): print 'threat was a radar point' else: print 'threat was a sonar point' if not ais_id in self._correlation: print '\tthis AIS ID was not correlated to a radar point.' else: print '\tAIS ID', ais_id, 'was correlated: R ', self._correlation[ais_id] for bearing in self._radar_data: if self._radar_data[bearing] == self._correlation[ais_id][1]: print '\tbearing of coord pt = ', bearing if self._radar_data[bearing] == sensor_pt: print '\tbearing of threat pt = ', bearing sonar_ais_key = str(ais_id) + 's' if not sonar_ais_key in self._correlation: print '\tthis AIS ID was not correlated to a sonar point.' else: print '\tAIS ID', ais_id, 'was correlated: S ', self._correlation[sonar_ais_key] ''' def _detect_threats(self): #self.lock.acquire() ''' Get list of all sensor points that were correlated ''' correlated_s_pts = [] for dist, s_pt in self._correlation.values(): correlated_s_pts.append( s_pt ) ''' Create cumulative list of all sensor data ''' all_sensor_data = self._all_sonar_data[:] all_sensor_data.extend( self._radar_data.values() ) ''' Go through all sensor points that were reported to agent...''' for sensor_pt in all_sensor_data: if sensor_pt is None: continue if not sensor_pt in correlated_s_pts: ''' This sensor_pt was not correlated with an AIS ship. It is a possible threat. Check if the sensor_pt is within self._threat_dist of an AIS ship''' for ais_id in self._ais_data: ais_pt = self._ais_data[ais_id] d = haver_distance( sensor_pt[0], sensor_pt[1], ais_pt[0], ais_pt[1] ) if d <= self._threat_dist: self._publish_threat(ais_id, sensor_pt, d) #self.lock.release() def _pt_in_sonar_land_data(self, pt): if pt in self._sonar_land_data: if self._sonar_land_data[pt] > 4: return True return False def _check_sonar_land_data(self, pt): #if self._pt_in_sonar_land_data(pt): if pt in self._sonar_land_data: # Test if land data is ready if (not self._sonar_land_data_is_ready) and (self._sonar_land_data[pt] > 5): self._sonar_land_data_is_ready = True print 'SONAR LAND DATA IS READY.' #self._sonar_land_data[pt] = self._sonar_land_data[pt] + 1 self._sonar_land_data[pt] += 1 #print 'land pt repeat count = ', self._sonar_land_data[pt] return True else: #print 'new possible land pt:', pt self._sonar_land_data[pt] = 1 # start count at 1 return False def _parse_sonar_data(self, event): msg = event.get_message().get_payload() info = msg.split() bearing = float(info[0]) return bearing, info[1:] # will always get something for the given bearing (if no ship, picks up land) def _new_sonar_data(self, event): sonar_id = event.get_owner_uid() bearing = event.get_bearing() detects = event.get_detects() data = None # what we'll actually store self.lock.acquire() if len(detects) == 3: #print 'Got', len(detects), 'data in bearing', bearing lat, lon, thing = detects data = (lat, lon) elif len(detects) == 1: # # probs got [None]. Anyway, useless info if just 1 item data = None else: print 'got data from sonar: \"',detects,'\" and did not parse' self.lock.release() return self.lock.release() if sonar_id == 901: self._sonar1_data[bearing] = data elif sonar_id == 902: self._sonar2_data[bearing] = data else: print "Sonar node is not ID 901 or 902. Not storing sonar data. sonar node id = ", sonar_id if not data is None: is_land_pt = self._check_sonar_land_data( data ) if not is_land_pt: self.refresh() #else: # print (lat,lon), 'is a land point' def _new_radar1_data(self, event): loc = event.get_target_location() bearing = event.get_bearing() bearing = int( 360.0*bearing / (2*math.pi) ) self.lock.acquire() #if (not self._radar_data.has_key(bearing)) or (not loc is self._radar_data[bearing]): # print 'Just added radar data at bearing ', bearing, '=', loc self._radar_data[bearing] = loc # is either point (lat,lon) or None self.lock.release() self.refresh() def _new_ais_data(self, event, iface): msg = event.get_message().get_payload() #print 'new ais msg: ', msg s_id, lat, lon, agl, speed = msg.split(',') s_id = int(s_id) lat = float(lat) lon = float(lon) #if s_id == 80 or s_id ==81: # print 'new ais msg:', msg self.lock.acquire() #if self._ais_data.has_key(s_id): # dist = haver_distance(lat, lon, self._ais_data[s_id][0], self._ais_data[s_id][1]) # print 'dist between last AIS data = ', dist self._ais_data[s_id] = (lat, lon) # ignoring agl and speed for now self.lock.release() self.refresh() def _update_sensor_data(self): ''' This method adds all SONAR sensor points to self._all_sonar_data. Sonar sensor points are no closest_dist, closest_pt = self._get_closest_pt(ais_pt, available_sensor_pts)t added if they appear to be land points. ''' self._all_sonar_data = [] # add radar sensor data ## Note: self._radar_data[bearing] = (lat, lon) ''' Add SONAR sensor points to sensor_data list. Iterate through all sonars' sensor points combined (which sonar sensor data came from does not matter at this point; it only matters when trying to delete sonar data from any one particular sonar sensor only) ''' if not self._sonar_land_data_is_ready: #print 'sonar data is not ready yet.' return all_sonar_pts = [] all_sonar_pts.extend( self._sonar1_data.values() ) all_sonar_pts.extend( self._sonar2_data.values() ) #print 'length of all_sonar_pts = ', len(all_sonar_pts) for sonar_loc in all_sonar_pts: if sonar_loc is None: continue if self._pt_in_sonar_land_data(sonar_loc) is True: ''' This is a land point''' #print sonar_loc, 'is a land pt' continue self._all_sonar_data.append(sonar_loc) ''' We no longer do this, because we will correlate BOTH a radar and a sonar point to each AIS ship (if it is within range).
class NetworkVisualizer : def __init__(self, ip, port) : self._nodelist = {} self._links = {} self._link_lock = Lock() self._net_map = {} self._phys_machines = {} self._entities = {} self._agents = {} ## self._nodecolor ={'Node':(100,100,255),'RadarSensor2':(255,100,100),'Scripted':(100,100,100)} ## self._radarlist = {} ## self._current_radar_bearing = None ## self._current_radar_loc = None ## dx, dy, ux, uy = 0,0,0,0 ## gotFirst = False self._remote_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self._remote_sock.connect((ip, port)) self._event_api = EventAPI(self._remote_sock) t = self._event_api.start() self._event_api.subscribe(LinkEvent, self._on_link) ##self._event_api.subscribe(EntityMoveEvent, self._on_move) ##self._event_api.subscribe(RadarEvent, self._on_radar) ##self._event_api.subscribe(StartupEvent, self._on_startup) ##self._event_api.subscribe(AckStartupEvent, self._on_ack_startup) self._event_api.subscribe(StartSimulationEvent, self._on_start_sim) self._event_api.subscribe(StopSimulationEvent, self._on_stop_sim) self._event_api.subscribe(CommunicationSendEvent, self._on_send_event) self._event_api.subscribe(CommunicationRecvEvent, self._on_recv_event) def _on_link(self, event): n1 = event.get_uid1() n2 = event.get_uid2() self._link_lock.acquire() self._links[tuple(sorted((n1, n2)))] = event.get_up() self._link_lock.release() ##print 'Got link: ', n1, 'and', n2, ': ', self._links[tuple(sorted((n1,n2)))] #~ #~ def _on_startup(self, event): #~ print 'Got startup' #~ #~ #~ def _on_ack_startup(self, event): #~ print 'Got ack startup' def _on_start_sim(self, event): ''' Got StartSimulationEvent. Calls get_mapping(), which gives the mapping of physical entities to nodes. ''' print 'Got start sim' mapping = event.get_mapping() for phy in mapping: daemon_d = DaemonDraw(phy) ##self._net_map[phy] = {} for entity in mapping[phy]: # entity_d = None if isinstance(entity, Node): entity_d = NodeDraw(entity) for agent_id in entity.get_agent_uids(): #agent = entity.get_agent( agent_id) agent_d = AgentDraw(agent_id) entity_d.add_agent( agent_d ) self._agents[agent_id] = agent_d else: entity_d = EntityDraw(entity) daemon_d.add_entity( entity_d ) self._entities[entity_d._id] = entity_d self._phys_machines[phy] = daemon_d #~ # #~ entity_id = entity.get_uid() #~ #~ ## Add agents to list of this node #~ agent_ids = [] #~ if isinstance(entity, Node): #~ agent_ids = entity.get_agent_uids() #~ print 'Got agent ids: ', agent_ids #~ #~ self._net_map[phy][(entity_id, type(entity))] = agent_ids #~ print 'mapping: ', self._net_map #~ for phy in self._net_map.keys(): #~ print 'self._net_map[',phy,'] = ', #~ for entity_key in self._net_map[phy].keys(): #~ print self._net_map[phy][entity_key] # prints agent_ids, if any def _on_stop_sim(self, event): print 'Got stop sim' sys.exit() def _on_send_event(self, event): print 'Got send event' def _on_recv_event(self, event): print 'Got receive event' ## def draw_nodes(self) : ## for uid in self._nodelist.keys() : ## (x, y), type = self._nodelist[uid] ## #print type ## #self.draw_node((100,255,100), (x,y)) ## self.draw_node((x,y),type) def draw_net(self): num_phys = len(self._net_map.keys()) s_width, s_height = surface.get_size() #width = s_width / num_phys height = 250 width = 150 border = 10 max_per_row = int(s_width/(width+border)) count = 0 for phy in self._phys_machines.keys(): left = (width+border) * (count % max_per_row) top = height * int(count/max_per_row) self._phys_machines[phy].draw(left, top) ##print 'num physical machines: ', len(self._net_map.keys()) #~ left = 10 #~ for phy in self._net_map.keys(): #~ entities = self._net_map[phy].keys() #~ num_entities = len(entities) #~ top_buffer = 0 #~ for entity_id, entity_type in entities: #~ top_buffer += 30 #~ #~ #~ pygame.draw.rect(surface, pygame.Color(0,0,255), pygame.Rect(left, 10, width, 10), 2) #~ left += 20 ##pygame.draw.rect(surface, pygame.Color(192,168,255), pygame.Rect(40,60,20,20), 5) ## def draw_radar(self) : ## if self._current_radar_loc is not None : ## x = int(self._current_radar_loc[0] + 800*math.cos(math.radians(self._current_radar_bearing-90))) ## y = int(self._current_radar_loc[1] + 600*math.sin(math.radians(self._current_radar_bearing-90))) ## pygame.draw.line(surface, (0,255,0), self._current_radar_loc, (x,y), 2) ## for bear in self._radarlist.keys() : ## t_loc = self._radarlist[bear] ## if t_loc is None : ## del self._radarlist[bear] ## else : ## self.draw_blip(self._get_pix(*t_loc)) ## def draw_blip(self, position) : ## x, y = position ## pygame.draw.circle(surface, (0,155,0), (x,y), 6, 0) ## pygame.draw.circle(surface, (100,255,100), (x,y), 4, 0) ## def draw_node(self, position, type) : ## x, y = position ## if self._nodecolor.has_key(type) : ## r,g,b = self._nodecolor[type] ## else : ## r,g,b = (100,100,100) ## ## pygame.draw.circle(surface, (255,255,255), (x,y), 10, 0) ## pygame.draw.circle(surface, (r,g,b), (x,y), 6, 0) ## pygame.draw.circle(surface, (r-100,g-100,b-100), (x,y), 8, 3) def draw_links(self) : self._link_lock.acquire() for link, up in self._links.iteritems() : if up : if self._nodelist.has_key(link[0]) and self._nodelist.has_key(link[1]) : p1 = self._nodelist[link[0]][0] p2 = self._nodelist[link[1]][0] pygame.draw.line(surface, (255, 0, 0), p1, p2, 2) self._link_lock.release()
class ProofOfConcept : def __init__(self, ip, port) : self._nodelist = {} self._links = {} self._correlations = {} self._fields = {} self._cameranodes = {} self._threats = {} self._chemsensors = {} self._chemsensors_detect = {} self._divert_points = [] self._threat_lock = Lock() self._link_lock = Lock() self._correlation_lock = Lock() self._fields_lock = Lock() self._cameranodes_lock = Lock() self._divert_lock = Lock() self._chem_lock = Lock() self._nodecolor ={'Node':(100,100,255),'RadarSensor2':(255,100,100),'Scripted':(100,100,100)} self._radarlist = {} self._sonarlist = {} self._aaron_sucks = {} # Agent lists self._current_radar_bearing = None self._current_radar_loc = None self._current_sonar_bearing= {} self._current_sonar_loc = {} self.tl_lat, self.tl_lon = (40.0140,-75.3321) self.br_lat, self.br_lon = (39.7590,-75.0000) self.d_lat = self.br_lat-self.tl_lat self.d_lon = self.br_lon-self.tl_lon dx, dy, ux, uy = 0,0,0,0 gotFirst = False self._remote_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self._remote_sock.connect((ip, port)) self._event_api = EventAPI(self._remote_sock) t = self._event_api.start() self._event_api.subscribe(StartupEvent, self._on_startup) self._event_api.subscribe(StopSimulationEvent, self._on_shutdown) self._event_api.subscribe(LinkEvent, self._on_link) self._event_api.subscribe(EntityMoveEvent, self._on_move) self._event_api.subscribe(RadarEvent, self._on_radar) self._event_api.subscribe(SonarEvent, self._on_sonar) self._event_api.subscribe(ChemicalSpillEvent, self._on_chemspill) # self._event_api.subscribe(SensorEvent, self._on_sensor) self._event_api.subscribe(CorrelationEvent, self._on_correlation) self._event_api.subscribe(ProximityThreatEvent, self._on_prox_threat) self._event_api.subscribe(CameraEvent, self._on_camera) self._event_api.subscribe(ChemicalDetectEvent, self._on_chem_detect) def _get_pix(self, lat, lon) : global center cx, cy = center try : x = int(((lon-self.tl_lon)/self.d_lon)*4800)+cx y = int(((lat-self.tl_lat)/self.d_lat)*4800)+cy return (x,y) except : return (0,0) def _get_ll(self, x, y) : global center cx, cy = center lon = (((x-cx)/4800.0)*self.d_lon)+self.tl_lon lat = (((y-cy)/4800.0)*self.d_lat)+self.tl_lat return (lat,lon) def _on_startup(self, event) : self._world = event.get_world() entities = self._world.get_entities() for entity in entities : sensors = entity.get_sensors().values() for sensor in sensors: if sensor.__class__.__name__ == 'ChemicalSensor' : uid = sensor.get_owner().get_uid() self._chemsensors[uid] = sensor.get_owner().get_position() def _on_shutdown(self, event) : quit(None, None) def _on_link(self, event) : n1 = event.get_uid1() n2 = event.get_uid2() self._link_lock.acquire() self._links[tuple(sorted((n1, n2)))] = event.get_up() self._link_lock.release() def _on_move(self, event) : uid = event.get_uid() lon = event.get_long() lat = event.get_lat() type = event.get_type() try : agent = map(lambda a : a.__class__.__name__, self._world.get_entity(uid).get_agents().values()) self._aaron_sucks[uid] = agent except AttributeError : print 'Simulation was started before interface' quit(None, None) self._nodelist[uid] = ((lat, lon),type) def _on_sonar(self, event) : uid = event.get_owner_uid() t_loc = event.get_detects() bear = event.get_bearing() self._current_sonar_bearing[uid] = bear self._current_sonar_loc[uid] = self._nodelist[uid][0] self._sonarlist[bear] = t_loc def _on_radar(self, event) : uid = event.get_owner_uid() t_loc = event.get_target_location() bear = round(math.degrees(event.get_bearing())) self._current_radar_bearing = bear self._current_radar_loc = self._nodelist[uid][0] self._radarlist[bear] = t_loc def _on_chemspill(self, event) : print 'Chemical spill happened' loc = event.get_location() #int = event.get_intensity() def _on_chem_detect(self,event) : loc = event.get_location() uid = event.get_owner_uid() self._chem_lock.acquire() self._chemsensors_detect[uid] = loc self._chem_lock.release() print 'Chemical detected at ' , loc def _on_sensor(self, event) : uid = event.get_owner_uid() def _on_correlation(self, event) : p1, p2 = event.get_locations() uid = event.get_ais_uid() self._correlation_lock.acquire() self._correlations[uid] = tuple(sorted((p1, p2))) self._correlation_lock.release() def _on_prox_threat(self, event) : uid = event.get_threatened_uid() loc = event.get_threat_location() self._threat_lock.acquire() self._threats[uid] = 60 self._threat_lock.release() def _on_camera(self, event) : field = event.get_field() cameranodes = event.get_visible() uid = event.get_owner_uid() self._fields_lock.acquire() self._fields[uid] = field self._fields_lock.release() self._cameranodes_lock.acquire() self._cameranodes[uid] = cameranodes self._cameranodes_lock.release() def send_bound(self, uid, dx, dy, ux, uy) : p1 = self._get_ll(dx, dy) p2 = self._get_ll(ux, uy) self._event_api.publish(UAVSurveilArea(uid, p1, p2)) def send_divert(self, points): ll_points = [] for p in points: lat, lon = self._get_ll(p[0], p[1]) ll_points.append([lat,lon]) self._event_api.publish(DivertEvent(ll_points)) def draw_chem_sensors(self) : detected = self._chemsensors_detect.keys() for uid in self._chemsensors.keys(): if (detected.count(uid) == 0): loc = self._chemsensors[uid] self.draw_chem_sensor(loc,(0,255,0)) for uid in detected: loc = self._chemsensors_detect[uid] self.draw_chem_sensor(loc,(255,0,0)) def draw_chem_sensor(self, loc, color): x,y = self._get_pix(loc[0], loc[1]) pygame.draw.circle(surface, color,(x,y),10,0) def draw_nodes(self) : for uid in self._nodelist.keys() : (lat, lon), type = self._nodelist[uid] if 'AISShip' in self._aaron_sucks[uid] : self.draw_node(self._get_pix(lat,lon),type,uid,(100,100,255)) elif 'Tanker' in self._aaron_sucks[uid] : self.draw_node(self._get_pix(lat,lon),type,uid,(100,100,255)) elif 'UAV' in self._aaron_sucks[uid] : self.draw_node(self._get_pix(lat,lon),type,uid,(255,100,100)) elif 'SmallShip' in self._aaron_sucks[uid] : pass elif 'ThreatShip' in self._aaron_sucks[uid] : pass elif 'ChemicalSensor' in self._aaron_sucks[uid] : pass else : self.draw_node(self._get_pix(lat,lon),type,uid) def draw_sonar(self) : global center cx, cy = center for uid in self._current_sonar_loc.keys() : px, py = self._get_pix(*self._current_sonar_loc[uid]) x = int(px + 4800*math.cos(math.radians(self._current_sonar_bearing[uid]-90))) y = int(py + 4800*math.sin(math.radians(self._current_sonar_bearing[uid]-90))) pygame.draw.line(surface, (255,255,0), self._get_pix(*self._current_sonar_loc[uid]), (x,y), 2) for bear in self._sonarlist.keys() : t_loc = self._sonarlist[bear] try: lat, lon, bs = t_loc[0] self.draw_blip(self._get_pix(lat, lon), (255,255,100)) except: if self._sonarlist.has_key(bear) : del self._sonarlist[bear] def draw_radar(self) : global center cx, cy = center if self._current_radar_loc is not None : px, py = self._get_pix(*self._current_radar_loc) x = int(px + 4800*math.cos(math.radians(self._current_radar_bearing-90))) y = int(py + 4800*math.sin(math.radians(self._current_radar_bearing-90))) pygame.draw.line(surface, (0,255,0), self._get_pix(*self._current_radar_loc), (x,y), 2) for bear in self._radarlist.keys() : t_loc = self._radarlist[bear] if t_loc is None : del self._radarlist[bear] else : self.draw_blip(self._get_pix(*t_loc), (100,255,100)) def draw_blip(self, position, color) : x, y = position r, g, b = color pygame.draw.circle(surface, (r-100, g-100, b-100), (x,y), 6, 0) pygame.draw.circle(surface, (r,g,b), (x,y), 4, 0) def draw_node(self, position, type, uid, color=(100,100,100)) : Font = pygame.font.Font(None,20) text = Font.render(','.join(str(n) for n in self._aaron_sucks[uid]) + (' (%s)' % uid),1,(0,0,0)) x, y = position r,g,b = color #if self._nodecolor.has_key(type) : # r,g,b = self._nodecolor[type] #else : # r,g,b = (100,100,100) pygame.draw.circle(surface, (255,255,255), (x,y), 10, 0) pygame.draw.circle(surface, (r,g,b), (x,y), 6, 0) pygame.draw.circle(surface, (r-100,g-100,b-100), (x,y), 8, 3) surface.blit(text,(x+7,y+7)) def draw_correlation(self) : self._correlation_lock.acquire() for uid, correlation in self._correlations.iteritems() : p1 = self._get_pix(*correlation[0]) p2 = self._get_pix(*correlation[1]) num_ais = 6.0 pygame.draw.circle(surface, (255, 255, 255), p1, 13, 3) pygame.draw.circle(surface, (255, 255, 255), p2, 13, 3) pygame.draw.line(surface, (255, 0, 255), p1, p2, 5) self._correlation_lock.release() def draw_links(self) : self._link_lock.acquire() for link, up in self._links.iteritems() : if up : if self._nodelist.has_key(link[0]) and self._nodelist.has_key(link[1]) : p1 = self._get_pix(*self._nodelist[link[0]][0]) p2 = self._get_pix(*self._nodelist[link[1]][0]) pygame.draw.line(surface, (255, 0, 0), p1, p2, 2) self._link_lock.release() def draw_fields(self) : self._fields_lock.acquire() for field in self._fields.values() : field_poly = [] for point in field : field_poly.append(self._get_pix(*point)) pygame.draw.polygon(surface,(0,0,255),field_poly,1) self._fields_lock.release() def draw_cameranodes(self) : self._cameranodes_lock.acquire() for uid in self._cameranodes.keys() : nodes = self._cameranodes[uid] for node in nodes : lat, lon, agl = node x, y = self._get_pix(lat, lon) box_width = 20 pygame.draw.rect(surface,(0,0,255),(x-(box_width/2),y-(box_width/2),box_width,box_width),1) self._cameranodes_lock.release() def draw_threats(self) : self._threat_lock.acquire() for uid in self._threats.keys() : t = self._threats[uid] if t > 0 : loc, type = self._nodelist[uid] pos = self._get_pix(*loc) pygame.draw.circle(surface, (255,0,0), pos, 10, 1) pygame.draw.circle(surface, (255,0,0), pos, 15, 1) pygame.draw.circle(surface, (255,0,0), pos, 20, 1) self._threats[uid] = t-1 else : del self._threats[uid] self._threat_lock.release() def draw_divert(self) : self._divert_lock.acquire() for point in self._divert_points: x, y = point pygame.draw.circle(surface,(255,0,255),(x,y),5) self._divert_lock.release() def draw(self) : self.draw_links() self.draw_nodes() self.draw_radar() #self.draw_correlation() self.draw_fields() self.draw_cameranodes() self.draw_threats() self.draw_chem_sensors() self.draw_divert() self.draw_sonar() def main(self) : probs = {} def redraw(move=(0,0)) : global window_center pygame.display.flip() new = map(operator.sub, window_center, move) surface.blit(image_surface,new) #poc.draw() for p1 in probs.keys(): p2 = probs[p1][0] prob = probs[p1][1] pygame.draw.line(surface, (50, 50, 50), p1, p2, 1) return new pygame.init() pygame.display.set_caption('Operations Center') image_surface = pygame.image.load('map_big.png') signal.signal(signal.SIGINT, quit) redraw() dx, dy, ux, uy = 0,0,0,0 gotFirst = False b1x, b1y, b2x, b2y = 0,0,0,0 boundFirst = False divert = False file = open('AIS_trimmed.dat','r') for line in file: redraw() line = line.rstrip('\n') data = line.split(':') lat1,lon1 = map(float, data[0].split(',')) lat2,lon2 = map(float,data[1].split(',')) prob = float(data[2]) p1 = poc._get_pix(lat1,lon1) p2 = poc._get_pix(lat2,lon2) probs[p1] = [p2, prob] while True: global center global window_center # Quit code for event in pygame.event.get(): if event.type == QUIT: quit(None, None) # Detecting 'shift' keys if pygame.mouse.get_pressed()[0] and pygame.key.get_mods() & KMOD_SHIFT : if not boundFirst : b1x, b1y = pygame.mouse.get_pos() boundFirst = True else : b2x, b2y = pygame.mouse.get_pos() pygame.draw.rect(surface,(0,0,255),(b1x,b1y,b2x-b1x,b2y-b1y),1) elif pygame.mouse.get_pressed()[0] and pygame.key.get_mods() & KMOD_CTRL : x,y = pygame.mouse.get_pos() if(self._divert_points.count([x,y]) == 0): self._divert_lock.acquire() self._divert_points.append([x,y]) self._divert_lock.release() elif pygame.mouse.get_pressed()[2] and pygame.key.get_mods() & KMOD_CTRL : divert = True elif pygame.mouse.get_pressed()[0] : if not gotFirst : dx, dy = pygame.mouse.get_pos() gotFirst = True ux, uy = pygame.mouse.get_pos() center = redraw((dx-ux, dy-uy)) else: if boundFirst : poc.send_bound(1, b1x, b1y, b2x, b2y) boundFirst = False if gotFirst : window_center = redraw((dx-ux, dy-uy)) dx, dy, ux, uy = 0,0,0,0 if divert: poc.send_divert(self._divert_points) #self._divert_points[:] = [] divert = False gotFirst = False redraw((dx-ux, dy-uy))
class KmlServer : def __init__(self, ip, port, offer_port) : self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self._sock.bind(('', offer_port)) self._sock.listen(1) self._remote_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self._remote_sock.connect((ip, port)) self._event_api = EventAPI(self._remote_sock) self._event_api.subscribe(EntityMoveEvent, self._on_move) self._event_api.subscribe(LinkEvent, self._on_link) self._pos = {} self._model_map = {} self._links = {} self._last_links = set([]) self._first = True def add_model(self, uid, model, **kwds) : if not kwds.has_key('scale') : kwds['scale'] = 1 if not kwds.has_key('heading') : kwds['heading'] = 0 if not kwds.has_key('tilt') : kwds['tilt'] = 0 if not kwds.has_key('roll') : kwds['roll'] = 0 self._model_map[uid] = { 'model' : model, 'args' : kwds } def _on_move(self, event) : if self._pos.has_key(event.get_uid()) : last = self._pos[event.get_uid()][0:3] else : last = (0, 0, 0) self._pos[event.get_uid()] = (event.get_lat(), event.get_long(), event.get_agl() * 1000, last) def _on_link(self, event) : key = tuple(sorted([event.get_uid1(), event.get_uid2()])) if event.get_up() : self._links[key] = True else : if self._links.has_key(key) : del self._links[key] def _get_contents(self) : s = '' for uid, loc in self._pos.iteritems() : lat, long, agl, last = loc if self._model_map.has_key(uid) : model = self._model_map[uid]['model'] args = self._model_map[uid]['args'] bearing = bearing_from_pts(lat, long, last[0], last[1]) - args['heading'] if self._first : s += '''<Placemark> <Model> <altitudeMode>absolute</altitudeMode> <Location id="%s"> <longitude>%s</longitude> <latitude>%s</latitude> <altitude>%s</altitude> </Location> <Orientation id="o%s"> <heading>%s</heading> <tilt>%s</tilt> <roll>%s</roll> </Orientation> <Scale> <x>%s</x> <y>%s</y> <z>%s</z> </Scale> <Link> <href>%s</href> </Link> </Model> </Placemark>\n''' % (uid, long, lat, agl, uid, args['heading'], args['tilt'], args['roll'], args['scale'], args['scale'], args['scale'], model) else : s += ''' <Update> <Change> <Location targetId="%s"> <longitude>%s</longitude> <latitude>%s</latitude> <altitude>%s</altitude> </Location> </Change> </Update> <Update> <Change> <Orientation targetId="o%s"> <heading>%s</heading> </Orientation> </Change> </Update> \n''' % (uid, long, lat, agl, uid, bearing) else : if self._first : s += ''' <Style id="icon"> <IconStyle> <Icon> <href>file:///Users/arosenfeld/ahoy/trunk/src/proto2/ahoy/viz/icon.png</href> </Icon> </IconStyle> </Style> <Placemark> <styleUrl>#icon</styleUrl> <Point id="%s"> <name>%s</name> <altitudeMode>absolute</altitudeMode> <coordinates>%s,%s,%s</coordinates> </Point> </Placemark>\n''' % (uid, uid, long, lat, agl) else : s += ''' <Update> <Change> <Point targetId="%s"> <coordinates>%s,%s,%s</coordinates> </Point> </Change> </Update> \n''' % (uid, long, lat, agl) for link in self._last_links : # if link not in self._links : print 'delete', link s += ''' <Update> <Delete> <Placemark targetId="l%sto%s"></Placemark> </Delete> </Update> ''' % (link[0], link[1]) self._last_links = set([]) for id, up in self._links.iteritems() : if self._pos.has_key(id[0]) and self._pos.has_key(id[1]) : print 'add', id lat1, lon1, agl1 = self._pos[id[0]][0:3] lat2, lon2, agl2 = self._pos[id[1]][0:3] s += ''' <Update> <Create> <Document targetId="main"> <Placemark id="l%sto%s"> <LineString> <extrude>0</extrude> <tessellate>1</tessellate> <altitudeMode>absolute</altitudeMode> <coordinates> %s,%s,%s %s,%s,%s </coordinates> </LineString> </Placemark> </Document> </Create> </Update> \n''' % (id[0], id[1], lon1, lat1, agl1, lon2, lat2, agl2) self._last_links.add(id) if self._first : self._first = False s = '<Document id="main">\n' + s + '</Document>\n' print s + '\n-----------------------------' else : s = '<NetworkLinkControl>\n' + s + '</NetworkLinkControl>\n' return s def _handle(self, conn) : data = 'HTTP/1.1 200 OK\n' data += 'Content-Type: application/vnd.google-earth.kml+xml\n\n' contents = self._get_contents() if contents != None : data += '<?xml version="1.0" encoding="UTF-8"?>\n' data += '<kml xmlns="http://www.opengis.net/kml/2.2">\n' data += contents data += '</kml>' conn.send(data) conn.close() def start(self) : self._event_api.start() while True : conn, addr = self._sock.accept() Thread(target=self._handle, args=(conn,)).start()
class ChemicalSensor(Sensor) : def __init__(self, interval, **kwds) : Sensor.__init__(self, **kwds) self._interval = interval self._spill_occurred = False self._spill_event = None self._event_api = None ## def initialize(self): ## self._event_api = EventAPI() ## self._event_api.start() ## self._event_api.subscribe(ChemicalSpillEvent, self._on_spill) def run(self) : self._event_api = EventAPI() self._event_api.start() self._event_api.subscribe(ChemicalSpillEvent, self._on_spill) while True : # If sensors aren't listening/publishing directly to API, # how is ChemicalSensor to know that a chemical spill has occurred? # for now, just assuming owner will tell chemical sensor this info through _on_spill() below if self._spill_occurred: lat, lon, agl = self.get_owner().get_position() # spill location spill_lat, spill_lon, spill_agl = self._spill_event.get_location() # if spill location is by latitude above the spill, just ignore the spill. # This is for AHOY team's demo purposes only, as spill will only travel south. if spill_lat < lat: self._spill_occurred = False self._spill_event = None else: # get distance in km distance = haver_distance( lat, lon, spill_lat, spill_lon ) print 'node ', self.get_owner().get_uid(), 'distance from spill = ', distance # for now, assuming spill spreads at a constant rate (does not slow down) time_to_reach_sensor = distance / self._spill_event.get_rate() print 'Sensor at ', self.get_owner().get_uid(), ' will go off at ', time_to_reach_sensor # wait until chemical spill would have reached self, then publish ChemicalDetectEvent time.sleep(time_to_reach_sensor) print self.get_owner().get_uid(), ' about to publish ChemicalDetectEvent..' self._publish_data( ChemicalDetectEvent(self.get_owner().get_uid(), self.get_owner().get_position() ) ) print self.get_owner().get_uid(), ' published ChemicalDetectEvent' # clear spill event data self._spill_occurred = False self._spill_event = None time.sleep( self._interval ) def _on_spill( self, event ): print self.get_owner().get_uid(), 'got spill event' self._spill_occurred = True self._spill_event = event
import time from ahoy.eventapi import EventAPI from ahoy.events.all import All start = time.time() def _on_event(event) : print time.time() - start, event.__class__.__name__ api = EventAPI() api.subscribe(All, _on_event) api.start() while True : pass