def test_remote_memory_request():
magic1 = 0x4141414141414141
magic2 = 0x4242424242424242
magic3 = 0x4343434343434343
magic4 = 0x4444444444444444
magic5 = 4
q = queue.Queue()
a = MessageQueue('/a', flags=O_CREAT | O_RDWR)
b = MessageQueue('/b', flags=O_CREAT | O_RDWR)
mprot1 = avatar2.protocols.remote_memory.RemoteMemoryProtocol(
'/a', '/b', q)
ret = mprot1.connect()
assert_equal(ret, True)
assert_not_equal(mprot1._rx_queue, None)
assert_not_equal(mprot1._tx_queue, None)
request = avatar2.protocols.remote_memory.RemoteMemoryReq(
magic1, magic2, magic3, magic4, magic5, 1)
a.send(request)
msg = q.get()
assert_equal(msg.id, magic1)
assert_equal(msg.pc, magic2)
assert_equal(msg.address, magic3)
assert_equal(msg.value, magic4)
assert_equal(msg.size, magic5)
def pi(n):
pids = []
unit = n / 10
q = Queue("/pi", flags=os.O_CREAT)
for i in range(10):
mink = unit * i
maxk = mink + unit
pid = os.fork()
if pid > 0:
pids.append(pid)
else:
s = slice(mink, maxk)
q.send(str(s))
q.close()
sys.exit(0)
sums = []
for pid in pids:
sums.append(float(q.receive()[0]))
os.waitpid(pid, 0)
q.close()
q.unlink()
return math.sqrt(sum(sums) * 8)
class SpotGyro():
def __init__(self):
try:
unlink_message_queue("/gyro")
except:
pass
self.mqueue = MessageQueue('/gyro', flags=O_CREX, max_messages=MAX_MSG)
try:
self.gyro = mpu6050(0x68)
except:
raise Exception("Could not connect to Gyroscope MPU6050")
self.angleFront = 0.
self.angleSide = 0.
def get_position(self):
try:
accelval = self.gyro.get_accel_data()
gyroval = self.gyro.get_gyro_data()
self.angleFront = 0.80 * (
self.angleFront + float(gyroval['y']) * 0.01 / 131
) + 0.20 * atan2(accelval['x'], accelval['z']) * 180 / 3.14159
self.angleSide = 0.80 * (
self.angleSide + float(gyroval['x']) * 0.01 / 131
) + 0.20 * atan2(accelval['y'], accelval['z']) * 180 / 3.14159
if self.mqueue.current_messages >= MAX_MSG:
self.mqueue.receive()
self.mqueue.send('{"front":"' + str(self.angleFront) +
'","side":"' + str(self.angleSide) +
'","time":"' + str(time()) + '"}')
except Exception:
pass
class RemoteMemoryProtocol(object):
"""
This class listens to memoryforward requests and lifts them to avatar
messages. Likewise it can be directed to emit memoryforward-response
messages
:param rx_queue_name: Name of the queue for receiving
:param tx_queue_name: Name of the queue for sending
:param avatar_queue: Queue to dispatch received requests to
:param origin: Reference to the Target utilizing this protocol
"""
def __init__(self,
rx_queue_name,
tx_queue_name,
avatar_queue,
origin=None):
self._rx_queue = None
self._tx_queue = None
self._rx_listener = None
self.rx_queue_name = rx_queue_name
self.tx_queue_name = tx_queue_name
self._avatar_queue = avatar_queue
self._origin = origin
self.log = logging.getLogger('%s.%s' %
(origin.log.name, self.__class__.__name__)
) if origin else \
logging.getLogger(self.__class__.__name__)
def connect(self):
"""
Connect to the message queues for remote memory
:return True on success, else False
"""
try:
self._rx_queue = MessageQueue(self.rx_queue_name,
flags=O_RDONLY,
read=True,
write=False)
except Exception as e:
self.log.error("Unable to create rx_queue: %s" % e)
return False
try:
self._tx_queue = MessageQueue(self.tx_queue_name,
flags=O_WRONLY,
read=False,
write=True)
except Exception as e:
self.log.error("Unable to create tx_queue: %s" % e)
self._rx_queue.close()
return False
self._rx_listener = RemoteMemoryRequestListener(
self._rx_queue, self._avatar_queue, self._origin)
self._rx_listener.daemon = True
self._rx_listener.start()
self.log.info("Successfully connected rmp")
return True
def __del__(self):
self.shutdown()
def shutdown(self):
if self._rx_listener:
self._rx_listener.stop()
self._rx_listener = None
if self._rx_queue:
try:
self._rx_queue.unlink()
self._rx_queue.close()
self._rx_queue = None
except ExistentialError:
self.log.warning("Tried to close/unlink non existent rx_queue")
if self._tx_queue:
try:
self._tx_queue.unlink()
self._tx_queue.close()
self._tx_queue = None
except ExistentialError:
self.log.warning("Tried to close/unlink non existent tx_queue")
def send_response(self, id, value, success):
response = RemoteMemoryResp(id, value, success)
try:
self._tx_queue.send(response)
self.log.debug("Send RemoteMemoryResponse with id %d, %x" %
(id, value))
return True
except Exception as e:
self.log.error("Unable to send response: %s" % e)
return False
class SpotGamepad():
def __init__(self):
try:
unlink_message_queue("/spotgamepad")
unlink_message_queue("/cameramotion")
except:
pass
self.motionqueue = MessageQueue('/spotgamepad',
flags=O_CREX,
max_messages=MAX_MSG)
self.cameraqueue = MessageQueue('/cameramotion',
flags=O_CREX,
max_messages=MAX_MSG)
# Initialize Gamepad
self.gamepad = evdev.InputDevice(INPUT_DEVICE)
def read_event(self):
for event in self.gamepad.read_loop():
if event.type == evdev.ecodes.EV_ABS:
absevent = evdev.categorize(event)
# If motion button
if evdev.ecodes.bytype[absevent.event.type][
absevent.event.code] == 'ABS_HAT0Y':
code = absevent.event.value
if code == -1:
self.send_msg(
'spotgamepad', '{"action":"forward","time":"' +
str(time()) + '"}')
elif code == 1:
self.send_msg(
'spotgamepad', '{"action":"backward","time":"' +
str(time()) + '"}')
else:
self.send_msg(
'spotgamepad',
'{"action":"stop","time":"' + str(time()) + '"}')
if evdev.ecodes.bytype[absevent.event.type][
absevent.event.code] == 'ABS_HAT0X':
code = absevent.event.value
if code == -1:
self.send_msg(
'spotgamepad',
'{"action":"left","time":"' + str(time()) + '"}')
elif code == 1:
self.send_msg(
'spotgamepad',
'{"action":"right","time":"' + str(time()) + '"}')
else:
self.send_msg(
'spotgamepad',
'{"action":"stop","time":"' + str(time()) + '"}')
# If body position joystick
if evdev.ecodes.bytype[absevent.event.type][
absevent.event.code] == 'ABS_X':
if absevent.event.value < 128:
self.send_msg(
'spotgamepad', '{"action":"bodyleft","time":"' +
str(time()) + '"}')
elif absevent.event.value > 128:
self.send_msg(
'spotgamepad', '{"action":"bodyright","time":"' +
str(time()) + '"}')
else:
self.send_msg(
'spotgamepad',
'{"action":"stop","time":"' + str(time()) + '"}')
if evdev.ecodes.bytype[absevent.event.type][
absevent.event.code] == 'ABS_Y':
if absevent.event.value < 128:
self.send_msg(
'spotgamepad', '{"action":"bodyfront","time":"' +
str(time()) + '"}')
elif absevent.event.value > 128:
self.send_msg(
'spotgamepad', '{"action":"bodyback","time":"' +
str(time()) + '"}')
else:
self.send_msg(
'spotgamepad',
'{"action":"stop","time":"' + str(time()) + '"}')
# If camera position joystick
if evdev.ecodes.bytype[absevent.event.type][
absevent.event.code] == 'ABS_Z':
print('lateral camera: ' + str(absevent.event.value))
if evdev.ecodes.bytype[absevent.event.type][
absevent.event.code] == 'ABS_RZ':
print('front camera: ' + str(absevent.event.value))
# If action button
if event.type == evdev.ecodes.EV_KEY:
keyevent = evdev.categorize(event)
# Laydown
if laydown_button in evdev.ecodes.bytype[keyevent.event.type][
keyevent.event.code]:
if keyevent.event.value == 0:
self.send_msg(
'spotgamepad', '{"action":"laydown","time":"' +
str(time()) + '"}')
# Wake up
if wakeup_button in evdev.ecodes.bytype[keyevent.event.type][
keyevent.event.code]:
if keyevent.event.value == 0:
self.send_msg(
'spotgamepad',
'{"action":"wakeup","time":"' + str(time()) + '"}')
def send_msg(self, queue, msg):
if self.cameraqueue.current_messages >= MAX_MSG:
self.cameraqueue.receive()
if self.motionqueue.current_messages >= MAX_MSG:
self.motionqueue.receive()
if queue == 'spotgamepad':
self.motionqueue.send(msg)
elif queue == 'cameramotion':
self.cameraqueue.send(msg)
class message_queue:
O_CREAT = threading.O_CREAT
O_RDONLY = threading.O_RDONLY
def __init__(self, mq_name, mode=O_CREAT):
log_format = '%(asctime)-15s : %(levelname)s : line %(lineno)-4d in %(module)s.%(funcName)s > %(message)s'
logging.basicConfig(level=logging.DEBUG, format=log_format)
self.logger = logging.getLogger('mq-mq_name')
self.name = mq_name
self.lock = threading.Lock()
self.valid = False
self.delete_on_destruct = False
if mode is message_queue.O_CREAT:
self.create_mq()
else: # RD_ONLY but need verify for RDWR and WR_ONLY
self.connect_mq()
return
def create_mq(self):
self.delete_on_destruct = True
self.logger.info('Attempting to create messsage_queue: ' + self.name)
try:
self.mq = MessageQueue(self.name, MessageQueue.O_CREAT)
self.valid = True
except:
self.mq = None
e = sys.exc_info()[0]
self.logger.info('Error creating message_queue:' + e.__repr__())
return None
def connect_mq(self):
self.delete_on_destruct = False
self.logger.info('Attemping to connect to message_queue:' + self.name)
try:
MessageQueue(self.name, threading.O_RDONLY)
self.valid = True
except ExistentialError as e:
self.logger.info(' - Could not connect to message_queue::' + e.__repr__())
self.mq = None
def send(self, message):
if not self.valid:
self.logger.info('Cannot send message:' + self.name + 'is invalid.')
try:
self.mq.send(message)
except:
e = sys.exc_info()[0]
self.logger.info('Error sending message:' + e.__repr__())
def receive(self, timeout=0):
if not self.valid:
self.logger.info('Cannot receive messages:' + self.name + 'is invalid.')
return
try:
message = self.mq.receive(timeout)
return message
except:
e = sys.exc_info()[0]
self.logger.info('Error receiving message:' + e.__repr__())
return None
def delete_when_finished(self, delete_when_finished):
# probably should test type for bool_type
self.delete_on_destruct = delete_when_finished
def __enter__(self):
return
def __exit__(self):
if not self.delete_on_destruct
return
try:
self.logger.info('Unlinking MessageQueue:' + self.name)
unlink_message_queue(self.name)
except ExistentialError as e:
pass
def run(self): # overrides threading.Thread.run()
"""
Overrides threading.Thread.run()
Creates initial connection to expected message queues; echometrics, frames, and tracks. It polls each one
waiting for the read-ready flag to be set and consumes the data. It reformats the data into an object and
writes it out to clients who are registered.
Returns
-------
None
"""
logger.info('Creating message_queues: ' + self.display_q_name)
display_q = MessageQueue(self.display_q_name, O_CREAT)
em_q = MessageQueue(self.em_q_name, O_CREAT)
# TODO: READ THE NIMS FRAMEBUFFER NAME FROM THE YAML
try: # connect to nims ingestor to get backscatter data
nims_framebuffer = '/' + self.config['FRAMEBUFFER_NAME']
logger.info('Connecting to ' + nims_framebuffer)
framebuffer_q = MessageQueue(nims_framebuffer, O_RDONLY)
framebuffer_q.send(self.display_q_name)
logger.info(" - sent queue: " + self.display_q_name)
except ExistentialError as e:
logger.info(' - Could not connect to ' + nims_framebuffer + '::' +
e.__repr__())
try: # connect to nims tracker to get track data (probably out of sync)
tracker_name = '/' + self.config['TRACKER_NAME']
logger.info('Connecting to ' + tracker_name)
trackbuffer_q = MessageQueue(tracker_name, O_RDONLY)
except ExistentialError as e:
logger.info(' - Could not connect to ' + tracker_name + '::' +
e.__repr__())
try: # connect to the echometrics queue for periodic em data
em_queue_name = self.config['ECHOMETRICS']['queue_name']
logger.info('Connecting to ' + em_queue_name)
em_request_q = MessageQueue(em_queue_name, O_RDONLY)
em_request_q.send(self.em_q_name)
except:
logger.info(' - Could not connect to ' + em_queue_name)
poller = select.poll()
poller.register(em_q.mqd, select.POLLIN)
poller.register(display_q.mqd, select.POLLIN)
poller.register(trackbuffer_q.mqd, select.POLLIN)
time.sleep(
1
) # apparently necessary to create this latency for the frame_buffer app?
while True:
frame_buffer = None
track_buffer = None
em_buffer = None
mq_state = poller.poll()
for state in mq_state:
mqd = state[0]
if mqd == em_q.mqd:
buf = em_q.receive()[0]
em_buffer = ast.literal_eval(buf)
elif mqd == display_q.mqd:
frame = frames.frame_message(display_q.receive()[0])
if frame.valid is False:
logger.info('Received invalid message: ignoring')
continue
try:
#logger.info(' -- Connecting to ' + frame.shm_location)
shm_frame = SharedMemory(frame.shm_location,
O_RDONLY,
size=frame.frame_length)
except StandardError as e:
logger.info(' -- Error connecting to',
frame.shm_location, '::', e.__repr__())
continue
mapped = mmap(shm_frame.fd, shm_frame.size)
shm_frame.close_fd()
frame_buffer = frames.frame_buffer(
mapped.read(frame.frame_length))
mapped.close()
if frame_buffer.valid is False:
logger.info(' -- Error Parsing Frame')
continue
image = np.array(frame_buffer.image)
image = image.reshape((frame_buffer.num_samples[0],
frame_buffer.num_beams[0]))
image = image[0:-1:4, ::]
frame_buffer.image = image.flatten().tolist()
frame_buffer.num_samples = (frame_buffer.num_samples[0] /
4, 0)
elif mqd == trackbuffer_q.mqd:
track_buffer = frames.track_message(
trackbuffer_q.receive()[0])
clients = copy.copy(self.clients)
for client in clients:
try:
if frame_buffer:
client.send_image(frame_buffer)
if track_buffer:
client.send_tracks(track_buffer)
if em_buffer:
client.send_metrics(em_buffer)
except StandardError as e:
logger.info("Error sending data to client")
print sys.exc_info()
continue
return
class ARMV7MInterruptProtocol(Thread):
"""
This protocol has two purposes:
a) injecting interrupts into an analysis target
b) extracting interrupt exits and putting them into the avatar queue
(b) is necessary in cases where two targets need to be synched on the
interrupts.
The way a v7m-nvic implements interrupt return is to put a magic value
into $pc, and the hardware does the actual magic of popping from the
interrupt stack and restoring the context.
However, the magic value defines the type of the interrupt return,
and is hence synchronized on interrupt exit, alongside with the
interrupt number
:param origin: Reference to the Target utilizing this protocol
:param rx_queue_name: Name of the queue for receiving
:param tx_queue_name: Name of the queue for sending
"""
def __init__(self, origin, rx_queue_name, tx_queue_name):
super(self.__class__, self).__init__()
self._rx_queue_name = rx_queue_name
self._tx_queue_name = tx_queue_name
self._rx_queue = None
self._tx_queue = None
self._avatar_queue = origin.avatar.queue
self._origin = origin
self._close = Event()
self._closed = Event()
self._close.clear()
self._closed.clear()
self.log = logging.getLogger('%s.%s' %
(origin.log.name, self.__class__.__name__)
) if origin else \
logging.getLogger(self.__class__.__name__)
def run(self):
while True:
if self._close.is_set():
break
request = None
try:
request = self._rx_queue.receive(0.5)
except:
continue
req_struct = V7MRemoteInterruptNotification.from_buffer_copy(
request[0])
if RINOperation(req_struct.operation) == RINOperation.ENTER:
msg = RemoteInterruptEnterMessage(self._origin, req_struct.id,
req_struct.num_irq)
elif RINOperation(req_struct.operation) == RINOperation.EXIT:
msg = RemoteInterruptExitMessage(self._origin, req_struct.id,
req_struct.type,
req_struct.num_irq)
self.log.debug(
"Received an InterruptExitRequest for irq %d (%x)" %
(req_struct.num_irq, req_struct.type))
else:
msg = None
raise Exception(
("Received V7MRemoteInterrupt Notification with"
"unknown operation type %d") % req_struct.operation)
self._avatar_queue.put(msg)
self._closed.set()
def stop(self):
self._close.set()
self._closed.wait()
def enable_interrupts(self):
if isinstance(self._origin, QemuTarget):
# TODO: Make this more clean, i.e., check for remote memory
rmem_rx_qname = self._origin.protocols.remote_memory.rx_queue_name
rmem_tx_qname = self._origin.protocols.remote_memory.tx_queue_name
# the tx-queue for qemu is the rx-queue for avatar and vice versa
self._origin.protocols.monitor.execute_command(
'avatar-armv7m-enable-irq', {
'irq_rx_queue_name': self._tx_queue_name,
'irq_tx_queue_name': self._rx_queue_name,
'rmem_rx_queue_name': rmem_tx_qname,
'rmem_tx_queue_name': rmem_rx_qname
})
else:
raise Exception("V7MInterruptProtocol is not implemented for %s" %
self._origin.__class__)
try:
self._rx_queue = MessageQueue(self._rx_queue_name,
flags=O_RDONLY,
read=True,
write=False)
except Exception as e:
self.log.error("Unable to create rx_queue: %s" % e)
return False
try:
self._tx_queue = MessageQueue(self._tx_queue_name,
flags=O_WRONLY,
read=False,
write=True)
except Exception as e:
self.log.error("Unable to create tx_queue: %s" % e)
self._rx_queue.close()
return False
self.daemon = True
self.start()
self.log.info("Enabled Interrupt Forwarding for %s" % self._origin)
return True
def ignore_interrupt_return(self, interrupt_number):
if isinstance(self._origin, QemuTarget):
self.log.info("Disable handling of irq return for %d" %
interrupt_number)
self._origin.protocols.monitor.execute_command(
'avatar-armv7m-ignore-irq-return',
{'num_irq': interrupt_number})
def unignore_interrupt_return(self, interrupt_number):
if isinstance(self._origin, QemuTarget):
self.log.info("Re-enable handling of irq return for %d" %
interrupt_number)
self._origin.protocols.monitor.execute_command(
'avatar-armv7m-unignore-irq-return',
{'num_irq': interrupt_number})
def inject_interrupt(self, interrupt_number, cpu_number=0):
if isinstance(self._origin, QemuTarget):
self.log.info("Injecting interrupt %d" % interrupt_number)
self._origin.protocols.monitor.execute_command(
'avatar-armv7m-inject-irq', {
'num_irq': interrupt_number,
'num_cpu': cpu_number
})
def set_vector_table_base(self, base, cpu_number=0):
if isinstance(self._origin, QemuTarget):
self.log.info("Setting vector table base to 0x%x" % base)
self._origin.protocols.monitor.execute_command(
'avatar-armv7m-set-vector-table-base', {
'base': base,
'num_cpu': cpu_number
})
def send_interrupt_exit_response(self, id, success):
response = V7MInterruptNotificationAck(id, success,
RINOperation.EXIT.value)
try:
self._tx_queue.send(response)
self.log.debug("Send RemoteInterruptExitResponse with id %d" % id)
return True
except Exception as e:
self.log.error("Unable to send response: %s" % e)
return False
def send_interrupt_enter_response(self, id, success):
response = V7MInterruptNotificationAck(id, success,
RINOperation.ENTER.value)
try:
self._tx_queue.send(response)
self.log.debug("Send RemoteInterruptEnterResponse with id %d" % id)
return True
except Exception as e:
self.log.error("Unable to send response: %s" % e)
return False
def __del__(self):
self.shutdown()
def shutdown(self):
self.stop()
if self._rx_queue:
try:
self._rx_queue.unlink()
self._rx_queue.close()
self._rx_queue = None
except ExistentialError:
self.log.warning("Tried to close/unlink non existent rx_queue")
if self._tx_queue:
try:
self._tx_queue.unlink()
self._tx_queue.close()
self._tx_queue = None
except ExistentialError:
self.log.warning("Tried to close/unlink non existent tx_queue")
from posix_ipc import MessageQueue as MsgQ
import sysv_ipc
import os
if __name__ == '__main__':
q = MsgQ("/tmp", flags=os.O_CREAT, mode=0666)
q.send("adfg")
rec = q.receive(1)
print rec
q.close()
q.unlink()
q2 = sysv_ipc.MessageQueue(16842753, flags=sysv_ipc.IPC_CREAT)
q2.send("sferogh", type=1)
def main():
signal.signal(signal.SIGINT, signal_handler)
global continue_running
continue_running = True
yaml_path = os.path.join(os.getenv("NIMS_HOME", "../build"), "config.yaml")
try:
config = ruamel.yaml.load(open(yaml_path, "r"),
ruamel.yaml.RoundTripLoader)
except:
import yaml
stream = file(config)
config = yaml.load(stream)
if type(config) == StringType:
config = ast.literal_eval(config)
logger.info('Configuration Path: ' + yaml_path)
emmq_name = os.getenv('EMMQ_NAME', '/em_mq') + repr(os.getpid())
nims_fb_name = '/' + config['FRAMEBUFFER_NAME']
bin_ping = int(config['ECHOMETRICS']['ping_bin'])
bin_depth = int(config['ECHOMETRICS']['depth_bin'])
mode = int(config['SONAR_TYPE'])
logger.info('Starting realtime_echometrics ...')
logger.info(' -- echoMetrics MessageQueue: ' + emmq_name)
logger.info(' -- framebuffer MessageQueue: ' + nims_fb_name)
logger.info(' -- ping bin size: ' + str(bin_ping))
logger.info(' -- depth bin size: ' + str(bin_depth))
logger.info(' -- mode = some mode')
logger.info('Creating MessageQueue: ' + emmq_name)
em_request_mq_name = config['ECHOMETRICS']['queue_name']
# we need a queue for requesting backscatter data and a queue for pushing em data
em_request_mq = MessageQueue(em_request_mq_name, O_CREAT)
em_clients_mq = []
em_mq = MessageQueue(emmq_name, O_CREAT)
try:
#logger.info('Connecting to' + nims_fb_name)
framebuffer_mq = MessageQueue(nims_fb_name, O_RDONLY)
framebuffer_mq.send(emmq_name)
logger.info(" -- sent private queue: " + emmq_name)
except ExistentialError as e:
logger.error(' -- Could not connect to' + nims_fb_name + e.__repr__())
# here, unless specified in the log, both processes are at least ready to communicate
poller = select.poll()
poller.register(em_mq.mqd, select.POLLIN)
poller.register(em_request_mq.mqd, select.POLLIN)
current_frames = []
#file_ = open('metrics.csv', 'w+')
#need_hdr = True
#import csv
#csvwriter = csv.writer(file_, delimiter=',')
logger.info('Starting fetch cycle.')
while continue_running:
mq_state = poller.poll()
for state in mq_state:
if state[0] == em_mq.mqd:
frame_buffer = fetch_framebuffer(em_mq)
if frame_buffer:
if mode == 1: # multbeam
frame_buffer = compress_beams(frame_buffer)
current_frames.append(frame_buffer)
if state[
0] == em_request_mq.mqd: # TODO catch error to drop this person.
client = accept_new_client(em_request_mq)
if client:
em_clients_mq.append(client)
if len(current_frames) >= bin_ping:
echo_metrics = calculate_echometrics(current_frames, bin_depth)
echo_metrics['pingid'] = frame_buffer.ping_num[0]
current_frames = []
#if need_hdr:
#csvwriter.writerow(echo_metrics.keys())
#need_hdr == False
#csvwriter.writerow(echo_metrics.values())
for q in em_clients_mq:
try:
print 'sending echometrics to' + q.name
q.send(str(echo_metrics))
except Exception as e:
logger.info(
'error writing to client q, removing from active clients.'
)
print e.__repr__()
em_clients_mq.remove(q)
do_exit(em_mq)
return