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.engine.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] > 4. and idx > 2:
raise ValueError(
"L1 dist:{} of {} is too large".format(
old_res[idx][0], idx))
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()
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_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 __init__(self, env_config):
self.ray = import_ray()
assert self.ray is not None, "Please install ray via: pip install ray " \
"if you wish to use multiple PGDrive in single process."
# Temporary environment
tmp = PGDriveEnv(env_config)
self.action_space = tmp.action_space
self.observation_space = tmp.observation_space
self.reward_range = tmp.reward_range
tmp.close()
del tmp
self.env = None
self.env_config = env_config
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 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 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_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 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 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_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 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"
)
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()
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()
break
env.render()
return spend, dist, rotate_displacement
if __name__ == '__main__':
for friction in [0.6, 0.8, 0.9, 1.0, 1.1, 1.2, 1.4, 1.6, 1.8, 2.0]:
# for friction in [0.9, 1.0, 1.1]:
env = PGDriveEnv({
"environment_num": 1,
"traffic_density": 0.0,
"start_seed": 4,
"pg_world_config": {
"debug": False,
},
"manual_control": False,
"use_render": True,
"use_image": True,
"map_config": {
Map.GENERATE_METHOD: MapGenerateMethod.BIG_BLOCK_SEQUENCE,
Map.GENERATE_PARA: "SSSSSSSSSS"
}
})
acc_time, brake_dist, rotate_dis = get_result(env)
print(
"Friction {}. Acceleration time: {:.3f}. Brake distance: {:.3f}. Rotation 90 degree displacement X: {:.3f}, Y: {:.3f}"
.format(friction, acc_time, brake_dist, rotate_dis[0],
rotate_dis[1]))
env.close()
"environment_num": 10,
"traffic_density": .0,
# "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
from pgdrive.envs.pgdrive_env import PGDriveEnv
if __name__ == "__main__":
env = PGDriveEnv({
"environment_num": 1,
"traffic_density": 0.1,
"start_seed": 4,
"image_source": "mini_map",
"manual_control": True,
"use_render": True,
"offscreen_render": True,
"rgb_clip": True,
"headless_machine_render": False
})
env.reset()
env.engine.accept(
"m", env.vehicle.image_sensors[env.config["image_source"]].save_image)
for i in range(1, 100000):
o, r, d, info = env.step([0, 1])
assert env.observation_space.contains(o)
if env.config["use_render"]:
# from pgdrive.envs.observation_type import ObservationType, ImageObservation
# for i in range(ImageObservation.STACK_SIZE):
# ObservationType.show_gray_scale_array(o["image"][:, :, i])
env.render(text={"can you see me": i})
if d:
print("Reset")
env.reset()
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()
from pgdrive.envs.pgdrive_env import PGDriveEnv
if __name__ == "__main__":
env = PGDriveEnv({
"environment_num": 1,
"traffic_density": 0.1,
"start_seed": 4,
"manual_control": True,
"use_render": True,
"offscreen_render":
True, # it is a switch telling pgdrive to use rgb as observation
"rgb_clip": True, # clip rgb to range(0,1) instead of (0, 255)
"pstats": True,
})
env.reset()
# print m to capture rgb observation
env.engine.accept("m",
env.vehicle.image_sensors[
env.vehicle.config["image_source"]].save_image,
extraArgs=[env.vehicle])
for i in range(1, 100000):
o, r, d, info = env.step([0, 1])
assert env.observation_space.contains(o)
# if env.config["use_render"]:
# for i in range(ImageObservation.STACK_SIZE):
# ObservationType.show_gray_scale_array(o["image"][:, :, i])
# image = env.render(mode="any str except human", text={"can you see me": i})
# ObservationType.show_gray_scale_array(image)
if d:
print("Reset")
import time
from pgdrive.envs.pgdrive_env import PGDriveEnv
from pgdrive.scene_creator.algorithm.BIG import BigGenerateMethod
from pgdrive.scene_creator.map import Map
if __name__ == "__main__":
env = PGDriveEnv(
dict(use_render=True,
map_config={
Map.GENERATE_METHOD: BigGenerateMethod.BLOCK_NUM,
Map.GENERATE_PARA: 7
},
traffic_density=0.5,
manual_control=True,
traffic_mode=0))
start = time.time()
env.reset()
env.render()
print("Render cost time: ", time.time() - start)
while True:
o, r, d, info = env.step([0, 1])
env.render()
# if d:
# env.reset()
env.close()
env = PGDriveEnv({
"environment_num":
1,
# "traffic_density": 1.0,
"traffic_mode":
"hybrid",
"start_seed":
82,
"onscreen_message":
True,
# "debug_physics_world": True,
# "pstats": True,
# "show_fps":False,
# "random_traffic":True,
"vehicle_config":
dict(
mini_map=(168 * w_f * 6, 84 * h_f * 6,
270), # buffer length, width
rgb_camera=(168 * w_f, 84 * h_f), # buffer length, width
depth_camera=(168 * w_f, 84 * h_f,
True), # buffer length, width, view_ground
show_navi_mark=False,
increment_steering=False,
wheel_friction=0.6,
show_lidar=True),
# "camera_height":100,
# "controller":"joystick",
"image_source":
"mini_map",
"manual_control":
True,
"use_render":
True,
"use_topdown":
True,
"decision_repeat":
5,
"rgb_clip":
True,
# "debug":True,
"map_config": {
BaseMap.GENERATE_TYPE: MapGenerateMethod.BIG_BLOCK_SEQUENCE,
BaseMap.GENERATE_CONFIG: "rrXCO",
BaseMap.LANE_WIDTH: 3.5,
BaseMap.LANE_NUM: 3,
}
})
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()
from pgdrive.envs.pgdrive_env import PGDriveEnv
if __name__ == "__main__":
env = PGDriveEnv({
"environment_num":
1,
"traffic_density":
0.1,
"start_seed":
4,
"manual_control":
True,
"use_render":
True,
"offscreen_render":
True,
"rgb_clip":
True,
"vehicle_config":
dict(depth_camera=(200, 88, False), image_source="depth_camera"),
"headless_machine_render":
False,
"map_config": {
BaseMap.GENERATE_TYPE: MapGenerateMethod.BIG_BLOCK_NUM,
BaseMap.GENERATE_CONFIG: 12,
BaseMap.LANE_WIDTH: 3.5,
BaseMap.LANE_NUM: 3,
}
})
env.reset()
env.engine.accept("m",
env.vehicle.image_sensors["depth_camera"].save_image,