def test_obs_noise():
env = PGDriveEnvV2({
"vehicle_config": {
"lidar": {
"gaussian_noise": 1.0,
"dropout_prob": 1.0
}
}
})
try:
obs = env.reset()
obs_cls = env.observations[env.DEFAULT_AGENT]
assert isinstance(obs_cls, LidarStateObservation)
ret = obs_cls._add_noise_to_cloud_points([0.5, 0.5, 0.5],
gaussian_noise=1.0,
dropout_prob=1.0)
np.testing.assert_almost_equal(np.array(ret), 0.0)
assert env.observation_space.contains(obs)
_act(env, env.action_space.sample())
for x in [-1, 0, 1]:
env.reset()
for y in [-1, 0, 1]:
_act(env, [x, y])
finally:
env.close()
env = PGDriveEnvV2({
"vehicle_config": {
"lidar": {
"gaussian_noise": 0.0,
"dropout_prob": 0.0
}
}
})
try:
obs = env.reset()
obs_cls = env.observations[env.DEFAULT_AGENT]
assert isinstance(obs_cls, LidarStateObservation)
ret = obs_cls._add_noise_to_cloud_points([0.5, 0.5, 0.5],
gaussian_noise=0.0,
dropout_prob=0.0)
assert not np.all(np.array(ret) == 0.0)
np.testing.assert_equal(np.array(ret), np.array([0.5, 0.5, 0.5]))
assert env.observation_space.contains(obs)
_act(env, env.action_space.sample())
for x in [-1, 0, 1]:
env.reset()
for y in [-1, 0, 1]:
_act(env, [x, y])
finally:
env.close()
def test_out_of_road():
# env = PGDriveEnvV2(dict(vehicle_config=dict(side_detector=dict(num_lasers=8))))
for steering in [-0.01, 0.01]:
for distance in [10, 50, 100]:
env = PGDriveEnvV2(
dict(
map="SSSSSSSSSSS",
vehicle_config=dict(
side_detector=dict(num_lasers=120, distance=distance)),
use_render=False,
fast=True))
try:
obs = env.reset()
tolerance = math.sqrt(env.vehicle.WIDTH**2 +
env.vehicle.LENGTH**2) / distance
for _ in range(100000000):
o, r, d, i = env.step([steering, 1])
if d:
points = env.vehicle.side_detector.get_cloud_points()
assert min(points) < tolerance, (min(points),
tolerance)
print(
"Side detector minimal distance: {}, Current distance: {}, steering: {}"
.format(
min(points) * distance, distance, steering))
break
finally:
env.close()
def useless_left_right_distance_printing():
# env = PGDriveEnvV2(dict(vehicle_config=dict(side_detector=dict(num_lasers=8))))
for steering in [-0.01, 0.01, -1, 1]:
# for distance in [10, 50, 100]:
env = PGDriveEnvV2(
dict(map="SSSSSSSSSSS",
vehicle_config=dict(
side_detector=dict(num_lasers=0, distance=50)),
use_render=False,
fast=True))
try:
for _ in range(100000000):
o, r, d, i = env.step([steering, 1])
vehicle = env.vehicle
l, r = vehicle.dist_to_left_side, vehicle.dist_to_right_side
total_width = float(
(vehicle.routing_localization.get_current_lane_num() + 1) *
vehicle.routing_localization.get_current_lane_width())
print("Left {}, Right {}, Total {}. Clip Total {}".format(
l / total_width, r / total_width, (l + r) / total_width,
clip(l / total_width, 0, 1) + clip(r / total_width, 0, 1)))
if d:
break
finally:
env.close()
def default_config(cls) -> PGConfig:
config = PGDriveEnvV2.default_config()
config["vehicle_config"]["lidar"]["num_others"] = 0
config["vehicle_config"]["lidar"]["num_lasers"] = 240
config["vehicle_config"]["side_detector"]["num_lasers"] = 120
config["vehicle_config"]["num_stacks"] = 1
config["obs_mode"] = None # ["w_navi", "w_ego", "w_both"]
return config
def test_seeding():
env = PGDriveEnvV2()
try:
env.seed(999999)
assert env.pg_world is None
assert env.current_seed != 999999
env.reset()
assert env.current_seed == 999999
assert env.pg_world is not None
finally:
env.close()
def test_pgdrive_env_rgb():
env = PGDriveEnvV2(dict(use_image=True))
try:
obs = env.reset()
assert env.observation_space.contains(obs)
_act(env, env.action_space.sample())
for x in [-1, 0, 1]:
env.reset()
for y in [-1, 0, 1]:
_act(env, [x, y])
finally:
env.close()
def test_pgdrive_env_v2():
env = PGDriveEnvV2({"vehicle_config": {"wheel_friction": 1.2}})
try:
obs = env.reset()
assert env.observation_space.contains(obs)
_act(env, env.action_space.sample())
for x in [-1, 0, 1]:
env.reset()
for y in [-1, 0, 1]:
_act(env, [x, y])
# print('finish {}! \n'.format((x, y)))
finally:
env.close()
def default_config(cls) -> PGConfig:
config = PGDriveEnvV2.default_config()
config["vehicle_config"]["lidar"] = {"num_lasers": 0, "distance": 0} # Remove lidar
config.update(
{
"frame_skip": 5,
"frame_stack": 3,
"post_stack": 5,
"rgb_clip": True,
"resolution_size": 84,
"distance": 30
}
)
return config
def test_pgdrive_env_v2_long_run():
try:
for m in ["X", "O", "C", "S", "R", "r", "T"]:
env = PGDriveEnvV2({"map": m})
o = env.reset()
for _ in range(300):
assert env.observation_space.contains(o)
o, r, d, i = env.step([0, 1])
if d:
break
env.close()
finally:
if "env" in locals():
env = locals()["env"]
env.close()
def local_test_close_and_restart():
try:
for m in ["X", "O", "C", "S", "R", "r", "T"]:
env = PGDriveEnvV2({"map": m, "use_render": True, "fast": True})
o = env.reset()
for _ in range(300):
assert env.observation_space.contains(o)
o, r, d, i = env.step([1, 1])
if d:
break
env.close()
finally:
if "env" in locals():
env = locals()["env"]
env.close()
def local_test_apply_action():
try:
env = PGDriveEnvV2({"map": "SSS", "use_render": True, "fast": True})
o = env.reset()
for act in [-1, 1]:
for _ in range(300):
assert env.observation_space.contains(o)
o, r, d, i = env.step([act, 1])
if d:
o = env.reset()
break
env.close()
finally:
if "env" in locals():
env = locals()["env"]
env.close()
def default_config() -> PGConfig:
config = PGDriveEnvV2.default_config()
target_vehicle_configs_dict = dict()
for agent_id in range(MultiAgentIntersectPGDrive.DEFAULT_AGENT_NUM):
target_vehicle_configs_dict[f"agent{agent_id}"] = dict()
config.update(
{
"environment_num": 1,
"traffic_density": 0.,
"start_seed": 10,
"vehicle_config": {
# "lane_line_detector": {
# "num_lasers": 100
# }
},
"target_vehicle_configs": target_vehicle_configs_dict,
"num_agents": MultiAgentIntersectPGDrive.DEFAULT_AGENT_NUM,
"crash_done": True
}
)
# Some collision bugs still exist, always set to False now!!!!
# config.extend_config_with_unknown_keys({"crash_done": True})
return config
def test_random_traffic():
env = PGDriveEnvV2({
"random_traffic": True,
"traffic_mode": "respawn",
# "fast": True, "use_render": True
})
try:
last_pos = None
for i in range(20):
obs = env.reset()
assert env.scene_manager.traffic_manager.random_traffic
assert env.scene_manager.traffic_manager.random_seed is None
new_pos = [
v.position for v in env.scene_manager.traffic_manager.vehicles
]
if last_pos is not None and len(new_pos) == len(last_pos):
assert sum([
norm(lastp[0] - newp[0], lastp[1] - newp[1]) >= 0.5
for lastp, newp in zip(last_pos, new_pos)
]), [(lastp, newp) for lastp, newp in zip(last_pos, new_pos)]
last_pos = new_pos
finally:
env.close()
from pgdrive.envs.pgdrive_env_v2 import PGDriveEnvV2
if __name__ == "__main__":
env = PGDriveEnvV2({
"environment_num": 100,
"start_seed": 5000,
"traffic_density": 0.08,
})
env.reset()
count = []
for i in range(1, 101):
o, r, d, info = env.step([0, 1])
env.reset()
print("Current map {}, vehicle number {}.".format(
env.current_seed,
env.scene_manager.traffic_manager.get_vehicle_num()))
count.append(env.scene_manager.traffic_manager.get_vehicle_num())
print(min(count), sum(count) / len(count), max(count))
env.close()
def default_config() -> PGConfig:
config = PGDriveEnvV2.default_config()
config.update(MULTI_AGENT_PGDRIVE_DEFAULT_CONFIG)
return config
from pgdrive.constants import TerminationState
from pgdrive.envs.pgdrive_env_v2 import PGDriveEnvV2
if __name__ == "__main__":
env = PGDriveEnvV2({
"environment_num": 1,
"traffic_density": 0.3,
"start_seed": 5,
# "controller": "joystick",
"manual_control": True,
"use_render": True,
"vehicle_config": {
"use_saver": True
},
"map": 16
})
o = env.reset()
env.pg_world.force_fps.toggle()
for i in range(1, 100000):
o, r, d, info = env.step([0, 1])
text = {"save": info["takeover_start"]}
env.render(text=text)
if info[TerminationState.SUCCESS]:
env.reset()
env.close()