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_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_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 useless_left_right_distance_printing():
# env = PGDriveEnv(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 = PGDriveEnv(
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.navigation.get_current_lane_num() + 1) *
vehicle.navigation.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 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_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 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 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_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 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 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_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 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 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 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_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.envs.pgdrive_env import PGDriveEnv
if __name__ == "__main__":
env = PGDriveEnv({
"environment_num": 1,
"traffic_density": 0.3,
"traffic_mode": "reborn",
"start_seed": 5,
# "controller": "joystick",
"manual_control": True,
"use_render": True,
"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": env.save_mode}
env.render(text=text)
if info["arrive_dest"]:
env.reset()
env.close()
# "destination_node":"2R1_3_",
# "show_side_detector": True,
# "show_lane_line_detector": True,
# "side_detector": dict(num_lasers=2, distance=50),
# "lane_line_detector": dict(num_lasers=2, distance=50),
# # "show_line_to_dest": True,
# "show_dest_mark": True
}
})
import time
start = time.time()
o = env.reset()
for s in range(1, 100000):
o, r, d, info = env.step(env.action_space.sample())
# info["fuel"] = env.vehicle.energy_consumption
env.render(
text={
"heading_diff": env.vehicle.heading_diff(env.vehicle.lane),
"engine_force": env.vehicle.config["max_engine_force"],
"current_seed": env.current_seed,
"lane_width": env.vehicle.lane.width
})
print({
env.engine.get_policy(
env.vehicle.id).controller.joystick.get_button(4)
})
# assert env.observation_space.contains(o)
# if (s + 1) % 100 == 0:
# print(
# "use_render":True,
"map": "SSSSS",
# "manual_control":True,
"controller": "joystick",
"random_agent_model": False,
"vehicle_config": {
"vehicle_model": "default",
# "vehicle_model":"s",
# "vehicle_model":"m",
# "vehicle_model":"l",
# "vehicle_model":"xl",
}
}
env = PGDriveEnv(config)
import time
start = time.time()
o = env.reset()
a = [.0, 1.]
for s in range(1, 100000):
o, r, d, info = env.step(a)
if env.vehicle.speed > 100:
a = [0, -1]
print("0-100 km/h acc use time:{}".format(s * 0.1))
pre_pos = env.vehicle.position[0]
if a == [0, -1] and env.vehicle.speed < 1:
print("0-100 brake use dist:{}".format(env.vehicle.position[0] -
pre_pos))
break
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()
