Files

86 lines
3.5 KiB
Python

"""One native MoveJ dispatch; resume callbacks never dispatch robot commands."""
import threading
import time
class ConfirmedNativeMoveJ:
def __init__(self, controller, gate, record, *, resume_timeout=4.):
# Discover services before callbacks, on the calling thread.
self.movej = controller.service('movej')
self.stop = controller.service('emergency_stop')
self.controller, self.gate, self.record = controller, gate, record
self.resume_timeout = resume_timeout
self.last = None
def _resume(self):
self.controller.resume_once = False
done = threading.Event()
reply = {}
def answered(value):
# Late replies cannot dispatch any motion.
reply.update(response=dict(value), received_at=time.monotonic())
done.set()
def failed(error):
reply.update(error=str(error), received_at=time.monotonic())
done.set()
self.record('resume_requested', data=False)
deadline = time.monotonic() + self.resume_timeout
self.stop.call({'data': False}, answered, failed)
while not done.is_set():
self.gate.check_cancelled()
remaining = deadline-time.monotonic()
if remaining <= 0:
self.record('resume_timeout', movej_dispatched=False)
raise TimeoutError('Software stop release unconfirmed; MoveJ not dispatched')
done.wait(min(.02, remaining))
self.gate.check_cancelled()
if reply['received_at'] > deadline:
self.record('resume_timeout', movej_dispatched=False)
raise TimeoutError('Software stop release unconfirmed; MoveJ not dispatched')
if 'error' in reply:
self.record('resume_error', error=reply['error'], movej_dispatched=False)
raise RuntimeError('Software stop release failed: '+reply['error'])
response = reply['response']
self.record('resume_result', response=response)
if response.get('success') is not True:
raise RuntimeError('Software stop release rejected; MoveJ not dispatched: '+str(response))
def send_move(self, payload, callback, errback):
# Callers hold MotionGate.send's non-reentrant lock. Never re-enter
# the gate or dispatch motion from the ROS response callback.
if getattr(self.controller, 'resume_once', False):
self._resume()
self.gate.check_cancelled()
status = dict(dispatch_started=True, transport_call_returned=False,
response_received=False, accepted=False, payload=payload)
self.last = status
self.record('movej_dispatch', payload=payload)
def answered(value):
response = dict(value)
status.update(response_received=True, accepted=response.get('success') is True,
response=response)
self.record('movej_response', response=response)
callback(value)
def failed(error):
status.update(response_received=True, accepted=False, error=str(error))
self.record('movej_error', error=str(error))
errback(error)
try:
self.movej.call(payload, answered, failed)
except Exception as error:
failed(error)
raise
status['transport_call_returned'] = True
def bind_confirmed_movej(controller, gate, record):
transport = ConfirmedNativeMoveJ(controller, gate, record)
controller.send_move = transport.send_move
return transport