86 lines
3.5 KiB
Python
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
|