def run(self):
setproctitle('python._YuMiSubscriberEthernet.{0}'.format(self._name))
logging.getLogger().setLevel(YMC.LOGGING_LEVEL)
self._reset_socket()
try:
while True:
if self._cmd_q.qsize() > 0:
cmd = self._cmd_q.get()
if cmd == 'stop':
break
if cmd == 'reset':
self._reset_socket()
else:
raw_res = None
try:
raw_res = self._socket.recv(self._bufsize)
except socket.error, e:
if e.errno == 114: # request time out
raise YuMiCommException('Request timed out')
if raw_res is None or len(raw_res) == 0:
self._stop()
break
raw_res = raw_res[:raw_res.rfind("!")]
raw_res = raw_res[raw_res.rfind("#") + 1:]
tokens = raw_res.split()
res = _RAW_RES(float(tokens[0]), ' '.join(tokens[1:]))
if self._data_q.full():
try:
self._data_q.get_nowait()
except Empty:
pass
self._data_q.put(res)
sleep(1e-3)
except KeyboardInterrupt:
pass
self._stop()
sys.exit(0)
def _request(self, req, wait_for_res, timeout=None):
if timeout is None:
timeout = self._comm_timeout
req_packet = _REQ_PACKET(req, timeout, wait_for_res)
logging.debug('Process req: {0}'.format(req_packet))
self._req_q.put(req_packet)
if wait_for_res:
try:
res = self._res_q.get(block=True,
timeout=self._process_timeout)
except (IOError, Empty):
raise YuMiCommException(
"Request timed out: {0}".format(req_packet))
logging.debug('res: {0}'.format(res))
if res.res_code != YMC.RES_CODES['success']:
raise YuMiControlException(req_packet, res)
return res
def _send_request(self, req_packet):
logging.debug("Sending: {0}".format(req_packet))
raw_res = None
if self._debug:
raw_res = '-1 1 MOCK RES for {0}'.format(req_packet)
else:
self._socket.settimeout(req_packet.timeout)
while True:
try:
self._socket.send(req_packet.req)
break
except socket.error, e:
# TODO: better way to handle this mysterious bad file descriptor error
if e.errno == 9:
self._reset_socket()
try:
raw_res = self._socket.recv(self._bufsize)
except socket.error, e:
if e.errno == 114: # request time out
raise YuMiCommException(
'Request timed out: {0}'.format(req_packet))
break
except socket.error, e:
# TODO: better way to handle this mysterious bad file descriptor error
if e.errno == 9:
self._reset_socket()
try:
raw_res = self._socket.recv(self._bufsize)
except socket.error, e:
if e.errno == 114: # request time out
raise YuMiCommException(
'Request timed out: {0}'.format(req_packet))
logging.debug("Received: {0}".format(raw_res))
if raw_res is None or len(raw_res) == 0:
raise YuMiCommException(
'Empty response! For req: {0}'.format(req_packet))
tokens = raw_res.split()
res = _RAW_RES(int(tokens[0]), int(tokens[1]), ' '.join(tokens[2:]))
return res
class YuMiArm:
""" Interface to a single arm of an ABB YuMi robot.
Communicates with the robot over Ethernet.
"""
def __init__(self,
name,
ip=YMC.IP,
port=YMC.PORTS["left"]["server"],
bufsize=YMC.BUFSIZE,