def __init__(self):
signal.signal(signal.SIGINT, self.exit_gracefully)
signal.signal(signal.SIGTERM, self.exit_gracefully)
AIDONODE_DATA_IN = '/fifos/agent-in'
AIDONODE_DATA_OUT = '/fifos/agent-out'
logger.info('DuckiebotBridge starting communicating with the agent.')
self.ci = ComponentInterface(AIDONODE_DATA_IN,
AIDONODE_DATA_OUT,
expect_protocol=protocol_agent_DB20,
nickname='agent',
timeout=3600)
self.ci.write_topic_and_expect_zero('seed', 32)
self.ci.write_topic_and_expect_zero('episode_start',
{'episode_name': 'episode'})
logger.info(
'DuckiebotBridge successfully sent to the agent the seed and episode name.'
)
self.client = ROSClient()
logger.info('DuckiebotBridge has created ROSClient.')
def get_episodes(sm_ci: ComponentInterface, episodes_per_scenario: int,
seed: int) -> List[EpisodeSpec]:
sm_ci.write_topic_and_expect_zero('seed', seed)
def iterate_scenarios() -> Iterator[Scenario]:
while True:
recv = sm_ci.write_topic_and_expect('next_scenario')
if recv.topic == 'finished':
sm_ci.close()
break
else:
yield recv.data
episodes = []
for scenario in iterate_scenarios():
scenario_name = scenario.scenario_name
logger.info(f'Received scenario {scenario}')
for i in range(episodes_per_scenario):
episode_name = f'{scenario_name}-{i}'
es = EpisodeSpec(episode_name=episode_name, scenario=scenario)
episodes.append(es)
return episodes
def main(cie, log_dir, attempts):
config_ = env_as_yaml('experiment_manager_parameters')
logger.info('parameters:\n\n%s' % config_)
config = cast(MyConfig, ipce_to_object(config_, {}, expect_type=MyConfig))
# first open all fifos
agent_ci = ComponentInterface(config.agent_in, config.agent_out,
expect_protocol=protocol_agent, nickname="agent")
agents = [agent_ci]
sim_ci = ComponentInterface(config.sim_in, config.sim_out,
expect_protocol=protocol_simulator, nickname="simulator")
sm_ci = ComponentInterface(config.sm_in, config.sm_out,
expect_protocol=protocol_scenario_maker, nickname="scenario_maker")
# then check compatibility
# so that everything fails gracefully in case of error
timeout_initialization = 20
agent_ci._get_node_protocol(timeout=timeout_initialization)
sm_ci._get_node_protocol(timeout=timeout_initialization)
sim_ci._get_node_protocol(timeout=timeout_initialization)
check_compatibility_between_agent_and_sim(agent_ci, sim_ci)
attempt_i = 0
per_episode = {}
stats = {}
try:
nfailures = 0
sim_ci.write('seed', config.seed)
agent_ci.write('seed', config.seed)
episodes = get_episodes(sm_ci, episodes_per_scenario=config.episodes_per_scenario,
seed=config.seed)
while episodes:
if nfailures >= config.max_failures:
msg = 'Too many failures: %s' % nfailures
raise Exception(msg) # XXX
episode_spec = episodes[0]
episode_name = episode_spec.episode_name
logger.info('Starting episode %s' % episode_name)
dn_final = os.path.join(log_dir, episode_name)
if os.path.exists(dn_final):
shutil.rmtree(dn_final)
dn = os.path.join(attempts, episode_name + '.attempt%s' % attempt_i)
if os.path.exists(dn):
shutil.rmtree(dn)
if not os.path.exists(dn):
os.makedirs(dn)
fn = os.path.join(dn, 'log.gs2.cbor')
fn_tmp = fn + '.tmp'
fw = open(fn_tmp, 'wb')
agent_ci.cc(fw)
sim_ci.cc(fw)
logger.info('Now running episode')
num_playable = len([_ for _ in episode_spec.scenario.robots.values() if _.playable])
if num_playable != len(agents):
msg = f'The scenario requires {num_playable} robots, but I only know {len(agents)} agents.'
raise Exception(msg) # XXX
try:
length_s = run_episode(sim_ci,
agents,
episode_name=episode_name,
scenario=episode_spec.scenario,
episode_length_s=config.episode_length_s,
physics_dt=config.physics_dt)
logger.info('Finished episode %s' % episode_name)
except:
msg = 'Anomalous error from run_episode():\n%s' % traceback.format_exc()
logger.error(msg)
raise
finally:
fw.close()
os.rename(fn_tmp, fn)
# output = os.path.join(dn, 'visualization')
logger.info('Now creating visualization and analyzing statistics.')
logger.warning('This might take a LONG time.')
with notice_thread("Visualization", 5):
evaluated = read_and_draw(fn, dn)
logger.info('Finally visualization is done.')
stats = {}
for k, evr in evaluated.items():
assert isinstance(evr, RuleEvaluationResult)
for m, em in evr.metrics.items():
assert isinstance(em, EvaluatedMetric)
assert isinstance(m, tuple)
if m:
M = "/".join(m)
else:
M = k
stats[M] = float(em.total)
per_episode[episode_name] = stats
if length_s >= config.min_episode_length_s:
logger.info('%1.f s are enough' % length_s)
episodes.pop(0)
out_video = os.path.join(dn, 'camera.mp4')
make_video1(fn, out_video)
os.rename(dn, dn_final)
else:
logger.error('episode too short with %1.f s < %.1f s' % (length_s, config.min_episode_length_s))
nfailures += 1
attempt_i += 1
except dc.InvalidSubmission:
raise
except BaseException as e:
msg = 'Anomalous error while running episodes:\n%s' % traceback.format_exc()
logger.error(msg)
raise dc.InvalidEvaluator(msg) from e
finally:
agent_ci.close()
sim_ci.close()
logger.info('Simulation done.')
cie.set_score('per-episodes', per_episode)
for k in list(stats):
values = [_[k] for _ in per_episode.values()]
cie.set_score('%s_mean' % k, float(np.mean(values)))
cie.set_score('%s_median' % k, float(np.median(values)))
cie.set_score('%s_min' % k, float(np.min(values)))
cie.set_score('%s_max' % k, float(np.max(values)))
def main():
# Set in the docker-compose env section
config_ = env_as_yaml('middleware_parameters')
logger.info('parameters:\n\n%s' % config_)
config = cast(MyConfig, object_from_ipce(config_, MyConfig))
# first open all fifos
logger.info("Opening the sim CI")
sim_ci = ComponentInterface(config.sim_in,
config.sim_out,
expect_protocol=protocol_simulator,
nickname="simulator",
timeout=config.timeout_regular)
logger.info("Pipes connected to simulator")
logger.info("Opening the agent CI")
agent_ci = ComponentInterface(config.agent_in,
config.agent_out,
expect_protocol=protocol_agent,
nickname="agent",
timeout=config.timeout_regular)
logger.info("Pipes connected to agent")
agents = [agent_ci]
# We enable CC of the pipes for debugging
if logger.level < logging.DEBUG:
logfile = "/fifos/agentlog"
logfile2 = "/fifos/simlog"
ff = open(logfile, "wb")
ff2 = open(logfile, "wb")
agent_ci.cc(ff)
sim_ci.cc(ff2)
logger.info(f"opened {logfile} as agent cc")
logger.info(f"opened {logfile2} as sim cc")
# then check compatibility
# so that everything fails gracefully in case of error
logger.info("Checking sim protocol compatibility...")
sim_ci._get_node_protocol(timeout=config.timeout_initialization)
logger.info(
"Sim protocol compatible, checking agent protocol compatibility...")
agent_ci._get_node_protocol(timeout=config.timeout_initialization)
logger.info("Acquired nodes protocols")
check_compatibility_between_agent_and_sim(agent_ci, sim_ci)
logger.info("Compatibility verified.")
attempt_i = 0
ep = 0
try:
nfailures = 0
logger.info("Sending seed to sim")
sim_ci.write_topic_and_expect_zero('seed', config.seed)
logger.info("Sending seed to agent")
agent_ci.write_topic_and_expect_zero('seed', config.seed)
logger.info("Received feedback from peer containers")
# TODO we should have a proper handling of invalid map name
map_name = os.environ.get('MAP_NAME', 'loop_empty')
yaml_string: str = _get_map_yaml(map_name)
yaml_data = yaml.load(yaml_string, Loader=yaml.SafeLoader)
placed_obj = construct_map(yaml_data)
pose = sample_good_starting_pose(placed_obj, only_straight=True)
vel = geometry.se2_from_linear_angular([0, 0], 0)
logger.info(f"Got good starting pose at: {pose}")
robot1_config = RobotConfiguration(pose=pose, velocity=vel)
robot1 = ScenarioRobotSpec(description="Development agent",
playable=True,
configuration=robot1_config,
motion=None)
scenario1 = Scenario("scenario1",
environment=yaml_string,
robots={"agent1": robot1})
unique_episode = EpisodeSpec("episode1", scenario1)
episodes = [unique_episode]
# Since we dont have a scenario maker, we will loop the episode (for now)
while episodes:
if nfailures >= config.max_failures:
msg = 'Too many failures: %s' % nfailures
raise Exception(msg) # XXX
episode_spec = episodes[0]
episode_name = episode_spec.episode_name
ep += 1
episode_name = f'episode_{ep}'
logger.info(f'Starting {episode_name}')
# dn_final = os.path.join(log_dir, episode_name)
# if os.path.exists(dn_final):
# shutil.rmtree(dn_final)
# dn = os.path.join(attempts, episode_name + '.attempt%s' % attempt_i)
# if os.path.exists(dn):
# shutil.rmtree(dn)
# if not os.path.exists(dn):
# os.makedirs(dn)
# fn = os.path.join(dn, 'log.gs2.cbor')
# fn_tmp = fn + '.tmp'
# fw = open(fn_tmp, 'wb')
# agent_ci.cc(fw)
# sim_ci.cc(fw)
logger.info('Now running episode')
num_playable = len([
_ for _ in episode_spec.scenario.robots.values() if _.playable
])
if num_playable != len(agents):
msg = f'The scenario requires {num_playable} robots, but I only know {len(agents)} agents.'
raise Exception(msg) # XXX
try:
length_s = run_episode(
sim_ci,
agents,
episode_name=episode_name,
scenario=episode_spec.scenario,
episode_length_s=config.episode_length_s,
physics_dt=config.physics_dt)
logger.info('Finished episode %s' % episode_name)
except:
msg = 'Anomalous error from run_episode():\n%s' % traceback.format_exc(
)
logger.error(msg)
raise
# finally:
# fw.close()
# os.rename(fn_tmp, fn)
# output = os.path.join(dn, 'visualization')
# logger.info('Now creating visualization and analyzing statistics.')
# logger.warning('This might take a LONG time.')
#
# with notice_thread("Visualization", 2):
# evaluated = read_and_draw(fn, dn)
# logger.info('Finally visualization is done.')
# stats = {}
# for k, evr in evaluated.items():
# assert isinstance(evr, RuleEvaluationResult)
# for m, em in evr.metrics.items():
# assert isinstance(em, EvaluatedMetric)
# assert isinstance(m, tuple)
# if m:
# M = "/".join(m)
# else:
# M = k
# stats[M] = float(em.total)
# per_episode[episode_name] = stats
if length_s >= config.min_episode_length_s:
logger.info('%1.f s are enough' % length_s)
# episodes.pop(0) #@ We dont pop the episode, run in loop
# out_video = os.path.join(dn, 'camera.mp4')
# with notice_thread("Make video", 2):
# make_video1(fn, out_video)
# os.rename(dn, dn_final)
else:
logger.error('episode too short with %1.f s < %.1f s' %
(length_s, config.min_episode_length_s))
nfailures += 1
attempt_i += 1
except dc.InvalidSubmission:
raise
except BaseException as e:
msg = 'Anomalous error while running episodes:'
msg += '\n\n' + indent(traceback.format_exc(), ' > ')
logger.error(msg)
raise dc.InvalidEvaluator(msg) from e
finally:
agent_ci.close()
sim_ci.close()
logger.info('Simulation done.')
def log_timing_info(tt, sim_ci: ComponentInterface):
ipce = ipce_from_object(tt)
msg = {'compat': ['aido2'], 'topic': 'timing_information', 'data': ipce}
j = sim_ci._serialize(msg)
sim_ci._cc.write(j)
sim_ci._cc.flush()
def run_episode(sim_ci: ComponentInterface, agents: List[ComponentInterface],
physics_dt: float, episode_name, scenario: Scenario,
episode_length_s: float) -> float:
''' returns number of steps '''
# clear simulation
sim_ci.write_topic_and_expect_zero('clear')
logger.info("Sent clear request so sim")
# set map data
sim_ci.write_topic_and_expect_zero('set_map',
SetMap(map_data=scenario.environment))
logger.info("Map has been set properly")
# spawn robot
for robot_name, robot_conf in scenario.robots.items():
sim_ci.write_topic_and_expect_zero(
'spawn_robot',
SpawnRobot(robot_name=robot_name,
configuration=robot_conf.configuration,
playable=robot_conf.playable,
motion=None))
# start episode
sim_ci.write_topic_and_expect_zero('episode_start',
EpisodeStart(episode_name))
logger.info("Sent episode start to sim")
for agent in agents:
agent.write_topic_and_expect_zero('episode_start',
EpisodeStart(episode_name))
logger.info("Sent episode start to an agent")
current_sim_time = 0.0
# for now, fixed timesteps
steps = 0
playable_robots = [
_ for _ in scenario.robots if scenario.robots[_].playable
]
not_playable_robots = [
_ for _ in scenario.robots if not scenario.robots[_].playable
]
playable_robots2agent: Dict[str, ComponentInterface] = {
_: v
for _, v in zip(playable_robots, agents)
}
while True:
if current_sim_time >= episode_length_s:
logger.info('Reached %1.f seconds. Finishing. ' % episode_length_s)
break
tt = TimeTracker(steps)
t_effective = current_sim_time
for robot_name in playable_robots:
agent = playable_robots2agent[robot_name]
# have this first, so we have something for t = 0
with tt.measure(f'sim_compute_robot_state-{robot_name}'):
grs = GetRobotState(robot_name=robot_name,
t_effective=t_effective)
_recv: MsgReceived[RobotState] = \
sim_ci.write_topic_and_expect('get_robot_state', grs,
expect='robot_state')
with tt.measure(f'sim_compute_performance-{robot_name}'):
_recv: MsgReceived[RobotPerformance] = \
sim_ci.write_topic_and_expect('get_robot_performance',
robot_name,
expect='robot_performance')
with tt.measure(f'sim_render-{robot_name}'):
gro = GetRobotObservations(robot_name=robot_name,
t_effective=t_effective)
recv: MsgReceived[RobotObservations] = \
sim_ci.write_topic_and_expect('get_robot_observations', gro,
expect='robot_observations')
with tt.measure(f'agent_compute-{robot_name}'):
try:
logger.debug("Sending observation to agent")
agent.write_topic_and_expect_zero('observations',
recv.data.observations)
gc = GetCommands(at_time=time.time())
logger.debug("Querying commands to agent")
r: MsgReceived = agent.write_topic_and_expect(
'get_commands', gc, expect='commands')
except BaseException as e:
msg = 'Trouble with communication to the agent.'
raise dc.InvalidSubmission(msg) from e
with tt.measure('set_robot_commands'):
commands = SetRobotCommands(robot_name=robot_name,
commands=r.data,
t_effective=t_effective)
sim_ci.write_topic_and_expect_zero('set_robot_commands',
commands)
for robot_name in not_playable_robots:
with tt.measure(f'sim_compute_robot_state-{robot_name}'):
logger.debug("get robot state")
rs = GetRobotState(robot_name=robot_name,
t_effective=t_effective)
_recv: MsgReceived[RobotState] = \
sim_ci.write_topic_and_expect('get_robot_state', rs,
expect='robot_state')
with tt.measure('sim_compute_sim_state'):
logger.debug("Computing sim state")
recv: MsgReceived[SimulationState] = \
sim_ci.write_topic_and_expect('get_sim_state', expect='sim_state')
sim_state: SimulationState = recv.data
if sim_state.done:
logger.info(
f'Breaking because of simulator ({sim_state.done_code} - {sim_state.done_why}'
)
break
with tt.measure('sim_physics'):
logger.debug("calling sim_step")
current_sim_time += physics_dt
sim_ci.write_topic_and_expect_zero('step', Step(current_sim_time))
# Following line disabled because we don't have a cc, if we need logging set sim_ci._cc and enable
# log_timing_info(tt, sim_ci)
return current_sim_time
def main():
# Set in the docker-compose env section
config_ = env_as_yaml('middleware_parameters')
logger.info('parameters:\n\n%s' % config_)
config = cast(MyConfig, object_from_ipce(config_, MyConfig))
# first open all fifos
logger.info("Opening the sim CI")
sim_ci = ComponentInterface(config.sim_in,
config.sim_out,
expect_protocol=protocol_simulator,
nickname="simulator",
timeout=config.timeout_regular)
#logfile = "/fifos3/simlog"
#ff = open(logfile,"wb")
#sim_ci.cc(ff)
#logger.info(f"opened {logfile} as cc")
logger.info("Opening the agent CI")
#agent_ci = ComponentInterface(config.agent_in, config.agent_out,
# expect_protocol=protocol_agent, nickname="agent",
# timeout=config.timeout_regular)
#agents = [agent_ci]
# sm_ci = ComponentInterface(config.sm_in, config.sm_out,
# expect_protocol=protocol_scenario_maker, nickname="scenario_maker",
# timeout=config.timeout_regular)
# then check compatibility
# so that everything fails gracefully in case of error
# sm_ci._get_node_protocol(timeout=config.timeout_initialization)
logger.info("Checking sim protocol compatibility")
sim_ci._get_node_protocol(timeout=config.timeout_initialization)
logger.info(">Verified sim_ci compatibility, pipes are connected !")
logger.info("Checking agent protocol compatibility")
agent_ci._get_node_protocol(timeout=config.timeout_initialization)
logger.info("Acquired node protocol")
check_compatibility_between_agent_and_sim(agent_ci, sim_ci)
logger.info("Compatibility verified.")
attempt_i = 0
# per_episode = {}
# stats = {}
quit_loop = False
ep = 0
try:
nfailures = 0
logger.info("Sending seed to sim")
sim_ci.write_topic_and_expect_zero('seed', config.seed)
logger.info("Sending seed to agent")
agent_ci.write_topic_and_expect_zero('seed', config.seed)
logger.info("Received feedback from peer containers")
# TODO this should be in docker-compose or yaml
map_name = "loop_empty"
robot1_config = RobotConfiguration(pose=0.0, velocity=0.0)
robot1 = ScenarioRobotSpec(description="Development agent",
playable=True,
configuration=robot1_config)
environment1 = _get_map_yaml(map_name)
scenario1 = Scenario("scenario1",
environment=environment1,
robots={"agent1": robot1})
unique_episode = EpisodeSpec("episode1", scenario1)
# episodes = get_episodes(sm_ci, episodes_per_scenario=config.episodes_per_scenario,
# seed=config.seed)
episodes = [unique_episode]
# while episodes:
while episodes:
if nfailures >= config.max_failures:
msg = 'Too many failures: %s' % nfailures
raise Exception(msg) # XXX
episode_spec = episodes[0]
episode_name = episode_spec.episode_name
ep += 1
episode_name = f'episode_{ep}'
logger.info('Starting episode 1')
#dn_final = os.path.join(log_dir, episode_name)
#if os.path.exists(dn_final):
# shutil.rmtree(dn_final)
#dn = os.path.join(attempts, episode_name + '.attempt%s' % attempt_i)
#if os.path.exists(dn):
# shutil.rmtree(dn)
#if not os.path.exists(dn):
# os.makedirs(dn)
#fn = os.path.join(dn, 'log.gs2.cbor')
#fn_tmp = fn + '.tmp'
#fw = open(fn_tmp, 'wb')
#agent_ci.cc(fw)
#sim_ci.cc(fw)
logger.info('Now running episode')
num_playable = len([
_ for _ in episode_spec.scenario.robots.values() if _.playable
])
if num_playable != len(agents):
msg = f'The scenario requires {num_playable} robots, but I only know {len(agents)} agents.'
raise Exception(msg) # XXX
try:
length_s = run_episode(
sim_ci,
agents,
episode_name=episode_name,
scenario=episode_spec.scenario,
episode_length_s=config.episode_length_s,
physics_dt=config.physics_dt)
logger.info('Finished episode %s' % episode_name)
except:
msg = 'Anomalous error from run_episode():\n%s' % traceback.format_exc(
)
logger.error(msg)
raise
finally:
fw.close()
os.rename(fn_tmp, fn)
# output = os.path.join(dn, 'visualization')
# logger.info('Now creating visualization and analyzing statistics.')
# logger.warning('This might take a LONG time.')
#
# with notice_thread("Visualization", 2):
# evaluated = read_and_draw(fn, dn)
# logger.info('Finally visualization is done.')
# stats = {}
# for k, evr in evaluated.items():
# assert isinstance(evr, RuleEvaluationResult)
# for m, em in evr.metrics.items():
# assert isinstance(em, EvaluatedMetric)
# assert isinstance(m, tuple)
# if m:
# M = "/".join(m)
# else:
# M = k
# stats[M] = float(em.total)
# per_episode[episode_name] = stats
if length_s >= config.min_episode_length_s:
logger.info('%1.f s are enough' % length_s)
# episodes.pop(0) #@ We dont pop the episode, run in loop
# out_video = os.path.join(dn, 'camera.mp4')
# with notice_thread("Make video", 2):
# make_video1(fn, out_video)
os.rename(dn, dn_final)
else:
logger.error('episode too short with %1.f s < %.1f s' %
(length_s, config.min_episode_length_s))
nfailures += 1
attempt_i += 1
except dc.InvalidSubmission:
raise
except BaseException as e:
msg = 'Anomalous error while running episodes:'
msg += '\n\n' + indent(traceback.format_exc(), ' > ')
logger.error(msg)
raise dc.InvalidEvaluator(msg) from e
finally:
agent_ci.close()
sim_ci.close()
logger.info('Simulation done.')
class DuckiebotBridge:
def __init__(self):
signal.signal(signal.SIGINT, self.exit_gracefully)
signal.signal(signal.SIGTERM, self.exit_gracefully)
AIDONODE_DATA_IN = '/fifos/agent-in'
AIDONODE_DATA_OUT = '/fifos/agent-out'
logger.info('DuckiebotBridge starting communicating with the agent.')
self.ci = ComponentInterface(AIDONODE_DATA_IN,
AIDONODE_DATA_OUT,
expect_protocol=protocol_agent_DB20,
nickname='agent',
timeout=3600)
self.ci.write_topic_and_expect_zero('seed', 32)
self.ci.write_topic_and_expect_zero('episode_start',
{'episode_name': 'episode'})
logger.info(
'DuckiebotBridge successfully sent to the agent the seed and episode name.'
)
self.client = ROSClient()
logger.info('DuckiebotBridge has created ROSClient.')
def exit_gracefully(self, signum, frame):
logger.info('DuckiebotBridge exiting gracefully.')
sys.exit(0)
def run(self):
nimages_received = 0
t0 = time.time()
t_last_received = None
while True:
if not self.client.initialized:
if nimages_received == 0:
elapsed = time.time() - t0
msg = 'DuckiebotBridge still waiting for the first image: elapsed %s' % elapsed
logger.info(msg)
time.sleep(0.5)
else:
elapsed = time.time() - t_last_received
if elapsed > 2:
msg = 'DuckiebotBridge has waited %s since last image' % elapsed
logger.info(msg)
time.sleep(0.5)
else:
time.sleep(0.01)
continue
jpg_data = self.client.image_data
camera = JPGImage(jpg_data)
resolution_rad: float = float(np.pi * 2 / 135)
axis_left_rad: float = float(self.client.left_encoder_ticks *
self.client.resolution_rad)
axis_right_rad: float = float(self.client.right_encoder_ticks *
self.client.resolution_rad)
odometry = DB20Odometry(axis_left_rad=axis_left_rad,
axis_right_rad=axis_right_rad,
resolution_rad=resolution_rad)
obs = DB20Observations(camera, odometry)
if nimages_received == 0:
logger.info('DuckiebotBridge got the first image from ROS.')
self.ci.write_topic_and_expect_zero('observations', obs)
gc = GetCommands(at_time=time.time())
r: MsgReceived = self.ci.write_topic_and_expect('get_commands',
gc,
expect='commands')
wheels = r.data.wheels
lw, rw = wheels.motor_left, wheels.motor_right
commands = {u'motor_right': rw, u'motor_left': lw}
self.client.send_commands(commands)
if nimages_received == 0:
logger.info('DuckiebotBridge published the first commands.')
nimages_received += 1
t_last_received = time.time()