def _send_missionlist(self, missionlist):
topic = topics.get('MissionList')
topic.send(
dict(name=missionlist.name,
missions=[
mission.name for mission in missionlist.get_missions()
]))
def get_command(self):
cmdtopic = topics.get('InteractionCommand')
while True:
try:
return cmdtopic.take()
except dds.Error, err:
sched.ddswait(cmdtopic)
continue
def _send_initial_messages(self):
availtopic = topics.get('AvailableMissions')
availtopic.send(
dict(missions=[
mission.name for mission in missionregistry.get_missions()
]))
for missionlist in missionlistmanager.get_lists():
self._send_missionlist(missionlist)
def get_killed_by():
topic = topics.get('WorkerKill')
try:
for msg in topic.read_all():
if msg['killed']:
return msg['name']
return None
except dds.Error:
return None
def __getattr__(self, name):
if name not in self._active_listeners:
self._active_listeners[name] = Listener(
topics.get(name), lambda: self._active_listeners.pop(name))
last = self._active_listeners[name].get_last()
if last is not None:
return last
else:
raise util.DataNotYetAvailableError(name)
def log(self, msg, logtype='info'):
topic = topics.get('WorkerLogMessage')
topic.send({
'worker': self.name,
'type': 'WORKERLOGTYPE_' + logtype.upper(),
'msg': msg,
'time': int(time.time())
})
print '[' + time.strftime(
'%h:%m:%s %p') + '] ' + self.name + ': ' + msg
def set_state(self, statecode, statemsg):
topic = topics.get('WorkerState')
topic.send({
'worker': self.name,
'state': {
'code': 'WORKERSTATECODE_' + statecode.upper(),
'msg': statemsg
}
})
self.state = statecode
self.statemsg = statemsg
def get_objects(object_names, cameraname):
if not isinstance(object_names, list):
object_names = [object_names]
topic = topics.get('VisionResults')
for msg in topic.read_all():
if msg['cameraname'] == cameraname:
result = json.loads(msg['result'])
return list(
itertools.chain.from_iterable(
result.get(object_name, [])
for object_name in object_names))
return []
def run(self):
topic = topics.get('WorkerKill')
prev_killedby = None
while True:
killedby = get_killed_by()
if prev_killedby is None and killedby is not None:
for callback in self.kill_callbacks:
callback(killedby)
elif prev_killedby is not None and killedby is None:
for callback in self.unkill_callbacks:
callback()
prev_killedby = killedby
sched.ddswait(topic)
def write(self, data):
self.realstream.write(data)
logfile.write(data)
logfile.flush()
self.buf += data
outtopic = topics.get('InteractionOutput')
while True:
(msg, newline, newbuf) = self.buf.partition('\n')
if newline == '':
break
outtopic.send({'type': 'IOT_' + self.outtype.upper(), 'data': msg})
self.buf = newbuf
def run(self):
self._send_initial_messages()
cmdtopic = topics.get('MissionCommand')
while True:
sched.ddswait(cmdtopic)
msg = cmdtopic.take()
if msg['type'] == 'MISSIONCOMMANDTYPE_START':
self.start()
elif msg['type'] == 'MISSIONCOMMANDTYPE_STOP':
self.stop()
elif msg['type'] == 'MISSIONCOMMANDTYPE_ADD_MISSION':
missionlist = missionlistmanager.get(msg['list'])
mission = missionregistry.get(msg['mission'])
if missionlist is not None and mission is not None:
missionlist.add(mission,
msg['pos'] if msg['pos'] != -1 else None)
self._send_missionlist(missionlist)
elif msg['type'] == 'MISSIONCOMMANDTYPE_REMOVE_MISSION':
missionlist = missionlistmanager.get(msg['list'])
if missionlist is not None:
missionlist.remove(msg['pos'])
self._send_missionlist(missionlist)
def wait():
topic = topics.get('VisionResults')
sched.ddswait(topic)
def set_object_names(object_names, cameraname):
if not isinstance(object_names, list):
object_names = [object_names]
topic = topics.get('VisionSetObjects')
topic.send({'objectnames': object_names, 'cameraname': cameraname})
def set_status(self, code):
statustopic = topics.get('InteractionStatus')
statustopic.send({'cmd': self.cmd, 'status': 'ISC_' + code.upper()})
def send_output(self, data, outtype):
outtopic = topics.get('InteractionOutput')
outtopic.send({'type': 'IOT_' + outtype.upper(), 'data': data})
def get_w_weights():
topic = topics.get('TrackingControllerLog')
try:
return topic.read()['W_hat']
except dds.Error:
return numpy.zeros((6, 6))
def get_rise_control():
topic = topics.get('TrackingControllerLog')
try:
return topic.read()['rise_control']
except dds.Error:
return numpy.zeros((6, 1))
def _ddscallback():
topic = topics.get('MissionState')
topic.send(
dict(running=True,
state=statemanager.state)) # TODO determine if actually running
def logtask():
topic = topics.get('WorkerLog')
while True:
sched.ddswait(topic)
print topic.take()
help='use this cameraname (ex: "forward")',
dest='cameraname',
type=str,
)
parser.add_argument(
metavar='OBJECTNAME',
nargs='*',
help='broadcast messages about this object (ex: "buoy/red")',
dest='objectnames',
)
args = parser.parse_args()
import dds
from subjugator import topics
t = topics.get('VisionResults')
while True:
messages = [
dict(
objectName=objectname,
center=args.center if args.center is not None else
(random.gauss(0, .001), random.gauss(0, .001)),
scale=args.scale
if args.scale is not None else random.expovariate(1 / 1000),
angle=0,
item=random.choice(['sword', 'shield', 'net', 'trident']),
hue=10,
) for objectname in args.objectnames
]
print "Sending with cameraname %s:" % (args.cameraname, )
def __init__(self):
self.debug_topic = topics.get('VisionDebug')
self.config_topic = topics.get('VisionConfig')
self.result_topic = topics.get('VisionResults')
self.setobjects_topic = topics.get('VisionSetObjects')
