Пример #1
0
class ChemicalSpillAnnounceAgent(Agent):
    def __init__(self, owner_node, interval, announce_time, rate):
        Agent.__init__(self, owner_node)
        self._interval = interval
        self._announce_time = announce_time
        self._announce_rate = rate
        self._total_time = 0
        self._announced_spill = False
        self._event_api = None
        print "created ChemicalSpillAnnounceAgent"

    ##    def initialize(self):
    ##        print 'Initializing ChemicalSpillAnnounceAgent'
    ##        self._event_api = EventAPI()
    ##        self._event_api.start()

    def run(self):
        ##o_lat, o_lon, o_agl = self.get_owner_node().get_position()

        self._event_api = EventAPI()
        self._event_api.start()

        while not self._announced_spill:
            if self._total_time > self._announce_time:
                # pass owner_uid, location, rate=5
                print "Agent about to publish ChemicalSpillEvent...."
                self._event_api.publish(ChemicalSpillEvent(self.get_owner_node().get_position(), self._announce_rate))
                self._announced_spill = True
            else:
                print "the time has not yet come!"
            time.sleep(self._interval)
            self._total_time += self._interval
Пример #2
0
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)))
Пример #3
0
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
Пример #4
0
    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)
Пример #5
0
    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)
Пример #6
0
    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)
Пример #7
0
 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)
Пример #8
0
    def initialize(self) :
        self._event_api = EventAPI()
        self._event_api.start()

        self._event_api.publish(EntityMoveEvent(self, self._lat, self._long, self._agl, self._forward_velocity, self._velocity))

        self._move_thread = None
        self._stop_move = False
        self._stopped_cond = Condition()

        for sensor in self._sensors.values() :
            Thread(target=sensor._run, args=(self.get_world(),)).start()
Пример #9
0
    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()
Пример #10
0
    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)
Пример #11
0
    def run(self):
        ##o_lat, o_lon, o_agl = self.get_owner_node().get_position()

        self._event_api = EventAPI()
        self._event_api.start()

        while not self._announced_spill:
            if self._total_time > self._announce_time:
                # pass owner_uid, location, rate=5
                print "Agent about to publish ChemicalSpillEvent...."
                self._event_api.publish(ChemicalSpillEvent(self.get_owner_node().get_position(), self._announce_rate))
                self._announced_spill = True
            else:
                print "the time has not yet come!"
            time.sleep(self._interval)
            self._total_time += self._interval
Пример #12
0
    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
Пример #13
0
    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 )
Пример #14
0
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)
Пример #15
0
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()
Пример #16
0
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()
Пример #17
0
 def __init__(self, world_inst) :
     self._event_api = EventAPI()
     self._event_api.start()
     self._startup_acks = set([])
     self._world = world_inst
Пример #18
0
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()
Пример #19
0
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
Пример #20
0
def stop() :
    _event_api = EventAPI()
    _event_api.publish(StopSimulationEvent())
    _event_api.stop()
Пример #21
0
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).
Пример #22
0
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)) 
Пример #23
0
 def run(self):
     print 'Running correlation agent...'
     
     self._event_api = EventAPI()
     self._event_api.start()
     
     self.lock = Lock()
     
     self.get_owner_node().get_interface(self._iface_s1).set_recv_callback(self._new_sonar1_data)
     self.get_owner_node().get_interface(self._iface_s2).set_recv_callback(self._new_sonar2_data)
     self.get_owner_node().get_interface(self._iface_r1).set_recv_callback(self._new_radar1_data)
     self.get_owner_node().get_interface(self._iface_ais).set_recv_callback(self._new_ais_data)
     
     while True:
         
         '''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._sensor_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)'''
         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._sensor_data:
             #print '\nCorrelating ', sensor_pt, '...'
             closest_dist = 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] )
                 
                 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
             
             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 '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
                 
                 
         #print 'final:'
         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))
             
         # update all blank AIS matches
         for ais_id in self._ais_data:
             if not ais_id in self._correlation:
                 ais_pt = self._ais_data[ais_id]
                 self._event_api.publish( CorrelationEvent(ais_pt[0], ais_pt[1], ais_pt[0], ais_pt[1], ais_id))
                 
         self.lock.release()
         
         ''' move sensor data to history, 
             while correlating the most recent sensor data w/ historical sensor data'''
         self._correlate_sensor_history()
         #self._detect_threats()
         
         time.sleep(3)
