def _create_scenarios(self, context: Context): available = list_maps() for map_name in self.config.maps: if not map_name in available: msg = f'Cannot find map name "{map_name}, know {available}' raise Exception(msg) s: str = _get_map_yaml(map_name) # yaml_str = update_map(yaml_str) yaml_data = yaml.load(s, Loader=yaml.SafeLoader) update_map(yaml_data) yaml_str = yaml.dump(yaml_data) po = dw.construct_map(yaml_data) for imap in range(self.config.scenarios_per_map): scenario_name = f'{map_name}-{imap}' nrobots = self.config.robots_npcs + self.config.robots_pcs poses = sample_many_good_starting_poses(po, nrobots, only_straight=self.config.only_straight, min_dist=self.config.min_dist, delta_theta_rad=np.deg2rad(self.config.theta_tol_deg), delta_y_m=self.config.dist_tol_m) poses_pcs = poses[:self.config.robots_pcs] poses_npcs = poses[self.config.robots_pcs:] robots = {} for i in range(self.config.robots_pcs): pose = poses_pcs[i] vel = g.se2_from_linear_angular([0, 0], 0) robot_name = 'ego' if i == 0 else "player%d" % i configuration = RobotConfiguration(pose=pose, velocity=vel) robots[robot_name] = ScenarioRobotSpec(description='Playable robot', playable=True, configuration=configuration) for i in range(self.config.robots_npcs): pose = poses_npcs[i] vel = g.se2_from_linear_angular([0, 0], 0) robot_name = "npc%d" % i configuration = RobotConfiguration(pose=pose, velocity=vel) robots[robot_name] = ScenarioRobotSpec(description='NPC robot', playable=False, configuration=configuration) ms = MyScenario(scenario_name=scenario_name, environment=yaml_str, robots=robots) self.state.scenarios_to_go.append(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 tag_positions(): map_yaml_data = yaml.load(map_yaml, Loader=yaml.Loader) m = construct_map(map_yaml_data) print(get_object_tree(m, attributes=True)) outdir = get_comptests_output_dir() draw_static(m, outdir)
def test_pwm1(): parameters = get_DB18_nominal(delay=0) # initial configuration init_pose = np.array([0, 0.8]) init_vel = np.array([0, 0]) q0 = geo.SE2_from_R2(init_pose) v0 = geo.se2_from_linear_angular(init_vel, 0) tries = { "straight_50": (PWMCommands(+0.5, 0.5)), "straight_max": (PWMCommands(+1.0, +1.0)), "straight_over_max": (PWMCommands(+1.5, +1.5)), "pure_left": (PWMCommands(motor_left=-0.5, motor_right=+0.5)), "pure_right": (PWMCommands(motor_left=+0.5, motor_right=-0.5)), "slight-forward-left": (PWMCommands(motor_left=0, motor_right=0.25)), "faster-forward-right": (PWMCommands(motor_left=0.5, motor_right=0)), # 'slight-right': (PWMCommands(-0.1, 0.1)), } dt = 0.03 t_max = 10 map_data_yaml = """ tiles: - [floor/W,floor/W, floor/W, floor/W, floor/W] - [straight/W , straight/W , straight/W, straight/W, straight/W] - [floor/W,floor/W, floor/W, floor/W, floor/W] tile_size: 0.61 """ map_data = yaml.load(map_data_yaml) root = construct_map(map_data) timeseries = {} for id_try, commands in tries.items(): seq = integrate_dynamics(parameters, q0, v0, dt, t_max, commands) ground_truth = seq.transform_values( lambda t: SE2Transform.from_SE2(t[0])) poses = seq.transform_values(lambda t: t[0]) velocities = get_velocities_from_sequence(poses) linear = velocities.transform_values(linear_from_se2) angular = velocities.transform_values(angular_from_se2) # print(linear.values) # print(angular.values) root.set_object(id_try, DB18(), ground_truth=ground_truth) sequences = {} sequences["motor_left"] = seq.transform_values( lambda _: commands.motor_left) sequences["motor_right"] = seq.transform_values( lambda _: commands.motor_right) plots = TimeseriesPlot(f"{id_try} - PWM commands", "pwm_commands", sequences) timeseries[f"{id_try} - commands"] = plots sequences = {} sequences["linear_velocity"] = linear sequences["angular_velocity"] = angular plots = TimeseriesPlot(f"{id_try} - Velocities", "velocities", sequences) timeseries[f"{id_try} - velocities"] = plots outdir = os.path.join(get_comptests_output_dir(), "together") draw_static(root, outdir, timeseries=timeseries)
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