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
class SdtInterface :
def __init__(self, ip, port, sdt_port) :
# Socket to local SDT instance
self._sdt_port = sdt_port
self._sdt_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# Socket to remote simulation event channel
self._remote_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._remote_sock.connect((ip, port))
self._event_api = EventAPI(self._remote_sock)
t = self._event_api.start()
self._event_api.subscribe(LinkEvent, self._on_link)
self._event_api.subscribe(EntityMoveEvent, self._on_move)
t.join()
def _send(self, msg) :
print 'To SDT:', msg
self._sdt_sock.sendto(msg, ('127.0.0.1', self._sdt_port))
def _on_radar(self, event) :
lat, lon, agl = event.get_radar_loc()
self._send('node %s position %s,%s,%s' % (event.get_sensor_id(), lon, lat, agl))
def _on_link(self, event) :
if event.get_up() :
thickness = 3#max(1, int(8 * (1-(event.get_pathloss() / -100))))
self._send('link %s,%s,%s line %s,%s' % (event.get_uid1(), event.get_uid2(), '802.11', 'red', thickness))
else :
self._send('delete link,%s,%s,%s' % (event.get_uid1(), event.get_uid2(), '802.11'))
def _on_move(self, event) :
self._send('node %s position %s,%s,%s' % (event.get_uid(), event.get_long(), event.get_lat(), int(event.get_agl() * 1000)))
class 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
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 __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 __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 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)
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 __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 __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 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
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 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 )
class TcpForward:
def __init__(self, port):
self._api = EventAPI()
self._api.start()
self._api.subscribe(All, self._on_event)
self._clients = set([])
self._tcp_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._tcp_server.bind(("", port))
self._tcp_server.listen(1)
def start(self):
t = Thread(target=self._acceptor)
t.start()
return t
def _acceptor(self):
while True:
conn, addr = self._tcp_server.accept()
self._clients.add(conn)
Thread(target=self._listener, args=(conn,)).start()
def _listener(self, conn):
while True:
length = conn.recv(4)
if len(length) < 4:
return None
length = struct.unpack(">L", length)[0]
packet = conn.recv(length)
while len(packet) < length:
packet += conn.recv(length - len(packet))
self._api.push_raw(packet)
def _on_event(self, event):
discards = set([])
for client in self._clients:
try:
raw = event.pickle()
client.sendall(struct.pack(">L", len(raw)))
client.sendall(raw)
except:
print "error sending:", sys.exc_info()[0]
discards.add(client)
for discard in discards:
self._clients.discard(discard)
class KmlServer :
def __init__(self, ip, port, offer_port) :
self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._sock.bind(('', offer_port))
self._sock.listen(1)
self._remote_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._remote_sock.connect((ip, port))
self._event_api = EventAPI(self._remote_sock)
self._event_api.subscribe(EntityMoveEvent, self._on_move)
self._event_api.subscribe(LinkEvent, self._on_link)
self._pos = {}
self._model_map = {}
self._links = {}
self._last_links = set([])
self._first = True
def add_model(self, uid, model, **kwds) :
if not kwds.has_key('scale') :
kwds['scale'] = 1
if not kwds.has_key('heading') :
kwds['heading'] = 0
if not kwds.has_key('tilt') :
kwds['tilt'] = 0
if not kwds.has_key('roll') :
kwds['roll'] = 0
self._model_map[uid] = { 'model' : model, 'args' : kwds }
def _on_move(self, event) :
if self._pos.has_key(event.get_uid()) :
last = self._pos[event.get_uid()][0:3]
else :
last = (0, 0, 0)
self._pos[event.get_uid()] = (event.get_lat(), event.get_long(), event.get_agl() * 1000, last)
def _on_link(self, event) :
key = tuple(sorted([event.get_uid1(), event.get_uid2()]))
if event.get_up() :
self._links[key] = True
else :
if self._links.has_key(key) :
del self._links[key]
def _get_contents(self) :
s = ''
for uid, loc in self._pos.iteritems() :
lat, long, agl, last = loc
if self._model_map.has_key(uid) :
model = self._model_map[uid]['model']
args = self._model_map[uid]['args']
bearing = bearing_from_pts(lat, long, last[0], last[1]) - args['heading']
if self._first :
s += '''<Placemark>
<Model>
<altitudeMode>absolute</altitudeMode>
<Location id="%s">
<longitude>%s</longitude>
<latitude>%s</latitude>
<altitude>%s</altitude>
</Location>
<Orientation id="o%s">
<heading>%s</heading>
<tilt>%s</tilt>
<roll>%s</roll>
</Orientation>
<Scale>
<x>%s</x>
<y>%s</y>
<z>%s</z>
</Scale>
<Link>
<href>%s</href>
</Link>
</Model>
</Placemark>\n''' % (uid, long, lat, agl, uid, args['heading'], args['tilt'], args['roll'], args['scale'], args['scale'], args['scale'], model)
else :
s += '''
<Update>
<Change>
<Location targetId="%s">
<longitude>%s</longitude>
<latitude>%s</latitude>
<altitude>%s</altitude>
</Location>
</Change>
</Update>
<Update>
<Change>
<Orientation targetId="o%s">
<heading>%s</heading>
</Orientation>
</Change>
</Update>
\n''' % (uid, long, lat, agl, uid, bearing)
else :
if self._first :
s += '''
<Style id="icon">
<IconStyle>
<Icon>
<href>file:///Users/arosenfeld/ahoy/trunk/src/proto2/ahoy/viz/icon.png</href>
</Icon>
</IconStyle>
</Style>
<Placemark>
<styleUrl>#icon</styleUrl>
<Point id="%s">
<name>%s</name>
<altitudeMode>absolute</altitudeMode>
<coordinates>%s,%s,%s</coordinates>
</Point>
</Placemark>\n''' % (uid, uid, long, lat, agl)
else :
s += '''
<Update>
<Change>
<Point targetId="%s">
<coordinates>%s,%s,%s</coordinates>
</Point>
</Change>
</Update>
\n''' % (uid, long, lat, agl)
for link in self._last_links :
# if link not in self._links :
print 'delete', link
s += '''
<Update>
<Delete>
<Placemark targetId="l%sto%s"></Placemark>
</Delete>
</Update>
''' % (link[0], link[1])
self._last_links = set([])
for id, up in self._links.iteritems() :
if self._pos.has_key(id[0]) and self._pos.has_key(id[1]) :
print 'add', id
lat1, lon1, agl1 = self._pos[id[0]][0:3]
lat2, lon2, agl2 = self._pos[id[1]][0:3]
s += '''
<Update>
<Create>
<Document targetId="main">
<Placemark id="l%sto%s">
<LineString>
<extrude>0</extrude>
<tessellate>1</tessellate>
<altitudeMode>absolute</altitudeMode>
<coordinates> %s,%s,%s
%s,%s,%s
</coordinates>
</LineString>
</Placemark>
</Document>
</Create>
</Update>
\n''' % (id[0], id[1], lon1, lat1, agl1, lon2, lat2, agl2)
self._last_links.add(id)
if self._first :
self._first = False
s = '<Document id="main">\n' + s + '</Document>\n'
print s + '\n-----------------------------'
else :
s = '<NetworkLinkControl>\n' + s + '</NetworkLinkControl>\n'
return s
def _handle(self, conn) :
data = 'HTTP/1.1 200 OK\n'
data += 'Content-Type: application/vnd.google-earth.kml+xml\n\n'
contents = self._get_contents()
if contents != None :
data += '<?xml version="1.0" encoding="UTF-8"?>\n'
data += '<kml xmlns="http://www.opengis.net/kml/2.2">\n'
data += contents
data += '</kml>'
conn.send(data)
conn.close()
def start(self) :
self._event_api.start()
while True :
conn, addr = self._sock.accept()
Thread(target=self._handle, args=(conn,)).start()
class 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()
def __init__(self, world_inst) :
self._event_api = EventAPI()
self._event_api.start()
self._startup_acks = set([])
self._world = world_inst
class Simulation :
def __init__(self, world_inst) :
self._event_api = EventAPI()
self._event_api.start()
self._startup_acks = set([])
self._world = world_inst
def get_world(self) :
return self._world
def _on_ack_startup(self, event) :
print 'Got ack startup:', event.get_daemon_id()
self._startup_acks.add(event.get_daemon_id())
def start(self, wait) :
self._event_api.subscribe(AckStartupEvent, self._on_ack_startup)
# Do *NOT* change self._world after this point. It is pickled here and sent to other phy nodes
self._event_api.publish(StartupEvent(self._world))
time.sleep(wait)
if len(self._startup_acks) > 0 :
print 'Got %s startup acks. Starting simulation.' % (len(self._startup_acks),)
self._event_api.unsubscribe_all(AckStartupEvent)
#TODO: Fix the division
entities_per_phy_node = int(len(self._world.get_entities()) / float(len(self._startup_acks)))
print ' allocating %s entities per node' % entities_per_phy_node
mapping = {}
to_allocate = list(self._world.get_entities())
for i, phy in enumerate(list(self._startup_acks)) :
alloc = to_allocate[i*entities_per_phy_node:(i+1)*entities_per_phy_node]
print i, alloc
if len(alloc) == 0 :
break
mapping[phy] = alloc
self._world.initialize()
self._event_api.publish(StartSimulationEvent(mapping))
else :
print 'Got no startup acks. Quitting...'
def stop(self) :
self._event_api.publish(StopSimulationEvent())
self._event_api.stop()
class 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
def stop() :
_event_api = EventAPI()
_event_api.publish(StopSimulationEvent())
_event_api.stop()
class CorrelationAgent(Agent):
def __init__(self, uid, threat_dist, max_corr_dist, iface_ais) :
Agent.__init__(self, uid)
self._iface_ais = iface_ais
# max distance from AIS data point to sensor data point, to consider a sensor data point a threat to AIS point
self._threat_dist = threat_dist
# max distance from an AIS ship and sensor pt, and be correlated to each other
self._max_corr_dist = max_corr_dist
self._correlation = {}
self._all_sonar_data = []
self._ais_data = {}
self._radar_data = {} # keyed by bearing
self._sonar1_data = {} # keyed by bearing
self._sonar2_data = {} # keyed by bearing
self._event_api = None
self._sensor_history = []
self._old_correlation = {}
self._sonar_land_data = {}
self._sonar_land_data_is_ready = False
self._threats = {}
#self._tanker_id = 5
def run(self):
print 'Running correlation agent...'
self.lock = Lock()
self._event_api = EventAPI()
self._event_api.subscribe(RadarEvent, self._new_radar1_data)
#self._event_api.subscribe(SonarEvent, self._new_sonar_data)
self._event_api.start()
self.get_owner_node().get_interface(self._iface_ais).set_recv_callback(self._new_ais_data)
#while True:
# self.refresh()
# time.sleep(0.5)
def refresh(self):
self.lock.acquire()
self._correlate()
self._detect_threats()
self.lock.release()
def _correlate(self):
#self.lock.acquire()
'''recreate list of most recent sensor data, as it may have been updated'''
# possibly move this action to functions where individual data points are added
self._update_sensor_data()
#print '*****************CORRELATING****'
#for pt in self._all_sonar_data:
# print pt
#print '\n\n'
#for ais_id in self._ais_data:
# print self._ais_data[ais_id], ais_id
#print '*************************'
#self.lock.acquire()
'''clear all correlations for now
This way, there will be no chance of having an AIS ship point still correlated to an outdated
sensor point (which may no longer even exist)'''
# copy old correlation into new
# used to check for duplicates. need this b/c correlation needs to start as a cleared dictionary
# (later we need to be able to check if all AIS ships were paired with a sensor point, before we try to find closest point to each unpaired AIS ship
self._old_correlation.clear()
for ais_id in self._correlation:
dist, s_pt = self._correlation[ais_id]
self._old_correlation[ais_id] = [dist, s_pt]
self._correlation.clear()
'''for each sensor data item ("current sensor item"), see which AIS data point is closest.
If distance from the current sensor item is less than or equal to
the most recent smallest distance from a sensor point to this AIS data point,
then set update the most recent smallest sensor point to be the current sensor item.'''
#for sensor_pt in self._all_sonar_data:
''' ------------------------------------------------------------
We now, at first, ONLY correlate with RADAR sensor points.
See below for sonar sensor point correlation
------------------------------------------------------------'''
for sensor_pt in self._radar_data.values():
if sensor_pt is None:
continue
#print '\nCorrelating ', sensor_pt, '...'
''' distance to AIS ship should be negligent '''
closest_dist = self._max_corr_dist #self._ais_threshold #self._ais_threshold #99999
closest_ais_id = None
for ais_id in self._ais_data:
ais_pt = self._ais_data[ais_id]
dist = haver_distance( sensor_pt[0], sensor_pt[1], ais_pt[0], ais_pt[1] )
#print 'dist = ', dist
if dist < closest_dist:
# Only match if we find an AIS point closer than we have found before
closest_dist = dist
closest_ais_id = ais_id
#print 'closest_ais_id = ', closest_ais_id, 'closest_dist = ', closest_dist
#print 'Found a new closest distance: ', closest_dist
#print 'closest_ais_id = ', closest_ais_id
if closest_ais_id is None:
continue
''' Now that we know which AIS data point is closest to this sensor_pt...'''
if self._correlation.has_key(closest_ais_id):
''' There was already a sensor point that was matched as closest to this AIS point.
Let's see if current sensor point is closer (or equidistant). If so, update the correlation'''
if closest_dist <= self._correlation[closest_ais_id][0]:
self._correlation[closest_ais_id] = [closest_dist, (sensor_pt)]
#print 'OVERWRITE: Paired this sensor pt to AIS ', closest_ais_id, 'dist=', closest_dist, '*'
else:
''' No sensor point had yet been paired with this AIS point,
and we know this is the closest AIS point we've found to this sensor so far. Update.'''
self._correlation[closest_ais_id] = [closest_dist, (sensor_pt)]
#print 'Paired this sensor pt to AIS ', closest_ais_id, 'dist=', closest_dist
for ais_id in self._correlation:
ais_pt = self._ais_data[ais_id]
dist, s_pt = self._correlation[ais_id]
#print 'Correlated AIS id', ais_id, 'at', self._ais_data[ais_id], 'with', s_pt, '. DIST=', dist
#self._event_api.publish( CorrelationEvent(ais_pt[0], ais_pt[1], s_pt[0], s_pt[1], ais_id))
self._publish_correlation(ais_id, ais_pt, dist, s_pt)
''' update all blank AIS matches'''
# first, create list of unmatched sensor points
available_sensor_pts = []
#for pt in self._all_sonar_data:
for pt in self._radar_data.values():
if pt is None:
continue
if pt not in self._correlation.values():
available_sensor_pts.append(pt)
for ais_id in self._ais_data:
if not ais_id in self._correlation:
#print 'About to correlate AIS ID', ais_id, ', which so far was unmatched.'
ais_pt = self._ais_data[ais_id]
closest_dist, closest_pt = self._get_closest_pt(ais_pt, available_sensor_pts)
if closest_pt is None:
#print 'No radar point correlated with AIS ID', ais_id
self._correlation[ais_id] = [0, ais_pt]
self._publish_correlation(ais_id, ais_pt, 0, ais_pt)
else:
if closest_dist <= self._max_corr_dist:
#print 'Correlating with closest radar pt ', closest_pt, ' dist = ', closest_dist
self._correlation[ais_id] = [closest_dist, closest_pt]
available_sensor_pts.remove(closest_pt)
self._publish_correlation(ais_id, ais_pt, closest_dist, closest_pt)
else:
#if ais_id == self._tanker_id:
# print '\tNo radar point correlated with AIS ID', ais_id, '. closest_d = ', closest_dist
self._correlation[ais_id] = [0, ais_pt]
self._publish_correlation(ais_id, ais_pt, 0, ais_pt)
''' ------------------------------------------------------
Now, correlate any of the sonar sensor data points
------------------------------------------------------'''
available_sonar_pts = self._all_sonar_data[:]
for ais_id in self._ais_data:
# Need new key so as not to duplicate key in self._correlation for radar point correlations
sonar_ais_key = str(ais_id) + 's'
ais_pt = self._ais_data[ais_id]
closest_dist, closest_pt = self._get_closest_pt(ais_pt, available_sonar_pts)
if closest_pt is None:
''' No closest point. Update to publish as correlation with self.'''
self._correlation[sonar_ais_key] = [0, ais_pt]
self._publish_correlation(ais_id, ais_pt, 0, ais_pt, sonar_ais_key)
#print 'No SONAR point correlated with AIS ID', ais_id
else:
''' Found a closest point, not necessarily within self._max_corr_dist'''
if closest_dist <= self._max_corr_dist:
self._correlation[sonar_ais_key] = [closest_dist, closest_pt]
available_sonar_pts.remove(closest_pt)
self._publish_correlation(ais_id, ais_pt, closest_dist, closest_pt, sonar_ais_key)
#print 'Correlating with closest SONAR pt ', closest_pt, ' dist = ', closest_dist
else:
''' Closest point is too far away. Publish as correlation with self.'''
self._correlation[sonar_ais_key] = [0, ais_pt]
self._publish_correlation(ais_id, ais_pt, 0, ais_pt, sonar_ais_key)
#print 'No SONAR point correlated with AIS ID', ais_id, '. closest_d = ', closest_dist
# self._old_correlation is used later, don't clear yet
def _publish_correlation(self, ais_id, ais_pt, dist, s_pt, ais_key=None):
if ais_key is None:
ais_key = ais_id
publish_event = False
if ais_key in self._old_correlation:
old_dist, old_pt = self._old_correlation[ais_key]
if not old_pt == s_pt:
publish_event = True
else:
publish_event = True
if publish_event:
self._event_api.publish( CorrelationEvent(ais_pt[0], ais_pt[1], s_pt[0], s_pt[1], ais_id))
#if ais_id != self._tanker_id:
# return
#print 'Correlated AIS id', ais_key, 'at', ais_pt, 'with', s_pt, '. DIST=', dist
def _get_closest_pt(self, loc, pts):
closest_dist = 99999
closest_pt = None
for pt in pts:
dist = haver_distance(pt[0], pt[1], loc[0], loc[1])
if dist < closest_dist:
closest_dist = dist
closest_pt = pt
return [closest_dist, closest_pt]
def _publish_threat(self, ais_id, sensor_pt, d):
if not ais_id in self._threats:
self._threats[ais_id] = []
if not sensor_pt in self._threats[ais_id]:
#print 'len of old correlation = ', len(self._old_correlation)
#print '\nold correlation = ', self._old_correlation
''' Prevent throwing a threat if sensor_pt was the second most recently correlated pt with this AIS ship
Do this for sonar and radar pts'''
if ais_id in self._old_correlation:
if sensor_pt == self._old_correlation[ais_id][1]:
#print 'Not throwing threat because this is a previously correlated radar sensor pt. *'
return
#else:
# print 'old hist: ', sensor_pt, '!=', self._old_correlation[ais_id][1]
sonar_ais_key = str(ais_id) + 's'
if sonar_ais_key in self._old_correlation:
if sensor_pt == self._old_correlation[sonar_ais_key][1]:
#print 'Not throwing threat because this is a previously correlated sonar sensor pt. *'
return
# End checking if sensor pt was just previously correlated
#if ais_id == self._tanker_id:
# print 'DETECTED THREAT at ', sensor_pt, ' to AIS ID', ais_id, 'at', self._ais_data[ais_id], ' d=', d
print 'DETECTED THREAT at ', sensor_pt, ' to AIS ID', ais_id, 'at', self._ais_data[ais_id], ' d=', d
self._event_api.publish( ProximityThreatEvent(sensor_pt[0], sensor_pt[1], ais_id))
self._threats[ais_id].append(sensor_pt)
# DEBUGGING STUFF
# Nothing gets modified in the rest of this function
#if ais_id != self._tanker_id: # skip all but tanker
# return
'''
if sensor_pt in self._radar_data.values():
print 'threat was a radar point'
else:
print 'threat was a sonar point'
if not ais_id in self._correlation:
print '\tthis AIS ID was not correlated to a radar point.'
else:
print '\tAIS ID', ais_id, 'was correlated: R ', self._correlation[ais_id]
for bearing in self._radar_data:
if self._radar_data[bearing] == self._correlation[ais_id][1]:
print '\tbearing of coord pt = ', bearing
if self._radar_data[bearing] == sensor_pt:
print '\tbearing of threat pt = ', bearing
sonar_ais_key = str(ais_id) + 's'
if not sonar_ais_key in self._correlation:
print '\tthis AIS ID was not correlated to a sonar point.'
else:
print '\tAIS ID', ais_id, 'was correlated: S ', self._correlation[sonar_ais_key]
'''
def _detect_threats(self):
#self.lock.acquire()
''' Get list of all sensor points that were correlated '''
correlated_s_pts = []
for dist, s_pt in self._correlation.values():
correlated_s_pts.append( s_pt )
''' Create cumulative list of all sensor data '''
all_sensor_data = self._all_sonar_data[:]
all_sensor_data.extend( self._radar_data.values() )
''' Go through all sensor points that were reported to agent...'''
for sensor_pt in all_sensor_data:
if sensor_pt is None:
continue
if not sensor_pt in correlated_s_pts:
''' This sensor_pt was not correlated with an AIS ship. It is a possible threat.
Check if the sensor_pt is within self._threat_dist of an AIS ship'''
for ais_id in self._ais_data:
ais_pt = self._ais_data[ais_id]
d = haver_distance( sensor_pt[0], sensor_pt[1], ais_pt[0], ais_pt[1] )
if d <= self._threat_dist:
self._publish_threat(ais_id, sensor_pt, d)
#self.lock.release()
def _pt_in_sonar_land_data(self, pt):
if pt in self._sonar_land_data:
if self._sonar_land_data[pt] > 4:
return True
return False
def _check_sonar_land_data(self, pt):
#if self._pt_in_sonar_land_data(pt):
if pt in self._sonar_land_data:
# Test if land data is ready
if (not self._sonar_land_data_is_ready) and (self._sonar_land_data[pt] > 5):
self._sonar_land_data_is_ready = True
print 'SONAR LAND DATA IS READY.'
#self._sonar_land_data[pt] = self._sonar_land_data[pt] + 1
self._sonar_land_data[pt] += 1
#print 'land pt repeat count = ', self._sonar_land_data[pt]
return True
else:
#print 'new possible land pt:', pt
self._sonar_land_data[pt] = 1 # start count at 1
return False
def _parse_sonar_data(self, event):
msg = event.get_message().get_payload()
info = msg.split()
bearing = float(info[0])
return bearing, info[1:] # will always get something for the given bearing (if no ship, picks up land)
def _new_sonar_data(self, event):
sonar_id = event.get_owner_uid()
bearing = event.get_bearing()
detects = event.get_detects()
data = None # what we'll actually store
self.lock.acquire()
if len(detects) == 3:
#print 'Got', len(detects), 'data in bearing', bearing
lat, lon, thing = detects
data = (lat, lon)
elif len(detects) == 1:
# # probs got [None]. Anyway, useless info if just 1 item
data = None
else:
print 'got data from sonar: \"',detects,'\" and did not parse'
self.lock.release()
return
self.lock.release()
if sonar_id == 901:
self._sonar1_data[bearing] = data
elif sonar_id == 902:
self._sonar2_data[bearing] = data
else:
print "Sonar node is not ID 901 or 902. Not storing sonar data. sonar node id = ", sonar_id
if not data is None:
is_land_pt = self._check_sonar_land_data( data )
if not is_land_pt:
self.refresh()
#else:
# print (lat,lon), 'is a land point'
def _new_radar1_data(self, event):
loc = event.get_target_location()
bearing = event.get_bearing()
bearing = int( 360.0*bearing / (2*math.pi) )
self.lock.acquire()
#if (not self._radar_data.has_key(bearing)) or (not loc is self._radar_data[bearing]):
# print 'Just added radar data at bearing ', bearing, '=', loc
self._radar_data[bearing] = loc # is either point (lat,lon) or None
self.lock.release()
self.refresh()
def _new_ais_data(self, event, iface):
msg = event.get_message().get_payload()
#print 'new ais msg: ', msg
s_id, lat, lon, agl, speed = msg.split(',')
s_id = int(s_id)
lat = float(lat)
lon = float(lon)
#if s_id == 80 or s_id ==81:
# print 'new ais msg:', msg
self.lock.acquire()
#if self._ais_data.has_key(s_id):
# dist = haver_distance(lat, lon, self._ais_data[s_id][0], self._ais_data[s_id][1])
# print 'dist between last AIS data = ', dist
self._ais_data[s_id] = (lat, lon) # ignoring agl and speed for now
self.lock.release()
self.refresh()
def _update_sensor_data(self):
''' This method adds all SONAR sensor points to self._all_sonar_data.
Sonar sensor points are no
closest_dist, closest_pt = self._get_closest_pt(ais_pt, available_sensor_pts)t added if they appear to be land points.
'''
self._all_sonar_data = []
# add radar sensor data
## Note: self._radar_data[bearing] = (lat, lon)
''' Add SONAR sensor points to sensor_data list.
Iterate through all sonars' sensor points combined
(which sonar sensor data came from does not matter at this point;
it only matters when trying to delete sonar data from any one particular sonar sensor only) '''
if not self._sonar_land_data_is_ready:
#print 'sonar data is not ready yet.'
return
all_sonar_pts = []
all_sonar_pts.extend( self._sonar1_data.values() )
all_sonar_pts.extend( self._sonar2_data.values() )
#print 'length of all_sonar_pts = ', len(all_sonar_pts)
for sonar_loc in all_sonar_pts:
if sonar_loc is None:
continue
if self._pt_in_sonar_land_data(sonar_loc) is True:
''' This is a land point'''
#print sonar_loc, 'is a land pt'
continue
self._all_sonar_data.append(sonar_loc)
''' We no longer do this, because we will correlate BOTH a radar and a sonar point to each AIS ship (if it is within range).
class 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))
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)
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()
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
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
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