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"]))
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)
def main():
# pulling out of the environment
MAP_NAME = os.getenv('DTG_MAP')
environment = os.environ.copy()
def env_as_json(name):
if not name in environment:
msg = 'Could not find variable "%s"; I know:\n%s' % (name, json.dumps(environment, indent=4))
raise Exception(msg)
return json.loads(environment[name])
DOMAIN_RAND = env_as_json('DTG_DOMAIN_RAND')
EPISODES = env_as_json('DTG_EPISODES')
STEPS_PER_EPISODE = env_as_json('DTG_STEPS_PER_EPISODE')
challenge = environment['DTG_CHALLENGE']
LOG_DIR = environment['DTG_LOG_DIR']
camera_width = env_as_json('DTG_CAMERA_WIDTH')
camera_height = env_as_json('DTG_CAMERA_HEIGHT')
misc = {} # init of info field for additional gym data
misc['challenge'] = challenge
# if challenge in ["LF", "LFV"]:
# logger.debug("Launching challenge: {}".format(challenge))
# from gym_duckietown.config import DEFAULTS
#
# MAP_NAME = DEFAULTS["challenges"][challenge]
# misc["challenge"] = challenge
# else:
# pass
# XXX: what if not? error?
logger.debug("Using map: {}".format(MAP_NAME))
env_parameters = dict(map_name=MAP_NAME,
max_steps=STEPS_PER_EPISODE * EPISODES,
domain_rand=DOMAIN_RAND,
camera_width=camera_width,
camera_height=camera_height,
)
env = DuckietownEnv(**env_parameters)
command_socket, command_poll = make_pull_socket()
logger.debug("Simulator listening to incoming connections...")
observations = env.reset()
logger.debug('Logging gym state to: {}'.format(LOG_DIR))
data_logger = ROSLogger(logdir=LOG_DIR)
min_nsteps = 10
MAX_FAILURES = 5
nfailures = 0
episodes = ['ep%03d' % _ for _ in range(EPISODES)]
try:
while episodes:
if nfailures >= MAX_FAILURES:
msg = 'Too many failures: %s' % nfailures
raise Exception(msg) # XXX
episode_name = episodes[0]
logger.info('Starting episode %s' % episode_name)
data_logger.start_episode(episode_name)
data_logger.log_misc(env_parameters)
try:
nsteps = run_episode(env, data_logger, max_steps_per_episode=STEPS_PER_EPISODE,
command_socket=command_socket,
command_poll=command_poll,
misc=misc)
logger.info('Finished episode %s' % episode_name)
if nsteps >= min_nsteps:
logger.info('%d steps are enough' % nsteps)
episodes.pop(0)
else:
logger.error('episode too short with %s steps' % nsteps)
nfailures += 1
finally:
data_logger.end_episode()
finally:
data_logger.close()
logger.info('Simulation done.')
misc['simulation_done'] = True
send_gym(
socket=Global.publisher_socket,
img=observations,
reward=0.0,
done=True,
misc=misc
)
logger.info('Clean exit.')
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
