def get_base_scenario(scenario_name: str, nduckies: int, ntiles: int, min_dist_from_other_duckie: float) -> Scenario: tile_size = 0.585 themap = {"tiles": [], "tile_size": tile_size} themap["tiles"] = [["asphalt"] * ntiles] * ntiles area = RectangularArea([0, 0], [tile_size * ntiles, tile_size * ntiles]) robots = {} L = tile_size * (ntiles / 2) # x = L / 8 # y = L / 2 pose = SE2_from_xytheta([0.2, 0.2, np.deg2rad(45)]) vel = FriendlyVelocity(0.0, 0.0, 0.0) robots["ego0"] = ScenarioRobotSpec( color="blue", configuration=RobotConfiguration(pose, vel), controllable=True, protocol=PROTOCOL_NORMAL, description="", ) yaml_str = yaml.dump(themap) duckie_poses = sample_duckies( nduckies, [pose_from_friendly(robots["ego0"].configuration.pose)], min_dist_from_robot=0.4, min_dist_from_other_duckie=min_dist_from_other_duckie, bounds=area, ) duckies = {} for i, pose in enumerate(duckie_poses): fpose = friendly_from_pose(pose) duckies[f"duckie{i}"] = ScenarioDuckieSpec("yellow", fpose) payload = {"info": "fill here the yaml"} ms = Scenario( payload_yaml=yaml.dump(payload), scenario_name=scenario_name, environment=yaml_str, robots=robots, duckies=duckies, player_robots=list(robots), ) return ms
def get_base_scenario(scenario_name: str, nduckies: int, ntiles: int) -> Scenario: tile_size = 0.585 themap = {"tiles": [], "tile_size": tile_size} themap["tiles"] = [["asphalt"] * ntiles] * ntiles area = RectangularArea([0, 0], [tile_size * ntiles, tile_size * ntiles]) robots = {} L = tile_size * (ntiles / 2) x = L / 8 y = L / 2 pose = SE2_from_xytheta([x, y, 0]) vel = np.zeros((3, 3)) robots["ego0"] = ScenarioRobotSpec( color="blue", configuration=RobotConfiguration(pose, vel), controllable=True, protocol=PROTOCOL_NORMAL, description="", ) yaml_str = yaml.dump(themap) duckie_poses = sample_duckies( nduckies, [robots["ego0"].configuration.pose], min_dist_from_robot=0.4, min_dist_from_other_duckie=0.3, bounds=area, ) duckies = {} for i, pose in enumerate(duckie_poses): duckies[f"duckie{i}"] = ScenarioDuckieSpec("yellow", pose) ms = Scenario( scenario_name=scenario_name, environment=yaml_str, robots=robots, duckies=duckies, player_robots=list(robots), ) return ms
def make_scenario( yaml_str: str, scenario_name: str, only_straight: bool, min_dist: float, delta_y_m: float, delta_theta_rad: float, robots_pcs: List[RobotName], robots_npcs: List[RobotName], robots_parked: List[RobotName], nduckies: int, duckie_min_dist_from_other_duckie: float, duckie_min_dist_from_robot: float, tree_density: float, tree_min_dist: float, duckie_y_bounds: Sequence[float], pc_robot_protocol: ProtocolDesc = PROTOCOL_NORMAL, npc_robot_protocol: ProtocolDesc = PROTOCOL_FULL, ) -> Scenario: yaml_data = yaml.load(yaml_str, Loader=yaml.SafeLoader) if "objects" not in yaml_data: yaml_data["objects"] = {} objects = yaml_data["objects"] if isinstance(objects, list): raise ZValueError(yaml_data=yaml_data) po = dw.construct_map(yaml_data) num_pcs = len(robots_pcs) num_npcs = len(robots_npcs) num_parked = len(robots_parked) nrobots = num_npcs + num_pcs + num_parked all_robot_poses = sample_many_good_starting_poses( po, nrobots, only_straight=only_straight, min_dist=min_dist, delta_theta_rad=delta_theta_rad, delta_y_m=delta_y_m, ) remaining_robot_poses = list(all_robot_poses) poses_pcs = remaining_robot_poses[:num_pcs] remaining_robot_poses = remaining_robot_poses[num_pcs:] # poses_npcs = remaining_robot_poses[:num_npcs] remaining_robot_poses = remaining_robot_poses[num_npcs:] # poses_parked = remaining_robot_poses[:num_parked] remaining_robot_poses = remaining_robot_poses[num_parked:] assert len(remaining_robot_poses) == 0 COLORS_PLAYABLE = ["red", "green", "blue"] COLOR_NPC = "blue" COLOR_PARKED = "grey" robots = {} for i, robot_name in enumerate(robots_pcs): pose = poses_pcs[i] fpose = friendly_from_pose(pose) velocity = FriendlyVelocity(0.0, 0.0, 0.0) configuration = RobotConfiguration(fpose, velocity) color = COLORS_PLAYABLE[i % len(COLORS_PLAYABLE)] robots[robot_name] = ScenarioRobotSpec( description=f"Playable robot {robot_name}", controllable=True, configuration=configuration, # motion=None, color=color, protocol=pc_robot_protocol, ) for i, robot_name in enumerate(robots_npcs): pose = poses_npcs[i] fpose = friendly_from_pose(pose) velocity = FriendlyVelocity(0.0, 0.0, 0.0) configuration = RobotConfiguration(fpose, velocity) robots[robot_name] = ScenarioRobotSpec( description=f"NPC robot {robot_name}", controllable=True, configuration=configuration, color=COLOR_NPC, protocol=npc_robot_protocol, ) for i, robot_name in enumerate(robots_parked): pose = poses_parked[i] fpose = friendly_from_pose(pose) velocity = FriendlyVelocity(0.0, 0.0, 0.0) configuration = RobotConfiguration(fpose, velocity) robots[robot_name] = ScenarioRobotSpec( description=f"Parked robot {robot_name}", controllable=False, configuration=configuration, # motion=MOTION_PARKED, color=COLOR_PARKED, protocol=None, ) # logger.info(duckie_y_bounds=duckie_y_bounds) names = [f"duckie{i:02d}" for i in range(nduckies)] poses = sample_duckies_poses( po, nduckies, robot_positions=all_robot_poses, min_dist_from_other_duckie=duckie_min_dist_from_other_duckie, min_dist_from_robot=duckie_min_dist_from_robot, from_side_bounds=(duckie_y_bounds[0], duckie_y_bounds[1]), delta_theta_rad=np.pi, ) d = [ScenarioDuckieSpec("yellow", friendly_from_pose(_)) for _ in poses] duckies = dict(zip(names, d)) trees = sample_trees(po, tree_density, tree_min_dist) for i, pose_tree in enumerate(trees): obname = f"tree_random_{i}" st = dw.SE2Transform.from_SE2(pose_tree) objects[obname] = { "kind": "tree", "pose": st.as_json_dict(), "height": random.uniform(0.15, 0.4) } add_signs(po, objects) yaml_str = yaml.dump(yaml_data) # logger.info(trees=trees) payload = {"info": "fill here the yaml"} ms = Scenario( payload_yaml=yaml.dump(payload), scenario_name=scenario_name, environment=yaml_str, robots=robots, duckies=duckies, player_robots=list(robots_pcs), ) return ms
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 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.')
def make_scenario( yaml_str: str, scenario_name: str, only_straight: bool, min_dist: float, delta_y_m: float, delta_theta_rad: float, robots_pcs: List[RobotName], robots_npcs: List[RobotName], robots_parked: List[RobotName], nduckies: int, duckie_min_dist_from_other_duckie: float, duckie_min_dist_from_robot: float, duckie_y_bounds: Sequence[float], ) -> Scenario: yaml_data = yaml.load(yaml_str, Loader=yaml.SafeLoader) po = dw.construct_map(yaml_data) num_pcs = len(robots_pcs) num_npcs = len(robots_npcs) num_parked = len(robots_parked) nrobots = num_npcs + num_pcs + num_parked poses = sample_many_good_starting_poses( po, nrobots, only_straight=only_straight, min_dist=min_dist, delta_theta_rad=delta_theta_rad, delta_y_m=delta_y_m, ) poses_pcs = poses[:num_pcs] poses = poses[num_pcs:] # poses_npcs = poses[:num_npcs] poses = poses[num_npcs:] # poses_parked = poses[:num_parked] poses = poses[num_parked:] assert len(poses) == 0 COLOR_PLAYABLE = "red" COLOR_NPC = "blue" COLOR_PARKED = "grey" robots = {} for i, robot_name in enumerate(robots_pcs): pose = poses_pcs[i] vel = g.se2_from_linear_angular([0, 0], 0) configuration = RobotConfiguration(pose=pose, velocity=vel) robots[robot_name] = ScenarioRobotSpec( description=f"Playable robot {robot_name}", controllable=True, configuration=configuration, # motion=None, color=COLOR_PLAYABLE, protocol=PROTOCOL_NORMAL, ) for i, robot_name in enumerate(robots_npcs): pose = poses_npcs[i] vel = g.se2_from_linear_angular([0, 0], 0) configuration = RobotConfiguration(pose=pose, velocity=vel) robots[robot_name] = ScenarioRobotSpec( description=f"NPC robot {robot_name}", controllable=True, configuration=configuration, # motion=MOTION_MOVING, color=COLOR_NPC, protocol=PROTOCOL_FULL, ) for i, robot_name in enumerate(robots_parked): pose = poses_parked[i] vel = g.se2_from_linear_angular([0, 0], 0) configuration = RobotConfiguration(pose=pose, velocity=vel) robots[robot_name] = ScenarioRobotSpec( description=f"Parked robot {robot_name}", controllable=False, configuration=configuration, # motion=MOTION_PARKED, color=COLOR_PARKED, protocol=None, ) # logger.info(duckie_y_bounds=duckie_y_bounds) names = [f"duckie{i:02d}" for i in range(nduckies)] poses = sample_duckies_poses( po, nduckies, robot_positions=poses, min_dist_from_other_duckie=duckie_min_dist_from_other_duckie, min_dist_from_robot=duckie_min_dist_from_robot, from_side_bounds=(duckie_y_bounds[0], duckie_y_bounds[1]), delta_theta_rad=np.pi, ) d = [ScenarioDuckieSpec("yellow", _) for _ in poses] duckies = dict(zip(names, d)) ms = Scenario( scenario_name=scenario_name, environment=yaml_str, robots=robots, duckies=duckies, player_robots=list(robots_pcs), ) return ms