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()