def make_async_controller(base):
""" allows to instantiate the motor controller as thread or as process
:param base: options are Thread|Process
:return:
"""
class AsyncController(base):
def __init__(self, queue):
super(AsyncController, self).__init__()
self.queue = queue
self.robot = Controller()
self.last_action_time = time.time()
self.last_action = []
def run(self):
keep_running = True
while keep_running:
if not self.queue.empty():
action = self.queue.get()
if action == "quit":
keep_running = False
self.robot.list_action([0, 0])
self.robot.rgb_off()
else:
ik = self.robot.ik_action(v=action[0], omega=action[1])
if len(action) == 5:
self.robot.rgb_action(action[2:])
self.last_action_time = time.time()
self.last_action = ik
else:
# check if it's time to break
delta = time.time(
) - self.last_action_time - DECELERATION_TIMEOUT
if 0 <= delta < DECELERATION_BREAK_TIME:
# decelerate
if len(self.last_action) == 2:
new_action = ease_out_action(
self.last_action, delta)
self.robot.list_action(new_action)
time.sleep(1 / DECELERATION_STEPS)
elif delta >= DECELERATION_BREAK_TIME and len(
self.last_action) == 2:
# this is run only once after decel to
# make sure the robot comes to a full stop
# and doesn't continue to move at 0.00001 speed
self.last_action = []
self.robot.stop()
queue = get_right_queue(base)
return AsyncController, queue
def make_async_camera(base):
""" allows to instantiate the camera as thread or as process
:param base: options are Thread|Process
:return:
"""
class AsyncPubCamera(base):
def __init__(self, queue):
super(AsyncPubCamera, self).__init__()
# Thread.__init__(self)
self.queue = queue
self.publisher_socket = None
self.cam = Camera(res=(160, 120))
self.context = zmq.Context()
def run(self):
# wait for first subscriber
# then create socket
# get camera image
# broadcast image
keep_running = True
while keep_running:
if not self.queue.empty():
cmd = self.queue.get()
if cmd == "kill":
keep_running = False
break # redundant I guess
else:
if self.publisher_socket is None:
self.publisher_socket = make_pub_socket(
for_images=True,
context_=self.context
)
# the pub / send_array method only works once the first subscriber is connected
if self.publisher_socket is not None:
img = self.cam.observe()
# on the real robot we are sending 0 reward, in simulation the reward is a float
# we also send done=False because the real robot doesn't know when to stop ^^
send_gym(self.publisher_socket, img, 0, False)
queue = get_right_queue(base)
return AsyncPubCamera, queue