def test_database_multiprocessing_runner(self):
dbs = DatabaseSerializer(test_scenarios=4,
test_world_steps=5,
num_serialize_scenarios=10)
dbs.process("data/database1")
local_release_filename = dbs.release(version="test")
db = BenchmarkDatabase(database_root=local_release_filename)
evaluators = {
"success": "EvaluatorGoalReached",
"collision": "EvaluatorCollisionEgoAgent",
"max_steps": "EvaluatorStepCount"
}
terminal_when = {
"collision": lambda x: x,
"max_steps": lambda x: x > 5
}
params = ParameterServer(
) # only for evaluated agents not passed to scenario!
behaviors_tested = {
"IDM": BehaviorIDMClassic(params),
"Const": BehaviorConstantVelocity(params)
}
benchmark_runner = BenchmarkRunnerMP(benchmark_database=db,
evaluators=evaluators,
terminal_when=terminal_when,
behaviors=behaviors_tested,
log_eval_avg_every=10)
result = benchmark_runner.run()
params2 = ParameterServer()
viewer = MPViewer(params=params2,
x_range=[5060, 5160],
y_range=[5070, 5150],
use_world_bounds=True)
rst, _ = benchmark_runner.run_benchmark_config(10, viewer=viewer)
rst = benchmark_runner.run(maintain_history=True)
self.assertEqual(len(rst.get_histories()), 40)
rst, scenario_history = benchmark_runner.run_benchmark_config(
11, viewer=None, maintain_history=True)
print(scenario_history)
viewer = MPViewer(params=params2,
x_range=[5060, 5160],
y_range=[5070, 5150],
use_world_bounds=True)
viewer.drawWorld(world=scenario_history[5].GetWorldState(),
eval_agent_ids=scenario_history[5].eval_agent_ids)
viewer.show(block=True)
df = result.get_data_frame()
print(df)
self.assertEqual(
len(df.index),
40) # 2 Behaviors * 10 Serialize Scenarios * 2 scenario sets
def test_obs_traj(self):
map_filename = "external/bark_ml_project/bark_ml/environments/blueprints/highway/city_highway_straight.xodr"
params = ParameterServer()
behavior = BehaviorDiscreteMacroActionsML(params)
evaluator = GoalReached(params)
observer = NearestAgentsObserver(params)
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
env = HyDiscreteHighway(params=params,
map_filename=map_filename,
behavior=behavior,
evaluator=evaluator,
observer=observer,
viewer=viewer,
render=True)
env.reset()
actions = np.random.randint(0, 7, 100)
for action in actions:
concatenated_state, _, _, _ = env.step(action)
nn_ip_state = concatenated_state
ego_nn_input_state = deepcopy(
concatenated_state[0:observer._len_ego_state])
reverted_observed_state = observer.rev_observe_for_ego_vehicle(
nn_ip_state)
ext_reverted_observed_state = np.zeros(
(reverted_observed_state.shape[0] + 1))
ext_reverted_observed_state[1:] = reverted_observed_state
renormed_ego_state = observer._select_state_by_index(
observer._norm(ext_reverted_observed_state))
time.sleep(0.2)
np.testing.assert_array_almost_equal(ego_nn_input_state,
renormed_ego_state)
def test_database_multiprocessing_runner(self):
dbs = DatabaseSerializer(test_scenarios=4, test_world_steps=5, num_serialize_scenarios=5)
dbs.process("data/database1")
local_release_filename = dbs.release(version="test")
db = BenchmarkDatabase(database_root=local_release_filename)
evaluators = {"success" : "EvaluatorGoalReached", "collision" : "EvaluatorCollisionEgoAgent",
"max_steps": "EvaluatorStepCount"}
terminal_when = {"collision" :lambda x: x, "max_steps": lambda x : x>2}
params = ParameterServer() # only for evaluated agents not passed to scenario!
behaviors_tested = {"IDM": BehaviorIDMClassic(params), "Const" : BehaviorConstantAcceleration(params)}
benchmark_runner = BenchmarkRunnerMP(benchmark_database=db,
evaluators=evaluators,
terminal_when=terminal_when,
behaviors=behaviors_tested,
log_eval_avg_every=10)
result = benchmark_runner.run()
df = result.get_data_frame()
print(df)
self.assertEqual(len(df.index), 20) # 2 Behaviors * 5 Serialize Scenarios * 2 scenario sets
params2 = ParameterServer()
viewer = MPViewer(
params=params2,
x_range=[5060, 5160],
y_range=[5070,5150],
use_world_bounds=True)
rst = benchmark_runner.run_benchmark_config(10, viewer=viewer)
def GenerateRuntime():
# parameters
param_server = ParameterServer()
# configure both lanes of the highway. the right lane has one controlled agent
left_lane = CustomLaneCorridorConfig(params=param_server,
lane_corridor_id=0,
road_ids=[0, 1],
behavior_model=BehaviorMobilRuleBased(param_server),
s_min=5.,
s_max=50.)
right_lane = CustomLaneCorridorConfig(params=param_server,
lane_corridor_id=1,
road_ids=[0, 1],
controlled_ids=True,
behavior_model=BehaviorMobilRuleBased(param_server),
s_min=5.,
s_max=20.)
scenarios = \
ConfigWithEase(num_scenarios=3,
map_file_name=Data.xodr_data("DR_DEU_Merging_MT_v01_shifted"),
random_seed=0,
params=param_server,
lane_corridor_configs=[left_lane, right_lane])
# viewer
viewer = MPViewer(params=param_server,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
env = Runtime(step_time=0.2,
viewer=viewer,
scenario_generator=scenarios,
render=True,
maintain_world_history=True)
return env
def test_python_model(self):
param_server = ParameterServer(filename=os.path.join(
os.path.dirname(__file__),
"../../runtime/tests/data/deterministic_scenario.json"))
param_server
mapfile = os.path.join(
os.path.dirname(__file__),
"../../runtime/tests/data/city_highway_straight.xodr")
param_server["Scenario"]["Generation"][
"DeterministicScenarioGeneration"]["MapFilename"] = mapfile
scenario_generation = DeterministicScenarioGeneration(
num_scenarios=3, random_seed=0, params=param_server)
viewer = MPViewer(params=param_server,
follow_agent_id=False,
use_world_bounds=True)
scenario, idx = scenario_generation.get_next_scenario()
world = scenario.GetWorldState()
single_track_model = SingleTrackModel(param_server)
behavior_model = PythonBehaviorModelWrapper(single_track_model,
param_server)
world.GetAgent(0).behavior_model = behavior_model
world.GetAgent(0).behavior_model.SetLastAction(
np.array([1., 1.], dtype=np.float32))
world.Step(0.2)
def main():
args = configure_args()
if is_local:
dir_prefix = ""
else:
dir_prefix="hy-iqn-lfd-full-exp.runfiles/hythe/"
logging.info(f"Executing job: {args.jobname}")
logging.info(f"Experiment server at: {os.getcwd()}")
params = ParameterServer(filename=os.path.join(dir_prefix, params_file),
log_if_default=True)
params = configure_params(params, seed=args.jobname)
experiment_id = params["Experiment"]["random_seed"]
params_filename = os.path.join(params["Experiment"]["dir"], "params_{}.json".format(experiment_id))
behavior = BehaviorDiscreteMacroActionsML(params)
evaluator = GoalReached(params)
observer = NearestAgentsObserver(params)
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
# extract params and save experiment parameters
params["ML"]["BaseAgent"]["SummaryPath"] = os.path.join(params["Experiment"]["dir"], "agent/summaries")
params["ML"]["BaseAgent"]["CheckpointPath"] = os.path.join(params["Experiment"]["dir"], "agent/checkpoints")
params.Save(filename=params_filename)
logging.info('-' * 60)
logging.info("Writing params to :{}".format(params_filename))
logging.info('-' * 60)
# database creation
dbs1 = DatabaseSerializer(test_scenarios=1, test_world_steps=2,
num_serialize_scenarios=num_demo_scenarios)
dbs1.process(os.path.join(dir_prefix, "configuration/database"),
filter_sets="**/**/interaction_merging_light_dense_1D.json")
local_release_filename = dbs1.release(version="lfd_offline")
db1 = BenchmarkDatabase(database_root=local_release_filename)
scenario_generator1, _, _ = db1.get_scenario_generator(0)
env = HyDiscreteHighway(params=params,
scenario_generation=scenario_generator1,
behavior=behavior,
evaluator=evaluator,
observer=observer,
viewer=viewer,
render=False)
scenario, _ = scenario_generator1.get_next_scenario()
world = scenario.GetWorldState()
observer.Reset(world)
assert env.action_space._n == 8, "Action Space is incorrect!"
run(args, params, env, db=db1)
params.Save(params_filename)
logging.info('-' * 60)
logging.info("Writing params to :{}".format(params_filename))
logging.info('-' * 60)
def _test_scenario(self, scenario_idx, num_steps, visualize, viewer):
logging.info("Running scenario {} of {} in set {}".format(scenario_idx,
self._scenario_generator.num_scenarios,
self._set_name))
try:
scenario = self._scenario_generator.get_scenario(scenario_idx)
except Exception as e:
logging.error("Deserialization failed with {}.".format(e))
return False
try:
world_state = scenario.GetWorldState()
except Exception as e:
logging.error("Get world state failed with {}.".format(e))
return False
if visualize:
if not viewer:
viewer = MPViewer(
params=ParameterServer(),
x_range=[5060, 5160],
y_range=[5070, 5150],
use_world_bounds=True)
sim_step_time = 0.2
sim_real_time_factor = 1
try:
for _ in range(0, num_steps): # run a few steps for each scenario
if visualize:
info_text = "SetName: {} | ScenarioIdx: {}".format(
self._set_name, scenario_idx)
viewer.drawText(position=(0.5, 1.05), text=info_text)
viewer.drawWorld(
world_state, scenario._eval_agent_ids, scenario_idx=scenario_idx)
viewer.show(block=False)
time.sleep(sim_step_time/sim_real_time_factor)
viewer.clear()
world_state.Step(sim_step_time)
return True
except Exception as e:
logging.error("Simulation failed with {}.".format(e))
return False
def main():
map_filename = "external/bark_ml_project/bark_ml/environments/blueprints/highway/city_highway_straight.xodr"
params_filename = glob.glob(os.path.join(demo_root, "params_[!behavior]*"))
params = ParameterServer(filename=params_filename[0], log_if_default=True)
behavior = BehaviorDiscreteMacroActionsML(params)
evaluator = GoalReached(params)
if is_belief_observer:
splits = 2
bparams_filename = glob.glob(os.path.join(demo_root, "behavior_*"))
params_behavior = ParameterServer(filename=bparams_filename[0])
behavior_space = BehaviorSpace(params_behavior)
hypothesis_set, hypothesis_params = behavior_space.create_hypothesis_set_fixed_split(
split=splits)
observer = BeliefObserver(params, hypothesis_set, splits=splits)
else:
observer = NearestAgentsObserver(params)
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
env = HyDiscreteHighway(params=params,
map_filename=map_filename,
behavior=behavior,
evaluator=evaluator,
observer=observer,
viewer=viewer,
render=False)
env.reset()
ego_world_states = []
_, demonstrations = unpack_load_demonstrations(demo_root)
for demo in demonstrations:
ego_state = np.zeros((observer._len_ego_state + 1))
(nn_ip_state, action, reward, next_state, done, is_demo) = demo
ego_nn_input_state = deepcopy(nn_ip_state[0:observer._len_ego_state])
ego_state[1:] = ego_nn_input_state
reverted_observed_state = observer.rev_observe_for_ego_vehicle(
ego_state)
ego_world_states.append(
(reverted_observed_state[int(StateDefinition.X_POSITION)],
reverted_observed_state[int(StateDefinition.Y_POSITION)],
reverted_observed_state[int(StateDefinition.THETA_POSITION)],
reverted_observed_state[int(StateDefinition.VEL_POSITION)],
action, int(is_demo)))
df = pd.DataFrame(ego_world_states,
columns=[
'pos_x', 'pos_y', 'orientation', 'velocity',
'action', 'is_demo'
])
print(df.head(10))
df.to_pickle(os.path.join(demo_root, "demonstrations/demo_dataframe"))
return
def __init__(self,
params=None,
number_of_senarios=250,
random_seed=0,
ml_behavior=None,
viewer=True):
params["BehaviorIDMClassic"]["BrakeForLaneEnd"] = True
params["BehaviorIDMClassic"]["BrakeForLaneEndEnabledDistance"] = 100.
params["BehaviorIDMClassic"]["BrakeForLaneEndDistanceOffset"] = 25.
params["BehaviorIDMClassic"]["DesiredVelocity"] = 12.5
params["World"]["remove_agents_out_of_map"] = False
left_lane = MergingLaneCorridorConfig(params=params,
road_ids=[0, 1],
min_vel=10.,
max_vel=15.,
s_min=5.,
s_max=25.,
lane_corridor_id=0,
controlled_ids=None)
right_lane = MergingLaneCorridorConfig(
params=params,
road_ids=[0, 1],
lane_corridor_id=1,
s_min=5.,
s_max=25.,
min_vel=8.,
max_vel=12.,
behavior_model=BehaviorIDMClassic(params),
controlled_ids=True)
scenario_generation = \
ConfigWithEase(
num_scenarios=number_of_senarios,
map_file_name=os.path.join(os.path.dirname(__file__), "../../../environments/blueprints/merging/DR_DEU_Merging_MT_v01_shifted.xodr"), # NOLINT
random_seed=random_seed,
params=params,
lane_corridor_configs=[left_lane, right_lane])
if viewer:
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
dt = 0.2
evaluator = GoalReachedGuiding(params)
# evaluator = GoalReached(params)
observer = NearestObserver(params)
ml_behavior = ml_behavior
super().__init__(scenario_generation=scenario_generation,
viewer=viewer,
dt=dt,
evaluator=evaluator,
observer=observer,
ml_behavior=ml_behavior)
def test_two_roads_one_lane(self):
params = ParameterServer()
world = World(params)
xodr_map = MakeXodrMapOneRoadTwoLanes()
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_map)
world.SetMap(map_interface)
start_point = Point2d(0, -11)
lanes_near_start = map_interface.find_nearest_lanes(start_point, 1)
assert(len(lanes_near_start) == 1)
goal_point = Point2d(-191.789, -50.1725)
lanes_near_goal = map_interface.find_nearest_lanes(goal_point, 1)
assert(len(lanes_near_goal) == 1)
viewer = MPViewer(params=params, use_world_bounds=True)
viewer.drawWorld(world)
# if this is not here, the second unit test is not executed (maybe parsing takes too long?)
time.sleep(2)
def test_dr_deu_merging(self):
# threeway_intersection
xodr_parser = XodrParser(
os.path.join(
os.path.dirname(__file__),
"../../../runtime/tests/data/DR_DEU_Merging_MT_v01_shifted.xodr"
))
# World Definition
params = ParameterServer()
world = World(params)
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_parser.map)
world.SetMap(map_interface)
roads = [0, 1]
driving_direction = XodrDrivingDirection.forward
map_interface.GenerateRoadCorridor(roads, driving_direction)
road_corridor = map_interface.GetRoadCorridor(roads, driving_direction)
# Draw map
viewer = MPViewer(params=params, use_world_bounds=True)
viewer.drawWorld(world)
viewer.drawPolygon2d(road_corridor.lane_corridors[0].polygon,
color="blue",
alpha=0.5)
viewer.drawPolygon2d(road_corridor.lane_corridors[1].polygon,
color="blue",
alpha=0.5)
viewer.show(block=False)
self.assertTrue(road_corridor.lane_corridors[0].polygon.Valid())
self.assertTrue(road_corridor.lane_corridors[1].polygon.Valid())
self.assertTrue(road_corridor.polygon.Valid())
def main():
args = configure_args()
exp_dir = args.exp_dir or "results/training/toy_evaluation"
params_filename = glob.glob(os.path.join(exp_dir, "params_*"))
params = ParameterServer(filename=params_filename[0])
behavior_params_filename = glob.glob(
os.path.join(exp_dir, "behavior_params*"))
if behavior_params_filename:
splits = 8
behavior_params = ParameterServer(filename=behavior_params_filename[0])
behavior_space = BehaviorSpace(behavior_params)
hypothesis_set, _ = behavior_space.create_hypothesis_set_fixed_split(
split=splits)
observer = BeliefObserver(params, hypothesis_set, splits=splits)
behavior = BehaviorDiscreteMacroActionsML(behavior_params)
else:
behavior = BehaviorDiscreteMacroActionsML(params)
observer = NearestAgentsObserver(params)
evaluator = GoalReached(params)
scenario_params = ParameterServer(
filename=
"configuration/database/scenario_sets/interaction_merging_light_dense_1D.json"
)
scenario_generator = ConfigurableScenarioGeneration(params=scenario_params,
num_scenarios=5)
scenario_file = glob.glob(os.path.join(exp_dir, "scenarios_list*"))
scenario_generator.load_scenario_list(scenario_file[0])
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
env = HyDiscreteHighway(behavior=behavior,
observer=observer,
evaluator=evaluator,
viewer=viewer,
scenario_generation=scenario_generator,
render=True)
env.reset()
actions = [0, 1, 2, 3, 4, 5, 6]
for action in actions:
print(action)
env.step(action)
agent = FQFAgent(env=env, test_env=env, params=params)
agent.load_models(os.path.join(exp_dir, "agent/checkpoints/final"))
def __init__(self,
params=None,
number_of_senarios=250,
random_seed=0,
ml_behavior=None,
viewer=True):
params["BehaviorIDMClassic"]["DesiredVelocity"] = 30.
left_lane = HighwayLaneCorridorConfig(params=params,
road_ids=[16],
lane_corridor_id=0,
min_vel=25.0,
max_vel=30.0,
controlled_ids=None)
right_lane = HighwayLaneCorridorConfig(params=params,
road_ids=[16],
lane_corridor_id=1,
min_vel=25.0,
max_vel=30.0,
controlled_ids=True)
scenario_generation = \
ConfigWithEase(
num_scenarios=number_of_senarios,
map_file_name=os.path.join(os.path.dirname(__file__), "../../../environments/blueprints/highway/city_highway_straight.xodr"), # NOLINT
random_seed=random_seed,
params=params,
lane_corridor_configs=[left_lane, right_lane])
if viewer:
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
dt = 0.2
evaluator = GoalReachedGuiding(params)
observer = NearestObserver(params)
ml_behavior = ml_behavior
super().__init__(
scenario_generation=scenario_generation,
viewer=viewer,
dt=dt,
evaluator=evaluator,
observer=observer,
ml_behavior=ml_behavior)
def run_configuration(argv):
params = ParameterServer(
filename="examples/example_params/tfa_params.json")
# params = ParameterServer()
# NOTE: Modify these paths in order to save the checkpoints and summaries
# params["ML"]["BehaviorTFAAgents"]["CheckpointPath"] = "/Users/hart/Development/bark-ml/checkpoints_merging_nn/"
# params["ML"]["TFARunner"]["SummaryPath"] = "/Users/hart/Development/bark-ml/checkpoints_merging_nn/"
params["Visualization"]["Agents"]["Alpha"]["Other"] = 0.2
params["Visualization"]["Agents"]["Alpha"]["Controlled"] = 0.2
params["Visualization"]["Agents"]["Alpha"]["Controlled"] = 0.2
params["ML"]["VisualizeCfWorlds"] = False
params["ML"]["VisualizeCfHeatmap"] = True
params["World"]["remove_agents_out_of_map"] = False
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
# create environment
bp = ContinuousMergingBlueprint(params, num_scenarios=10000, random_seed=0)
env = SingleAgentRuntime(blueprint=bp, render=False, viewer=viewer)
# PPO-agent
# ppo_agent = BehaviorPPOAgent(environment=env,
# params=params)
# env.ml_behavior = ppo_agent
# runner = PPORunner(params=params,
# environment=env,
# agent=ppo_agent)
# SAC-agent
sac_agent = BehaviorSACAgent(environment=env, params=params)
env.ml_behavior = sac_agent
runner = SACRunner(params=params, environment=env, agent=sac_agent)
if FLAGS.mode == "train":
runner.SetupSummaryWriter()
runner.Train()
elif FLAGS.mode == "visualize":
runner.Run(num_episodes=50, render=True)
elif FLAGS.mode == "evaluate":
runner.Run(num_episodes=100, render=False)
def GetParamServerAndWorld():
param_server = ParameterServer(filename=os.path.join(
os.path.dirname(__file__),
"../../runtime/tests/data/deterministic_scenario.json"))
mapfile = os.path.join(
os.path.dirname(__file__),
"../../runtime/tests/data/city_highway_straight.xodr")
param_server["Scenario"]["Generation"]["DeterministicScenarioGeneration"][
"MapFilename"] = mapfile
scenario_generation = DeterministicScenarioGeneration(num_scenarios=3,
random_seed=0,
params=param_server)
viewer = MPViewer(params=param_server,
follow_agent_id=False,
use_world_bounds=True)
scenario, idx = scenario_generation.get_next_scenario()
world = scenario.GetWorldState()
return world, param_server
def _get_viewer(self, param_server: None, renderer: str):
"""Getter for a viewer to display the simulation.
Args:
param_server (ParameterServer): The parameters that specify the scenario.
renderer (str): The renderer type used. [pygame, matplotlib]
Returns:
bark.runtime.viewer.Viewer: A viewer depending on the renderer type
"""
fig = plt.figure(figsize=[10, 10])
if renderer == "pygame":
self._viewer = PygameViewer(params=param_server,
use_world_bounds=True,
axis=fig.gca())
else:
self._viewer = MPViewer(params=param_server,
use_world_bounds=True,
axis=fig.gca())
def test_database_multiprocessing_history(self):
dbs = DatabaseSerializer(test_scenarios=4,
test_world_steps=5,
num_serialize_scenarios=2)
dbs.process("data/database1")
local_release_filename = dbs.release(version="test")
db = BenchmarkDatabase(database_root=local_release_filename)
evaluators = {
"success": "EvaluatorGoalReached",
"collision": "EvaluatorCollisionEgoAgent",
"max_steps": "EvaluatorStepCount"
}
terminal_when = {
"collision": lambda x: x,
"max_steps": lambda x: x > 2
}
params = ParameterServer(
) # only for evaluated agents not passed to scenario!
behaviors_tested = {
"IDM": BehaviorIDMClassic(params),
"Const": BehaviorConstantAcceleration(params)
}
benchmark_runner = BenchmarkRunnerMP(benchmark_database=db,
evaluators=evaluators,
terminal_when=terminal_when,
behaviors=behaviors_tested,
log_eval_avg_every=10)
benchmark_runner.clear_checkpoint_dir()
rst = benchmark_runner.run(maintain_history=True)
rst.load_histories()
self.assertEqual(len(rst.get_histories()), 2 * 2 * 2)
rst = benchmark_runner.run_benchmark_config(3,
viewer=None,
maintain_history=True)
scenario_history = rst.get_histories()[3]
print(scenario_history)
params = ParameterServer()
viewer = MPViewer(params=params,
x_range=[5060, 5160],
y_range=[5070, 5150],
use_world_bounds=True)
viewer.drawWorld(world=scenario_history[1].GetWorldState(),
eval_agent_ids=scenario_history[1].eval_agent_ids)
viewer.show(block=True)
def SimulateWorld(self, world, local_tracer, N=5, **kwargs):
"""Simulates the world for N steps."""
self.ml_behavior.set_action_externally = False
eval_id = self._scenario._eval_agent_ids[0]
self._world.agents[eval_id].behavior_model = self.ml_behavior
replaced_agent_id = kwargs.get("replaced_agent", 0)
if replaced_agent_id not in self._cf_axs and self._visualize_cf_worlds:
self._cf_axs[replaced_agent_id] = {"ax": plt.subplots(3, 1, constrained_layout=True)[1], "count": 0}
for i in range(0, N):
if i == N - 1 and kwargs.get("num_virtual_world", 0) is not None and \
self._visualize_cf_worlds and replaced_agent_id is not None:
# NOTE: outsource
for ftype in [".png", ".pgf"]:
viewer = MPViewer(
params=self._params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True,
axis=self._cf_axs[replaced_agent_id]["ax"][self._cf_axs[replaced_agent_id]["count"]])
# se
for agent_id in world.agents.keys():
viewer.agent_color_map[agent_id] = "gray"
viewer.agent_color_map[replaced_agent_id] = (127/255, 205/255, 187/255)
viewer.agent_color_map[eval_id] = (34/255, 94/255, 168/255)
if replaced_agent_id == 1:
viewer.drawWorld(
world,
eval_agent_ids=self._scenario._eval_agent_ids,
filename=self._results_folder + "cf_%03d_replaced_" % self._count + str(replaced_agent_id)+ftype,
debug_text=False)
self._cf_axs[replaced_agent_id]["count"] += 1
observed_world = world.Observe([eval_id])[0]
eval_state = observed_world.Evaluate()
agent_states = CaptureAgentStates(observed_world)
eval_state = {**eval_state, **agent_states}
# TODO: break at collision
local_tracer.Trace(eval_state, **kwargs)
if eval_state["collision"] or eval_state["drivable_area"]:
break
world.Step(self._step_time)
self.ml_behavior.set_action_externally = True
def test_env(self):
map_filename = "external/bark_ml_project/bark_ml/environments/blueprints/highway/city_highway_straight.xodr"
params = ParameterServer()
behavior = BehaviorDiscreteMacroActionsML(params)
evaluator = GoalReached(params)
observer = NearestAgentsObserver(params)
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
env = HyDiscreteHighway(params=params,
map_filename=map_filename,
behavior=behavior,
evaluator=evaluator,
observer=observer,
viewer=viewer,
render=True)
env.reset()
actions = [5] * 100
print(actions)
for action in actions:
env.step(action)
time.sleep(0.2)
return
def __init__(self,
params=None,
ml_behavior=None,
num_scenarios=100,
viewer=True):
# NOTE: scneario number is wrong
scenario_generation = ConfigurableScenarioGeneration(
num_scenarios=num_scenarios, params=params)
if viewer:
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
dt = 0.2
# NOTE: evaluator and observer could be overwritten
evaluator = GoalReached(params)
observer = NearestAgentsObserver(params)
ml_behavior = ml_behavior
super().__init__(scenario_generation=scenario_generation,
viewer=viewer,
dt=dt,
evaluator=evaluator,
observer=observer,
ml_behavior=ml_behavior)
def test_curved_road(self):
params = ParameterServer()
world = World(params)
xodr_map = MakeXodrMapCurved(50, 0.1)
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_map)
world.SetMap(map_interface)
roads = [100]
driving_direction = XodrDrivingDirection.forward
map_interface.GenerateRoadCorridor(roads, driving_direction)
road_corr = map_interface.GetRoadCorridor(roads, driving_direction)
viewer = MPViewer(params=params, use_world_bounds=True)
viewer.drawWorld(world)
viewer.drawRoadCorridor(road_corr)
#viewer.show(block=True)
# if this is not here, the second unit test is not executed (maybe parsing takes too long?)
time.sleep(1)
controlled_ids=True,
behavior_model=BehaviorMobilRuleBased(param_server),
s_min=5.,
s_max=20.)
scenarios = \
ConfigWithEase(num_scenarios=3,
map_file_name=Data.xodr_data("DR_DEU_Merging_MT_v01_centered"),
random_seed=0,
params=param_server,
lane_corridor_configs=[left_lane, right_lane])
# viewer
viewer = MPViewer(
params=param_server,
# x_range=[-35, 35],
# y_range=[-35, 35],
follow_agent_id=False)
sim_step_time = param_server["simulation"]["step_time",
"Step-time used in simulation",
0.05]
sim_real_time_factor = param_server["simulation"][
"real_time_factor", "execution in real-time or faster", 1.]
viewer = VideoRenderer(renderer=viewer,
world_step_time=sim_step_time,
fig_path="/tmp/video")
env = Runtime(step_time=0.2,
viewer=viewer,
def main():
args = configure_args()
if is_local:
dir_prefix = ""
else:
dir_prefix = "hy-x-iqn-beliefs.runfiles/hythe/"
print("Experiment server at :", os.getcwd())
params = ParameterServer(filename=os.path.join(
dir_prefix, "configuration/params/iqn_params.json"))
params = configure_params(params, seed=args.jobname)
experiment_id = params["Experiment"]["random_seed"]
params_filename = os.path.join(params["Experiment"]["dir"],
"params_{}.json".format(experiment_id))
params_behavior_filename = os.path.join(
params["Experiment"]["dir"],
"behavior_params_{}.json".format(experiment_id))
# check if exp exists and handle preemption
exp_exists = check_if_exp_exists(params)
if exp_exists:
print("Loading existing experiment from: {}".format(
args.jobname, (params["Experiment"]["dir"])))
if os.path.isfile(params_filename):
params = ParameterServer(filename=params_filename,
log_if_default=True)
if os.path.isfile(params_behavior_filename):
params_behavior = ParameterServer(
filename=params_behavior_filename, log_if_default=True)
else:
Path(params["Experiment"]["dir"]).mkdir(parents=True, exist_ok=True)
params["ML"]["BaseAgent"]["SummaryPath"] = os.path.join(
params["Experiment"]["dir"], "agent/summaries")
params["ML"]["BaseAgent"]["CheckpointPath"] = os.path.join(
params["Experiment"]["dir"], "agent/checkpoints")
params_behavior = ParameterServer(filename=os.path.join(
dir_prefix, "configuration/params/1D_desired_gap_no_prior.json"),
log_if_default=True)
params.Save(filename=params_filename)
params_behavior.Save(filename=params_behavior_filename)
# configure belief observer
splits = 2
behavior_space = configure_behavior_space(params_behavior)
hypothesis_set, hypothesis_params = behavior_space.create_hypothesis_set_fixed_split(
split=splits)
observer = BeliefObserver(params, hypothesis_set, splits=splits)
behavior = BehaviorDiscreteMacroActionsML(params)
evaluator = GoalReached(params)
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
# database creation
dbs = DatabaseSerializer(test_scenarios=2,
test_world_steps=2,
num_serialize_scenarios=num_scenarios)
dbs.process(os.path.join(dir_prefix, "configuration/database"),
filter_sets="**/**/interaction_merging_mid_dense_1D_new.json")
local_release_filename = dbs.release(version="test")
db = BenchmarkDatabase(database_root=local_release_filename)
scenario_generator, _, _ = db.get_scenario_generator(scenario_set_id=0)
env = HyDiscreteHighway(params=params,
scenario_generation=scenario_generator,
behavior=behavior,
evaluator=evaluator,
observer=observer,
viewer=viewer,
render=is_local)
print('Observation/state space size', env.observation_space)
run(params, env, exp_exists)
params.Save(filename=params_filename)
logging.info('-' * 60)
logging.info("Writing params to :{}".format(params_filename))
logging.info('-' * 60)
params_behavior.Save(filename=params_behavior_filename)
logging.info('-' * 60)
logging.info(
"Writing behavior params to :{}".format(params_behavior_filename))
logging.info('-' * 60)
return
controlled_ids=True)
# create 5 scenarios
scenarios = \
ConfigWithEase(
num_scenarios=5,
map_file_name=os.path.join(
os.path.dirname(__file__),
"../bark/runtime/tests/data/city_highway_straight.xodr"),
random_seed=0,
params=param_server,
lane_corridor_configs=[left_lane, right_lane])
# viewer
viewer = MPViewer(params=param_server,
x_range=[-75, 75],
y_range=[-75, 75],
follow_agent_id=True)
sim_step_time = param_server["simulation"]["step_time",
"Step-time used in simulation",
0.05]
sim_real_time_factor = param_server["simulation"][
"real_time_factor",
"execution in real-time or faster",
0.5]
# viewer = VideoRenderer(renderer=viewer,
# world_step_time=sim_step_time,
# fig_path="/home/hart/Dokumente/2020/bark/video")
# gym like interface
from bark.core.world import World
from bark.core.geometry import Line2d
from bark.runtime.commons.parameters import ParameterServer
from bark.runtime.viewer.matplotlib_viewer import MPViewer
from bark.core.world.map import MapInterface
from bark.core.models.behavior import BehaviorIDMClassic
from bark_ml.environments.external_runtime import ExternalRuntime
from bark_ml.library_wrappers.lib_tf_agents.agents.sac_agent import BehaviorSACAgent
from bark_ml.observers.nearest_state_observer import NearestAgentsObserver
from bark_ml.environments.blueprints import ContinuousHighwayBlueprint
from bark_ml.environments.single_agent_runtime import SingleAgentRuntime
from bark_ml.behaviors.cont_behavior import BehaviorContinuousML # pylint: disable=unused-import
from bark.runtime.scenario.scenario import Scenario
viewer = MPViewer(params=ParameterServer(),
x_range=[-50, 50],
y_range=[-50, 50],
follow_agent_id=True)
class PyEnvironmentTests(unittest.TestCase):
def setUp(self):
params = ParameterServer()
self.params = params
csvfile = os.path.join(
os.path.dirname(__file__),
"../environments/blueprints/single_lane/base_map_lanes_guerickestr_short_assymetric_48.csv"
)
print(csvfile)
self.map_interface = MapInterface()
self.map_interface.SetCsvMap(csvfile, 692000, 5.339e+06)
def main():
args = configure_args()
if is_local:
dir_prefix = ""
else:
dir_prefix = "hy-iqnfd-beliefs-exp.runfiles/hythe/"
print("Executing job :", args.jobname)
print("Experiment server at :", os.getcwd())
params = ParameterServer(filename=os.path.join(
dir_prefix, "configuration/params/iqn_params_demo_full_local.json"),
log_if_default=True)
params = configure_params(params, seed=args.jobname)
experiment_id = params["Experiment"]["random_seed"]
params_filename = os.path.join(params["Experiment"]["dir"],
"params_{}.json".format(experiment_id))
behavior = BehaviorDiscreteMacroActionsML(params)
evaluator = GoalReached(params)
params_behavior_filename = os.path.join(
params["Experiment"]["dir"],
"behavior_params_{}.json".format(experiment_id))
params_behavior = ParameterServer(filename=os.path.join(
dir_prefix, "configuration/params/1D_desired_gap_no_prior.json"),
log_if_default=True)
params_behavior.Save(filename=params_behavior_filename)
# configure belief observer
splits = 2
behavior_space = BehaviorSpace(params_behavior)
hypothesis_set, hypothesis_params = behavior_space.create_hypothesis_set_fixed_split(
split=splits)
observer = BeliefObserver(params, hypothesis_set, splits=splits)
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
# extract params and save experiment parameters
params["ML"]["BaseAgent"]["SummaryPath"] = os.path.join(
params["Experiment"]["dir"], "agent/summaries")
params["ML"]["BaseAgent"]["CheckpointPath"] = os.path.join(
params["Experiment"]["dir"], "agent/checkpoints")
params.Save(filename=params_filename)
logging.info('-' * 60)
logging.info("Writing params to :{}".format(params_filename))
logging.info('-' * 60)
# database creation
dbs = DatabaseSerializer(test_scenarios=1,
test_world_steps=2,
num_serialize_scenarios=num_scenarios)
dbs.process(os.path.join(dir_prefix, "configuration/database"),
filter_sets="**/**/interaction_merging_light_dense_1D.json")
local_release_filename = dbs.release(version="test")
db = BenchmarkDatabase(database_root=local_release_filename)
scenario_generator, _, _ = db.get_scenario_generator(0)
env = HyDiscreteHighway(params=params,
scenario_generation=scenario_generator,
behavior=behavior,
evaluator=evaluator,
observer=observer,
viewer=viewer,
render=False)
scenario, _ = scenario_generator.get_next_scenario()
world = scenario.GetWorldState()
observer.Reset(world)
assert env.action_space._n == 8, "Action Space is incorrect!"
run(args, params, env, db=db)
params.Save(params_filename)
logging.info('-' * 60)
logging.info("Writing params to :{}".format(params_filename))
logging.info('-' * 60)
params_behavior.Save(filename=params_behavior_filename)
logging.info('-' * 60)
logging.info(
"Writing behavior params to :{}".format(params_behavior_filename))
logging.info('-' * 60)
from bark.runtime.scenario.scenario_generation.interaction_dataset_scenario_generation import \
InteractionDatasetScenarioGeneration
from bark.runtime.commons.parameters import ParameterServer
from bark.runtime.viewer.matplotlib_viewer import MPViewer
from bark.runtime.viewer.video_renderer import VideoRenderer
import os
import argparse
# set you json config that contains a map and matching tracks.
param_server = ParameterServer(filename=os.path.join(os.path.join(os.path.dirname(__file__),"params/interaction_example.json")))
scenario_generation = InteractionDatasetScenarioGeneration(num_scenarios=1,
random_seed=0,
params=param_server)
viewer = MPViewer(params=param_server, use_world_bounds=True)
sim_step_time = param_server["simulation"]["step_time",
"Step-time used in simulation",
0.2]
sim_real_time_factor = param_server["simulation"]["real_time_factor",
"execution in real-time or faster", 1]
scenario = scenario_generation.create_scenarios(param_server, 1)[0]
world_state = scenario.GetWorldState()
sim_time_steps = param_server["simulation"]["simulation_time_steps", "Number of time steps to simulate", 50]
video_renderer = VideoRenderer(renderer=viewer, world_step_time=sim_step_time)
for _ in range(0, sim_time_steps):
world_state.PlanAgents(sim_step_time)
def main():
map_filename = "external/bark_ml_project/bark_ml/environments/blueprints/highway/city_highway_straight.xodr"
params = ParameterServer(filename=os.path.join(
exp_root, "params_iqn_pre_exp_2sdi64.json"),
log_if_default=True)
params_behavior = ParameterServer(filename=os.path.join(
exp_root, "behavior_params_iqn_pre_exp_2sdi64.json"))
behavior = BehaviorDiscreteMacroActionsML(params)
evaluator = GoalReached(params)
if is_belief_observer:
splits = 2
behavior_space = BehaviorSpace(params_behavior)
hypothesis_set, hypothesis_params = behavior_space.create_hypothesis_set_fixed_split(
split=splits)
observer = BeliefObserver(params, hypothesis_set, splits=splits)
else:
observer = NearestAgentsObserver(params)
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
# database creation
dbs1 = DatabaseSerializer(test_scenarios=1,
test_world_steps=2,
num_serialize_scenarios=1)
dbs1.process(os.path.join("", "configuration/database"),
filter_sets="**/**/interaction_merging_light_dense_1D.json")
local_release_filename = dbs1.release(version="lfd_offline")
db1 = BenchmarkDatabase(database_root=local_release_filename)
scenario_generator1, _, _ = db1.get_scenario_generator(0)
env = HyDiscreteHighway(params=params,
scenario_generation=scenario_generator1,
map_filename=map_filename,
behavior=behavior,
evaluator=evaluator,
observer=observer,
viewer=viewer,
render=False)
scenario, _ = scenario_generator1.get_next_scenario()
world = scenario.GetWorldState()
observer.Reset(world)
env.reset()
agent = pick_agent(exp_root, env, params)
ego_world_states = []
memory = agent.memory
learned_states = memory["state"]
actions = memory['action']
is_demos = memory['is_demo']
for state, action, is_demo in zip(learned_states, actions, is_demos):
ego_state = np.zeros((observer._len_ego_state + 1))
ego_nn_input_state = deepcopy(state[0:observer._len_ego_state])
ego_state[1:] = ego_nn_input_state
reverted_observed_state = observer.rev_observe_for_ego_vehicle(
ego_state)
ego_world_states.append(
(reverted_observed_state[int(StateDefinition.X_POSITION)],
reverted_observed_state[int(StateDefinition.Y_POSITION)],
reverted_observed_state[int(StateDefinition.THETA_POSITION)],
reverted_observed_state[int(StateDefinition.VEL_POSITION)],
action[0], is_demo[0]))
df = pd.DataFrame(ego_world_states,
columns=[
'pos_x', 'pos_y', 'orientation', 'velocity',
'action', 'is_demo'
])
print(df.head(10))
if not os.path.exists(os.path.join(exp_root, "demonstrations")):
os.makedirs(os.path.join(exp_root, "demonstrations"))
df.to_pickle(os.path.join(exp_root, "demonstrations", "learned_dataframe"))
return
def main():
print("Experiment server at :", os.getcwd())
args = configure_args()
#load exp params
exp_dir = args.output_dir
params_filename = glob.glob(os.path.join(exp_dir, "params_[!behavior]*"))
params = ParameterServer(filename=params_filename[0])
params.load(fn=params_filename[0])
params["ML"]["BaseAgent"]["SummaryPath"] = os.path.join(exp_dir, "agent/summaries")
params["ML"]["BaseAgent"]["CheckpointPath"] = os.path.join(exp_dir, "agent/checkpoints")
splits = 8
behavior_params_filename = glob.glob(os.path.join(exp_dir, "behavior_params*"))
if behavior_params_filename:
params_behavior = ParameterServer(filename=behavior_params_filename[0])
else:
params_behavior = ParameterServer(filename="configuration/params/1D_desired_gap_no_prior.json")
behavior_space = configure_behavior_space(params_behavior)
hypothesis_set, hypothesis_params = behavior_space.create_hypothesis_set_fixed_split(split=splits)
observer = BeliefObserver(params, hypothesis_set, splits=splits)
behavior = BehaviorDiscreteMacroActionsML(params_behavior)
evaluator = GoalReached(params)
viewer = MPViewer(params=params,
x_range=[-35, 35],
y_range=[-35, 35],
follow_agent_id=True)
# database creation
dir_prefix = ""
dbs = DatabaseSerializer(test_scenarios=2, test_world_steps=2,
num_serialize_scenarios=10)
dbs.process(os.path.join(dir_prefix, "configuration/database"), filter_sets="**/**/interaction_merging_light_dense_1D.json")
local_release_filename = dbs.release(version="test")
db = BenchmarkDatabase(database_root=local_release_filename)
scenario_generator, _, _ = db.get_scenario_generator(0)
video_renderer = VideoRenderer(renderer=viewer, world_step_time=0.2)
env = HyDiscreteHighway(params=params,
scenario_generation=scenario_generator,
behavior=behavior,
evaluator=evaluator,
observer=observer,
viewer=video_renderer,
render=is_local)
# non-agent evaluation mode
num_steps = 100
num_samples = params_behavior["BehaviorSpace"]["Hypothesis"]["BehaviorHypothesisIDM"]["NumSamples"]
print("Steps, samples, splits", num_steps, num_samples, splits)
step = 1
env.reset()
threshold = observer.is_enabled_threshold
discretize = observer.is_discretize
beliefs_df = pd.DataFrame(columns=["Step", "Action", "Agent", "Beliefs", "HyNum"])
beliefs_orig_df = pd.DataFrame(columns=["Step", "Action", "Agent", "Beliefs", "HyNum"])
while step <= num_steps:
action = 5 #np.random.randint(0, behavior.action_space.n)
next_state, reward, done, info = env.step(action)
for agent, beliefs in observer.beliefs.items():
beliefs = np.asarray(beliefs)
oring = deepcopy(beliefs)
for i, belief in enumerate(oring):
beliefs_orig_df = beliefs_orig_df.append({"Step": step, "Action": action, "Agent": agent, "Beliefs": belief, "HyNum": i}, ignore_index=True)
if discretize:
beliefs = observer.discretize_beliefs(beliefs)
if threshold:
beliefs = observer.threshold_beliefs(beliefs)
for i, belief in enumerate(beliefs):
beliefs_df = beliefs_df.append({"Step": step, "Action": action, "Agent": agent, "Beliefs": belief, "HyNum": i}, ignore_index=True)
step += 1
suffix = "switch"
if threshold:
suffix += "_threshold"
if discretize:
suffix += "_discretize"
beliefs_data_filename = "beliefs_{}_{}_{}".format(splits, num_samples, num_steps)
beliefs_data_filename += suffix
print(beliefs_data_filename)
beliefs_df.to_pickle(os.path.join(str(Path.home()), "master_thesis/code/hythe-src/beliefs_data/", beliefs_data_filename))
beliefs_data_filename = "orig_beliefs_{}_{}_{}".format(splits, num_samples, num_steps)
beliefs_data_filename += suffix
print(beliefs_data_filename)
beliefs_orig_df.to_pickle(os.path.join(str(Path.home()), "master_thesis/code/hythe-src/beliefs_data/", beliefs_data_filename))
video_filename = os.path.join(str(Path.home()), "master_thesis/code/hythe-src/beliefs_data/", "video_{}".format(num_samples))
print(video_filename)
video_filename += suffix
print(video_filename)
video_renderer.export_video(filename=video_filename)
return
from maude_interface import make_initial_world, get_ego_agent, apply_action_to_ego_agent
from bark.runtime.viewer.matplotlib_viewer import MPViewer
from bark.runtime.commons.parameters import ParameterServer
from bark.runtime.viewer.video_renderer import VideoRenderer
import time
# list of tuples with acceleration / steering angle
primitives = [(0, 0), (-5, 0), (5, 0)]
world = make_initial_world(primitives)
ego_agent = get_ego_agent(world)
apply_action_to_ego_agent(world,
0) # applies action with idx 0 (keep velocity)
params = ParameterServer()
viewer = MPViewer(params=params, use_world_bounds=True)
# World Simulation
sim_step_time = params["simulation"]["step_time", "Step-time in simulation",
0.2]
sim_real_time_factor = params["simulation"]["real_time_factor",
"execution in real-time or faster",
1]
# Draw map
video_renderer = VideoRenderer(renderer=viewer, world_step_time=sim_step_time)
for _ in range(0, 20):
world.Step(sim_step_time)
viewer.clear()
viewer.drawWorld(world)