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()