Пример #24
0
class HistoryCorrelationAgent(Agent):
    
    def __init__(self, uid, threat_dist, ais_threshold, iface_s1, iface_s2, iface_r1, iface_ais, history_num, s_accuracy, threat_step) :
        Agent.__init__(self, uid)
        self._iface_s1 = iface_s1
        self._iface_s2 = iface_s2
        self._iface_r1 = iface_r1
        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 for an AIS and sensor point to be considered the same point
        # this may not actually get used yet
        self._ais_threshold = ais_threshold
        
        
        self._correlation = {}
        self._sensor_data = []
        self._ais_data = {}
        self._radar_data = {}   # keyed by bearing
        self._sonar1_data = {}  # keyed by bearing
        self._sonar2_data = {}  # keyed by bearing
        
        # Number of sensor data points to keep in history per correlated set
        self._history_num = history_num
        # Max distance the sensor data can be apart to be considered the same sensor point
        self._sensor_accuracy = s_accuracy
        # How quickly to change threat level from new data (weight of old threat level vs new). (0,1]
        self._threat_step = threat_step
        
        self._event_api = None
        
        self._sensor_history = []
    
    def run(self):
        print 'Running correlation agent...'
        
        self._event_api = EventAPI()
        self._event_api.start()
        
        self.lock = Lock()
        
        self.get_owner_node().get_interface(self._iface_s1).set_recv_callback(self._new_sonar1_data)
        self.get_owner_node().get_interface(self._iface_s2).set_recv_callback(self._new_sonar2_data)
        self.get_owner_node().get_interface(self._iface_r1).set_recv_callback(self._new_radar1_data)
        self.get_owner_node().get_interface(self._iface_ais).set_recv_callback(self._new_ais_data)
        
        while True:
            
            '''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._sensor_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)'''
            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._sensor_data:
                #print '\nCorrelating ', sensor_pt, '...'
                closest_dist = 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] )
                    
                    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
                
                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 '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
                    
                    
            #print 'final:'
            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))
                
            # update all blank AIS matches
            for ais_id in self._ais_data:
                if not ais_id in self._correlation:
                    ais_pt = self._ais_data[ais_id]
                    self._event_api.publish( CorrelationEvent(ais_pt[0], ais_pt[1], ais_pt[0], ais_pt[1], ais_id))
                    
            self.lock.release()
            
            ''' move sensor data to history, 
                while correlating the most recent sensor data w/ historical sensor data'''
            self._correlate_sensor_history()
            #self._detect_threats()
            
            time.sleep(3)
            
    
    
    def _get_sensor_hist_by_pt(self, pt):
        #self.lock.acquire()
        pt = None
        for info in self._sensor_history:
            if info[1] == pt:
                pt = info
                break
        #self.lock.release()
        return pt
        
    
    def _correlate_sensor_history(self):
        print '\nCorrelating sensor history...'
        self.lock.acquire() 
        
        ''' If there is no most recent sensor data, ignore this '''
        if len(self._sensor_data) == 0:
            print 'No sensor data. Exiting function.'
            self.lock.release()
            return
        
        hist_correlation = {}
        print 'len of sensor data = ', len(self._sensor_data)
        print 'len of history data = ', len(self._sensor_history)
        
        for new_pt in self._sensor_data:
            ''' Try to correlate each new sensor pt to historical points.
                If none are less than self._sensor_accuracy km away,
                consider this a new sensor point'''
            print '\nCorrelating', new_pt
            
            closest_dist = self._sensor_accuracy
            closest_pt = None
            
            for hist in self._sensor_history:
                hist_pt = hist[1]
                
                dist = haver_distance( hist_pt[0], hist_pt[1], new_pt[0], new_pt[1] )
                
                if dist < closest_dist:
                    closest_dist = dist
                    closest_pt = hist_pt
                #else:
                #    print 'point too far away, dist = ', dist
                    
            if closest_pt is None:
                print 'Could not find closest sensor pt history match for ', new_pt, '. Adding it as new entry.'
                self._sensor_history.append([0, new_pt])
                continue
            
            ''' Update hist_correlation'''
            if hist_correlation.has_key( closest_pt ):
                if closest_dist <= hist_correlation[closest_pt][0]:
                    print 'bumping out ', hist_correlation[closest_pt], 'with new data: ', new_pt, ', d = ', closest_dist
                    hist_correlation[closest_pt] = [closest_dist, new_pt]
                    
            else:
                print 'assigning ', closest_pt, ' as correlation. d = ', closest_dist
                hist_correlation[closest_pt] = [closest_dist, new_pt]
        
        ''' Update self._sensor_history with new correlations'''
        ''' This is the part where we tack on new set of sensor points, to their corresponding (correlated)
            historical sensor points.
            We iterate through all these already existing historical sensor data points, to which new ones were correlated to,
            and tack on the new points into the old ones' historical list.
            If a new sensor point was not correlated with a historical point, it was added above.'''
        for correlated_hist_pt in hist_correlation:
            dist, new_hist_pt = hist_correlation[correlated_hist_pt]
            inserted_new_pt = False
            
            for hist_info in self._sensor_history:
                if correlated_hist_pt == hist_info[1]:
                    ''' This is the item (list, technically) in self._sensor_history 
                        where we need to tack on the newly correlated sensor pt'''
                    hist_info.insert(1, new_hist_pt)
                    print 'Just inserted ', new_hist_pt, ' into ', hist_info
                    if len(hist_info) > self._history_num :
                        hist_info.pop(-1)   # the last item is the value of the threat, NOT a historical point
                    inserted_new_pt = True
                    break
            
            if not inserted_new_pt:
                print 'Did not insert', new_hist_pt, ' into correlation history. Problem.'
        
        self.lock.release()
        
        print '\t..finished correlating sensor history.'
            
                
                
        
        
    
    def _detect_threats(self):
        print 'Detecting threats...'
        self.lock.acquire()
        
        print len(self._correlation), ' = length of self._correlation'
        ''' 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 )
        print len(correlated_s_pts), 'correlated sensor points.'
        print len(self._correlation), 'keys in self._correlation'
        
        ''' Go through all sensor points that were reported to agent...'''    
        for sensor_pt in self._sensor_data:
            #print 'analyzing ', sensor_pt
            hist_info = self._get_sensor_hist_by_pt(sensor_pt)
            print hist_info

            if not sensor_pt in correlated_s_pts:
                #if hist_info is not None:
                #    ''' adjust hist_info. check if threat level is high for this sensor point'''
                
                ''' 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]
                    
                    dist = haver_distance( sensor_pt[0], sensor_pt[1], ais_pt[0], ais_pt[1] )
                    
                    if dist <= self._threat_dist:
                        if hist_info is not None:
                            threat_level = hist_info[0]
                            new_threat_level = (self._threat_dist - dist) / self._threat_dist
                            threat_level = self._threat_step*threat_level + (1-self._threat_level)*new_threat_level
                            hist_info[0] = threat_level
                            
                            if threat_level > 0.5:
                                print 'POSSIBLE THREAT at ', sensor_pt
                                self._event_api.publish( ProximityThreatEvent(sensor_pt[0], sensor_pt[1], ais_id))
                            else:
                                print sensor_pt, 'is not quite a threat yet..'
                        else:
                            'No sensor data history for ', sensor_pt
                            if dist <= (0.5*self._threat_dist):
                                print 'POSSIBLE THREAT at ', sensor_pt, '. reporting WITHOUT historical data.'
                                self._event_api.publish( ProximityThreatEvent(sensor_pt[0], sensor_pt[1], ais_id))
            else:
                print '\t\tthis point was correlated.'
                if hist_info is not None:
                    # If point was correlated to AIS data, set its threat level to 0 (it may already be at that amount)
                    hist_info[0] = 0
                    
                
        self.lock.release()
        print 'Done detecting threats.'
            
        
    
    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_sonar1_data(self, event, iface):
        msg = event.get_message().get_payload()
        #print 'CA: new sonar1 data: ', msg
        info = msg.split()
        bearing = float(info[0])
        if not self._sonar1_data.has_key(bearing):
            self._sonar1_data[bearing] = []
        
        self.lock.acquire()
        for entry in info[1:]:      
            lat, lon, snr = entry.split(',')
            lat = float(lat)
            lon = float(lon)
            self._sonar1_data[bearing] = (lat, lon)
        self.lock.release()
        
    def _new_sonar2_data(self, event, iface):
        msg = event.get_message().get_payload()
        #print 'CA: new sonar2 data: ', msg
        info = msg.split()
        bearing = float(info[0])
        if not self._sonar2_data.has_key(bearing):
            self._sonar2_data[bearing] = []
        
        self.lock.acquire()
        for entry in info[1:]:      # will always get something for the given bearing (if no ship, picks up land)
            lat, lon, snr = entry.split(',')
            lat = float(lat)
            lon = float(lon)
            self._sonar2_data[bearing] = (lat, lon)
        self.lock.release()
        
        
    def _new_radar1_data(self, event, iface):
        msg = event.get_message().get_payload()
        node_uid, bearing, dist, lat, lon = msg.split()
        #print 'CA: new radar1 data: ', msg
        
        if lat == "none":
            # There is no data in the current sweep
            self.lock.acquire()
            self._radar_data[bearing] = None
            self.lock.release()
            #print ' r', bearing, 
        else:
            bearing = float(bearing)    # arc sweeps may be arbitrarily small.
            lat = float(lat)
            lon = float(lon)
            
            self.lock.acquire()
            self._radar_data[bearing] = (lat, lon)
            self.lock.release()
            #print 'Just added radar data at bearing ', bearing, '=', (lat, lon)
            
        
        
    def _new_ais_data(self, event, iface):
        msg = event.get_message().get_payload()
        s_id, lat, lon, agl, speed = msg.split(',')
        s_id = int(s_id)
        lat = float(lat)
        lon = float(lon)
        self.lock.acquire()
        self._ais_data[s_id] = (lat, lon)   # ignoring agl and speed for now
        self.lock.release()
        
        #print 'Just added ais data at id ', s_id, '=', (lat, lon)
        
        # check if this AIS ship ID is in correlation{}.  If not, add it, default the distance to 
            
        ##this was for history
        #if s_id in self._ais_data.keys(): 
            #if len(self._ais_data[s_id]) > self._ais_hist:
            #    self._ais_data[s_id].pop(0)
            #    self._ais_data.append([lat, lon, speed])    # ignoring agl
        
    
    def _add_sensor_pt(self, lat, lon):
            
        #for pt_id, pt_lat, pt_lon in self._sensor_data:
        for pt in self._sensor_data:
            ''' See if given lat, lon point is within threshold distance of pt.
                If so, consider them the same point '''
            pass
            
            
    def _update_sensor_data(self):
        self._sensor_data = []
        
        # add radar sensor data
        ## Note: self._radar_data[bearing] = (lat, lon)
        self.lock.acquire()
        
        ''' Add RADAR sensor points to sensor_data list '''
        #print 'length of radar data = ', len(self._radar_data)
        for bearing in self._radar_data:
            loc = self._radar_data[bearing]
            if loc is not None:
                self._sensor_data.append(loc)
        #print 'length of sensor_data = ', len(self._sensor_data)
        
        ''' Add SONAR sensor points to sensor_data list,
            unless they are within self._ais_threshold away from one of the sensor points.
            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) '''
        
        #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:
        #    uniquePoint = True
        #    '''make sure sonar_loc is not within ais_threshold dist from each point'''
        #    for loc in self._sensor_data:
        #        if haver_distance( sonar_loc[0], sonar_loc[1], loc[0], loc[1]) <= self._ais_threshold:
        #            uniquePoint = False
        #            break
        #    if uniquePoint:
        #        self._sensor_data.append(sonar_loc)
        print 'length of self._sensor_data = ', len(self._sensor_data) 
        
        self.lock.release()
