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
