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)
def main():
""" Main launcher that starts the gym thread when the command 'duckietown-start-gym' is invoked
"""
# get parameters from environment (set during docker launch) otherwise take default
map = os.getenv('DUCKIETOWN_MAP', DEFAULTS["map"])
domain_rand = bool(
os.getenv('DUCKIETOWN_DOMAIN_RAND', DEFAULTS["domain_rand"]))
env = SimpleSimEnv(
map_name=map,
# draw_curve = args.draw_curve,
# draw_bbox = args.draw_bbox,
domain_rand=domain_rand)
obs = env.reset()
# env.render("rgb_array") # TODO: do we need this? does this initialize anything?
publisher_socket = None
command_socket, command_poll = make_pull_socket()
print("Simulator listening to incoming connections...")
while True:
if has_pull_message(command_socket, command_poll):
success, data = receive_data(command_socket)
if not success:
print(data) # in error case, this will contain the err msg
continue
if data["topic"] == 0:
obs, reward, done, misc = env.step(data["msg"])
print('step_count = %s, reward=%.3f' %
(env.step_count, reward))
if done:
env.reset()
if data["topic"] == 1:
print("received ping:", data)
# can only initialize socket after first listener is connected - weird ZMQ bug
if publisher_socket is None:
publisher_socket = make_pub_socket(for_images=True)
if data["topic"] in [0, 1]:
send_array(publisher_socket, obs)
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()
send_array(self.publisher_socket, img)
def main():
print("Launching Duckie Slimremote Robot Server...")
# require MAX_WAIT in seconds
# (MAX_WAIT is the period,
# i.e. the inverse of the comm frequency)
robot = FailsafeController()
cam = Camera()
subscriber_queue = start_thread_w_queue(ThreadedActionSubscriber)
sockets_pub = []
# controller queue to give priority
controllers = []
# action queue bc multiple actions could be received
actions = []
start = time.time()
# logbook for keeping track when a controller goes dark
# logs format: (time.time(), controller_id, topic, action)
logs = []
counter = 0
timer2 = time.time()
timer2_tests = 100
timer2_counter = 0
timer_msg_total = []
timer_pub_total = []
timer_srt_total = []
timer_obs_total = []
timer_cln_total = []
timer_total_counter = 0
while True:
# print ("waiting for action")
timer_msg = time.time()
data = get_last_queue_element(subscriber_queue)
timer_msg_total.append(time.time() - timer_msg)
topic, controller_id, controller_ip, msg = data
logs.append((time.time(), controller_id, topic, msg))
if controller_id not in controllers:
controllers.append(controller_id)
# create new publisher socket for this controller
# which is where we send the observations later
sock_pub = make_pub_socket(controller_ip, for_images=True)
# be polite
# HOWEVER DONT RELY ON THIS MESSAGE. IT MIGHT BE DROPPED
# say_hi(sock_pub)
# add this controller to the list of sockets
# which are interested in this robot
sockets_pub.append((controller_id, sock_pub))
if topic == 0:
actions.append((controller_id, msg))
time_passed = time.time() - start
# TODO: add dealing with signal 99
if time_passed >= MAX_WAIT:
timer_srt = time.time()
if len(actions) > 0:
# go over the list of actions and pick out the one
# that came from the latest-joined controller
action = select_action(actions, controllers)
robot.run(action)
timer_srt_total.append(time.time() - timer_srt)
timer_obs = time.time()
observation = cam.observe()
timer_obs_total.append(time.time() - timer_obs)
timer_pub = time.time()
for sock_pub in sockets_pub:
# non-blocking, i.e. doesn't rely on hosts being alive
# print ("sending out pic to",sock_pub[0])
send_array(sock_pub[1], observation)
timer_pub_total.append(time.time() - timer_pub)
actions = []
start = time.time()
timer2_counter += time.time() - timer2
if (counter + 1) % timer2_tests == 0:
diff = timer2_counter / timer2_tests
print("msg speed: {}s/{}Hz".format(
round(diff, 4),
round(1 / diff, 4),
))
timer2_counter = 0
timer2 = time.time()
timer_cln = time.time()
counter += 1
if counter == INACTIVE_CLEANUP_TIMER:
# this function removes controllers from the list
# which haven't sent something in N cycles, so that
# after hours of running the controller list
# doesn't overflow
remove_inactive(logs, controllers, sockets_pub)
counter = 0
timer_cln_total.append(time.time() - timer_cln)
timer_total_counter += 1
if timer_total_counter == 50:
print("msg {}\npub {}\nsrt {}\nobs {}\ncln {}\n".format(
np.mean(timer_msg_total),
np.mean(timer_pub_total),
np.mean(timer_srt_total),
np.mean(timer_obs_total),
np.mean(timer_cln_total),
))
timer_total_counter = 0
def main():
""" Main launcher that starts the gym thread when the command 'duckietown-start-gym' is invoked
"""
# get parameters from environment (set during docker launch) otherwise take default
map = os.getenv('DUCKIETOWN_MAP', DEFAULTS["map"])
domain_rand = bool(
os.getenv('DUCKIETOWN_DOMAIN_RAND', DEFAULTS["domain_rand"]))
max_steps = os.getenv('DUCKIETOWN_MAX_STEPS', DEFAULTS["max_steps"])
# if a challenge is set, it overrides the map selection
misc = {} # init of info field for additional gym data
challenge = os.getenv('DUCKIETOWN_CHALLENGE', "")
if challenge in ["LF", "LFV"]:
print("Launching challenge:", challenge)
map = DEFAULTS["challenges"][challenge]
misc["challenge"] = challenge
print("Using map:", map)
env = DuckietownEnv(
map_name=map,
# draw_curve = args.draw_curve,
# draw_bbox = args.draw_bbox,
max_steps=max_steps,
domain_rand=domain_rand)
obs = env.reset()
publisher_socket = None
command_socket, command_poll = make_pull_socket()
print("Simulator listening to incoming connections...")
while True:
if has_pull_message(command_socket, command_poll):
success, data = receive_data(command_socket)
if not success:
print(data) # in error case, this will contain the err msg
continue
reward = 0 # in case it's just a ping, not a motor command, we are sending a 0 reward
done = False # same thing... just for gym-compatibility
misc_ = {} # same same
if data["topic"] == 0:
obs, reward, done, misc_ = env.step(data["msg"])
if DEBUG:
print("challenge={}, step_count={}, reward={}, done={}".
format(challenge, env.unwrapped.step_count,
np.around(reward, 3), done))
if done:
env.reset()
if data["topic"] == 1:
print("received ping:", data)
if data["topic"] == 2:
obs = env.reset()
# can only initialize socket after first listener is connected - weird ZMQ bug
if publisher_socket is None:
publisher_socket = make_pub_socket(for_images=True)
if data["topic"] in [0, 1]:
misc.update(misc_)
send_gym(publisher_socket, obs, reward, done, misc)
import time
import zmq
from duckietown_slimremote.helpers import random_id, get_py_version
from duckietown_slimremote.networking import make_sub_socket, make_pub_socket, construct_action, recv_array, \
get_ip
print(get_py_version())
context = zmq.Context()
image_sub = make_sub_socket(for_images=True)
action_pub = make_pub_socket("quacksparrow.local")
poller = zmq.Poller()
poller.register(image_sub, zmq.POLLIN)
own_id = random_id()
own_ip = get_ip()
msg = construct_action(own_id, own_ip)
time.sleep(1) # wait for sock init
print("sending init", msg)
# init
action_pub.send_string(msg)
# during testing the following block took out the JSON header from the first image.
# In other words the hello message arrived too quickly.
def run_episode(env, data_logger, max_steps_per_episode, command_socket, command_poll, misc):
''' returns number of steps '''
observations = env.reset()
steps = 0
while steps < max_steps_per_episode:
# logger.debug('received: %s' % data)
reward = 0 # in case it's just a ping, not a motor command, we are sending a 0 reward
done = False # same thing... just for gym-compatibility
misc_ = {} # same same
data = get_next_data(command_socket, command_poll)
if data["topic"] == 0:
action = data['msg']
observations, reward, done, misc_ = env.step(action)
if not np.isfinite(reward):
msg = 'Invalid reward received: %s' % reward
raise Exception(msg)
# XXX cannot be serialized later if misc['vels'] is an array
if 'vels' in misc_:
misc_['vels'] = list(misc_['vels'])
delta_time = 1.0 / env.unwrapped.frame_rate
t = env.unwrapped.step_count * delta_time
data_logger.log_action(t=t, action=action)
data_logger.log_observations(t=t, observations=observations)
data_logger.log_reward(t=t, reward=reward)
steps += 1
# logger.debug('action: {}'.format(action))
# logger.debug('steps: {}'.format(steps))
# we log the current environment step
#
if DEBUG:
logger.info("step_count={}, reward={}, done={}".format(
env.unwrapped.step_count,
np.around(reward, 3),
done)
)
if done:
break
if data["topic"] == 1:
logger.debug("received ping:", data)
if data["topic"] == 2:
observations = env.reset()
# can only initialize socket after first listener is connected - weird ZMQ bug
if Global.publisher_socket is None:
Global.publisher_socket = make_pub_socket(for_images=True)
if data["topic"] in [0, 1]:
misc.update(misc_)
# print('sending misc = %s' % misc)
send_gym(Global.publisher_socket, observations, reward, done, misc)
else:
logger.info('breaking because steps = %s' % max_steps_per_episode)
return steps