def test_gen_map_read():
setup_logger(debug=True)
env = PGDriveEnv({"environment_num": 10, "load_map_from_json": False})
try:
data = env.dump_all_maps()
with open("test_10maps.json", "w") as f:
json.dump(data, f)
with open("test_10maps.json", "r") as f:
restored_data = json.load(f)
env.close()
env = PGDriveEnv({
"environment_num": 10,
})
env.lazy_init()
print("Start loading.")
env.engine.map_manager.read_all_maps(restored_data)
while any([v is None for v in env.maps.values()]):
env.reset()
for i in range(10):
m = env.maps[i].save_map()
recursive_equal(m, data["map_data"][i], need_assert=True)
print("Finish!")
finally:
env.close()
def test_traffic_respawn(vis=False):
setup_logger(vis)
env = PGDriveEnv(
{
"environment_num": 1,
"manual_control": vis,
"use_render": vis,
"use_topdown": True,
"traffic_mode": "respawn"
}
)
env.reset()
try:
for i in range(1, 3000):
env.step([0, 0])
current_v = set(env.engine.traffic_manager.vehicles)
for v in list(env.engine.traffic_manager.traffic_vehicles) + [env.vehicle]:
if v is env.vehicle:
current_v.discard(v)
else:
current_v.discard(v)
assert len(current_v) == 0, "vehicles didn't release"
assert len(env.engine.traffic_manager.vehicles) - len(env.engine.traffic_manager.traffic_vehicles) == 1, \
"vehicles didn't release"
finally:
env.close()
def test_pgdrive_env_rgb():
env = PGDriveEnv(dict(offscreen_render=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_save_map_image():
os.makedirs("tmp_images", exist_ok=True)
setup_logger(debug=True)
env = PGDriveEnv(dict(environment_num=20, start_seed=0, map=10))
try:
for i in range(5):
env.reset()
surface = draw_top_down_map(env.current_map, resolution=(128, 128))
plt.imshow(surface, cmap="Greys")
plt.savefig("tmp_images/map_{}.png".format(i))
env.close()
finally:
env.close()
def test_lidar_with_mask(render=False):
setup_logger(debug=True)
env = PGDriveEnv({
"use_render": render,
"manual_control": render,
"environment_num": 1,
"traffic_density": 0.3,
"vehicle_config": {
"show_lidar": True,
"side_detector": dict(num_lasers=2, distance=50),
"lane_line_detector": dict(num_lasers=2, distance=50),
},
"_disable_detector_mask": False,
"map": "XXX"
})
try:
env.reset()
v_config = env.config["vehicle_config"]
v_config["spawn_longitude"] = 0
v_config["spawn_lateral"] = 7.5
another_v = DefaultVehicle(v_config, random_seed=0)
another_v.reset()
# for test
objs = env.vehicle.side_detector.perceive(
env.vehicle, env.vehicle.engine.physics_world.static_world
).detected_objects + env.vehicle.lane_line_detector.perceive(
env.vehicle,
env.vehicle.engine.physics_world.static_world).detected_objects
yellow = 0
for obj in objs:
if obj.getNode().getName() == BodyName.Yellow_continuous_line:
yellow += 1
assert yellow == 2, "side detector and lane detector broken"
detect_traffic_vehicle = False
detect_base_vehicle = False
for i in range(1, 100000):
o, r, d, info = env.step([0, 1])
if len(
env.vehicle.lidar.get_surrounding_vehicles(
env.observations[DEFAULT_AGENT].detected_objects)) > 2:
detect_traffic_vehicle = True
for hit in env.observations[DEFAULT_AGENT].detected_objects:
if isinstance(hit, BaseVehicle):
detect_base_vehicle = True
if d:
break
if not (detect_traffic_vehicle and detect_base_vehicle):
print("Lidar detection failed")
assert detect_traffic_vehicle and detect_base_vehicle, "Lidar detection failed"
finally:
env.close()
def test_destroy():
# Close and reset
env = PGDriveEnv({
"environment_num": 1,
"start_seed": 3,
"manual_control": False
})
try:
env.reset()
for i in range(1, 20):
env.step([1, 1])
env.close()
env.reset()
env.close()
# Again!
env = PGDriveEnv({
"environment_num": 1,
"start_seed": 3,
"manual_control": False
})
env.reset()
for i in range(1, 20):
env.step([1, 1])
env.reset()
env.close()
finally:
env.close()
def test_save_episode(vis=False):
setup_logger(True)
test_dump = True
save_episode = True,
vis = True
env = PGDriveEnv(
{
"environment_num": 1,
"traffic_density": 0.1,
"start_seed": 5,
# "manual_control": vis,
"use_render": vis,
"traffic_mode": TrafficMode.Trigger,
"record_episode": save_episode,
"map_config": {
BaseMap.GENERATE_TYPE: MapGenerateMethod.BIG_BLOCK_SEQUENCE,
BaseMap.GENERATE_CONFIG: "XTXTXTXTXT",
BaseMap.LANE_WIDTH: 3.5,
BaseMap.LANE_NUM: 3,
}
}
)
try:
o = env.reset()
epi_info = None
for i in range(1, 100000 if vis else 2000):
o, r, d, info = env.step([0, 1])
if vis:
env.render()
if d:
epi_info = env.engine.dump_episode()
# test dump json
if test_dump:
with open("test.json", "w") as f:
json.dump(epi_info, f)
break
o = env.reset(epi_info)
for i in range(1, 100000 if vis else 2000):
o, r, d, info = env.step([0, 1])
if vis:
env.render()
if d:
break
finally:
env.close()
def test_save_map_image():
os.makedirs("tmp_images", exist_ok=True)
setup_logger(debug=True)
env = PGDriveEnv(dict(environment_num=20, start_seed=0, map=10))
try:
for i in range(5):
env.reset()
surface = env.get_map(resolution=(128, 128))
plt.imshow(surface, cmap="Greys")
plt.savefig("tmp_images/map_{}.png".format(i))
env.current_map.draw_navi_line(env.vehicle,
dest_resolution=(2048, 2048),
save=False)
env.close()
finally:
env.close()
def local_test_apply_action():
try:
env = PGDriveEnv({"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 test_get_lane_index(use_render=False):
env = PGDriveEnv({
"map": "rRCXSOTCR",
"environment_num": 1,
"traffic_density": 0.3,
"traffic_mode": "respawn",
"use_render": use_render
})
try:
o = env.reset()
for i in range(1, 1000):
o, r, d, info = env.step([0, 0])
for v in env.scene_manager.traffic_manager.vehicles:
old_res = env.current_map.road_network.get_closest_lane_index(
v.position, True)
old_lane_idx = [index[1] for index in old_res]
if v.lane_index not in old_lane_idx:
raise ValueError((v.lane_index), old_lane_idx)
else:
idx = old_lane_idx.index(v.lane_index)
if old_res[idx][0] > 2. and idx > 2:
raise ValueError(
"L1 dist:{} of {} is too large".format(
old_res[idx][0], idx))
finally:
env.close()
def test_out_of_road():
# env = PGDriveEnv(dict(vehicle_config=dict(side_detector=dict(num_lasers=8))))
for steering in [-0.01, 0.01]:
for distance in [10, 50, 100]:
env = PGDriveEnv(
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.perceive(env.vehicle,
env.vehicle.engine.physics_world.static_world).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 test_top_down_rendering():
env = PGDriveEnv(
dict(environment_num=20,
start_seed=0,
map=10,
use_topdown=True,
use_render=False))
try:
env.reset()
for i in range(5):
env.step(env.action_space.sample())
env.render(mode="human")
env.render(mode="rgb_array")
finally:
pygame.image.save(env.pg_world.highway_render.frame_surface,
"save_offscreen.jpg")
env.close()
def vis_installation(headless=True):
loadPrcFileData("", "notify-level-task fatal")
try:
env = PGDriveEnv({"use_render": False, "offscreen_render": False})
env.reset()
for i in range(1, 100):
o, r, d, info = env.step([0, 1])
env.close()
del env
except:
print("Error happens in Bullet physics world !")
sys.exit()
else:
print("Bullet physics world is launched successfully!")
try:
capture_image(headless)
except:
print("Error happens when drawing scene in offscreen mode!")
def vis_highway_render_with_panda_render():
setup_logger(True)
env = PGDriveEnv({
"environment_num": 1,
"manual_control": True,
"use_render": True,
"use_image": False,
"use_topdown": True,
})
o = env.reset()
s = time.time()
for i in range(1, 100000):
o, r, d, info = env.step(env.action_space.sample())
env.render()
if d:
env.reset()
if i % 1000 == 0:
print("Steps: {}, Time: {}".format(i, time.time() - s))
env.close()
def test_config_sync():
"""
The config in BaseEngine should be the same as env.config, if BaseEngine exists in process
"""
try:
env = PGDriveEnv(
{"vehicle_config": dict(show_lidar=False, show_navi_mark=False)})
env.reset()
recursive_equal(env.config, env.engine.global_config)
env.config.update(
{"vehicle_config": dict(show_lidar=True, show_navi_mark=True)})
recursive_equal(env.config, env.engine.global_config)
env.close()
env.reset()
recursive_equal(env.config, env.engine.global_config)
env.engine.global_config.update(
{"vehicle_config": dict(show_lidar=False, show_navi_mark=False)})
recursive_equal(env.config, env.engine.global_config)
finally:
env.close()
def test_line_contact():
env = PGDriveEnv({"traffic_density": .0})
o = env.reset()
on_broken_line = False
on_continuous_line = False
try:
for i in range(1, 100):
o, r, d, info = env.step([-0.5, 1])
on_broken_line = on_broken_line or env.vehicle.on_broken_line
on_continuous_line = on_continuous_line or env.vehicle.on_white_continuous_line
assert on_broken_line and on_continuous_line, "Collision function is broken!"
finally:
env.close()
def test_collision_with_vehicle():
env = PGDriveEnv({"traffic_density": 1.0})
o = env.reset()
pass_test = False
try:
for i in range(1, 100):
o, r, d, info = env.step([0, 1])
if env.vehicle.crash_vehicle:
pass_test = True
break
assert pass_test, "Collision function is broken!"
finally:
env.close()
def capture_image(headless):
env = PGDriveEnv(
dict(use_render=False,
start_seed=666,
traffic_density=0.1,
offscreen_render=True,
headless_machine_render=headless))
env.reset()
for i in range(10):
env.step([0, 1])
img = PNMImage()
env.engine.win.getScreenshot(img)
img.write("vis_installation.png")
env.close()
if not headless:
im = Image.open("vis_installation.png")
im.show()
os.remove("vis_installation.png")
print("Offscreen render launched successfully! \n ")
else:
print(
"Headless mode Offscreen render launched successfully! \n "
"A image named \'tset_install.png\' is saved. Open it to check if offscreen mode works well"
)
def local_test_close_and_restart():
try:
for m in ["X", "O", "C", "S", "R", "r", "T"]:
env = PGDriveEnv({"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 test_get_lane_index(use_render=False):
env = PGDriveEnv(
{
"map": "rRCXSOTCR",
"environment_num": 1,
"traffic_density": 0.3,
"traffic_mode": "reborn",
"use_render": use_render
}
)
o = env.reset()
for i in range(1, 1000):
o, r, d, info = env.step([0, 0])
for v in env.scene_manager.traffic.vehicles:
old = env.current_map.road_network.get_closest_lane_index(v.position)
if v.lane_index != old[0]:
raise ValueError
env.close()
def test_obs_noise():
env = PGDriveEnv({
"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 = PGDriveEnv({
"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_collision_with_vehicle(use_render=False):
if not use_render:
env = PGDriveEnv({"traffic_density": 1.0, "map": "SSS"})
else:
env = PGDriveEnv({
"traffic_density": 1.0,
"map": "SSS",
"use_render": True,
"fast": True
})
o = env.reset()
pass_test = False
try:
for i in range(1, 500):
o, r, d, info = env.step([0, 1])
if env.vehicle.crash_vehicle:
pass_test = True
break
assert pass_test, "Collision function is broken!"
finally:
env.close()
from pgdrive.envs.pgdrive_env import PGDriveEnv
if __name__ == "__main__":
env = PGDriveEnv({
"map": 30,
"environment_num": 1,
"traffic_density": 0.1,
"pstats": True,
"traffic_mode": "respawn"
})
o = env.reset()
for i in range(1, 10000):
print(i)
o, r, d, info = env.step([0, 0])
env.close()
from pgdrive.component.map.base_map import BaseMap, MapGenerateMethod
from pgdrive.envs.pgdrive_env import PGDriveEnv
from pgdrive.utils import setup_logger
setup_logger(debug=True)
if __name__ == "__main__":
env = PGDriveEnv({
"environment_num": 4,
"traffic_density": 0.1,
"start_seed": 3,
"image_source": "mini_map",
"manual_control": True,
"use_render": True,
"offscreen_render": False,
"decision_repeat": 5,
"rgb_clip": True,
"map_config": {
BaseMap.GENERATE_TYPE: MapGenerateMethod.BIG_BLOCK_NUM,
BaseMap.GENERATE_CONFIG: 12,
BaseMap.LANE_WIDTH: 3.5,
BaseMap.LANE_NUM: 3,
}
})
env.reset()
for i in range(1, 100000):
o, r, d, info = env.step([0, 1])
env.render(text={"Frame": i, "Speed": env.vehicle.speed})
env.close()
def test_navigation(vis=False):
env = PGDriveEnv({
"environment_num": 10,
"traffic_density": 0.0,
"use_render": vis,
"start_seed": 5,
"map_config": {
BaseMap.GENERATE_TYPE: MapGenerateMethod.BIG_BLOCK_NUM,
BaseMap.GENERATE_CONFIG: 7,
BaseMap.LANE_WIDTH: 3.5,
BaseMap.LANE_NUM: 3,
}
})
target = Target(0.375, 30)
o = env.reset()
if vis:
env.engine.accept('d', target.go_right)
env.engine.accept('a', target.go_left)
env.engine.accept('w', target.faster)
env.engine.accept('s', target.slower)
steering_controller = PIDController(1.6, 0.0008, 27.3)
acc_controller = PIDController(0.1, 0.001, 0.3)
steering_error = o[0] - target.lateral
steering = steering_controller.get_result(steering_error)
acc_error = env.vehicles[env.DEFAULT_AGENT].speed - target.speed
acc = acc_controller.get_result(acc_error)
for i in range(1, 1000000 if vis else 2000):
o, r, d, info = env.step([-steering, acc])
# calculate new action
steering_error = o[0] - target.lateral
steering = steering_controller.get_result(steering_error)
t_speed = target.speed if abs(o[12] -
0.5) < 0.01 else target.speed - 10
acc_error = env.vehicles[env.DEFAULT_AGENT].speed - t_speed
acc = acc_controller.get_result(acc_error)
if vis:
if i < 700:
env.render(
text={
"W": "Target speed +",
"S": "Target speed -",
"A": "Change to left lane",
"D": "Change to right lane"
})
if i == 500:
env.engine.on_screen_message.data.clear()
else:
env.render()
if d:
print("Reset")
o = env.reset()
steering_controller.reset()
steering_error = o[0] - target.lateral
steering = steering_controller.get_result(steering_error, o[11])
acc_controller.reset()
acc_error = env.vehicles[env.DEFAULT_AGENT].speed - target.speed
acc = acc_controller.get_result(acc_error)
env.close()