Пример #25
0
 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
Пример #26
0
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
Пример #27
0
class Entity :
    MAX_DISTANCE = kilometers(0.01)
    def __init__(self, uid) :
        self._uid = uid
        self._lat = 0
        self._long = 0
        self._agl = 0
        self._params = {}
        self._event_api = None
        self._world = None
        self._velocity = (0, 0, 0)
        self._forward_velocity = 0
        self._bearing = 0

        self._sensors = {}

    def get_parameter(self, param, default=None) :
        if self._params.has_key(param) :
            return self._params[param]
        return default

    def set_parameter(self, param, value) :
        self._params[param] = value

    def add_sensor(self, name, sensor) :
        sensor.set_uid(len(self._sensors))
        self._sensors[name] = sensor
        sensor.set_owner(self)

    def get_sensor(self, name) :
        return self._sensors[name]

    def get_sensors(self):
        return self._sensors

    def get_bearing(self) :
        return math.degrees(self._bearing)

    def get_lin_velocity(self) :
        return self._velocity

    def get_forward_velocity(self) :
        return self._forward_velocity

    def set_lin_velocity(self, v) :
        self._velocity = v

    def set_forward_velocity(self, v) :
        self._forward_velocity = v

    def set_world(self, world) :
        self._world = world

    def get_world(self) :
        return self._world

    def set_position(self, lat, long, agl) :
        self._lat = lat
        self._long = long
        self._agl = agl
        if self._event_api != None :
            self._event_api.publish(EntityMoveEvent(self, lat, long, agl, self._forward_velocity, self._velocity))

    def move(self, lat, lon, agl, forward_vel, vert_vel, block=False) :
        if self._move_thread != None :
            self._stop_move = True
            self._stopped_cond.acquire()
            self._stopped_cond.wait()
            self._stopped_cond.release()
            self._stop_move = False
        if not block :
            self._move_thread = Thread(target=self._iterate_move, args=(lat, lon, agl, forward_vel, vert_vel))
            self._move_thread.start()
        else :
            self._move_thread = self
            self._iterate_move(lat, lon, agl, forward_vel, vert_vel)
        
    def _iterate_move(self, lat, lon, agl, forward_vel, vert_vel) :
        last_tic = time.time() - Entity.MAX_DISTANCE / forward_vel
        self._forward_velocity = forward_vel
        while (lat != self._lat or lon != self._long or self._agl != agl) and not self._stop_move :
            lon1 = math.radians(self._long)
            lon2 = math.radians(lon)
            lat1 = math.radians(self._lat)
            lat2 = math.radians(lat)

            y = math.sin(lon2 - lon1) * math.cos(lat2)
            x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(lon2 - lon1)
            self._bearing = math.atan2(y, x)

            self._velocity = (math.cos(self._bearing) * self._forward_velocity, math.sin(self._bearing) * self._forward_velocity, vert_vel)

            dt = time.time() - last_tic

            d = self._forward_velocity * dt
            R = kilometers(6378.1)
            new_lat = math.degrees(math.asin(math.sin(lat1)*math.cos(d/R) + math.cos(lat1)*math.sin(d/R)*math.cos(self._bearing)))
            new_lon = math.degrees(lon1 + math.atan2(math.sin(self._bearing)*math.sin(d/R)*math.cos(lat1), math.cos(d/R)-math.sin(lat1)*math.sin(new_lat)))
            if self._agl < agl :
                new_agl = min(self._agl + vert_vel * dt, agl)
            elif self._agl > agl :
                new_agl = max(0, self._agl - vert_vel * dt)
            else :
                new_agl = self._agl

            if haver_distance(self._lat, self._long, lat, lon) < d :
                self._velocity = (0, 0, 0)
                self._forward_velocity = 0
                self.set_position(lat, lon, agl)
                self._move_thread = None
                self._stopped_cond.acquire()
                self._stopped_cond.notify()
                self._stopped_cond.release()
                break

            self.set_position(new_lat, new_lon, new_agl)
            #print 'moved to', new_lat, new_lon, new_agl

            last_tic = time.time()
            time.sleep(Entity.MAX_DISTANCE / self._forward_velocity)

        self._move_thread = None
        self._stopped_cond.acquire()
        self._stopped_cond.notify()
        self._stopped_cond.release()

    def get_position(self) :
        return self._lat, self._long, self._agl

    def pickle(self) :
        return serialize(self)

    @staticmethod
    def from_pickle(pickled) :
        return unserialize(pickled)

    def get_event_api(self) :
        return self._event_api

    def get_uid(self) :
        return self._uid

    def initialize(self) :
        self._event_api = EventAPI()
        self._event_api.start()

        self._event_api.publish(EntityMoveEvent(self, self._lat, self._long, self._agl, self._forward_velocity, self._velocity))

        self._move_thread = None
        self._stop_move = False
        self._stopped_cond = Condition()

        for sensor in self._sensors.values() :
            Thread(target=sensor._run, args=(self.get_world(),)).start()

    def run(self) :
        pass