From 20ce77f5623b856cdaf1675fe751cb87f28e9747 Mon Sep 17 00:00:00 2001 From: luigiberducci Date: Wed, 12 Jul 2023 23:12:48 +0200 Subject: [PATCH] general: code formatting, tests: rename module, rename test file "test_*" for auto test discovery, update test scan simulator and collision checks. --- examples/waypoint_follow.py | 10 +- gym/f110_gym/__init__.py | 6 +- gym/f110_gym/envs/action.py | 2 - gym/f110_gym/envs/base_classes.py | 1 - gym/f110_gym/envs/collision_models.py | 124 +++++---- gym/f110_gym/envs/f110_env_backup.py | 191 ++++++++----- gym/f110_gym/envs/laser_models.py | 253 ++++++++++++------ gym/f110_gym/envs/rendering.py | 131 +++++---- gym/f110_gym/{unittest => test}/__init__.py | 0 .../{unittest => test}/legacy_scan.npz | Bin .../{unittest => test}/legacy_scan_gen.py | 73 ++--- .../pyglet_test.py => test/pyglet_test_.py} | 76 ++++-- gym/f110_gym/test/pyglet_test_camera.py | 127 +++++++++ .../test_collision_checks.py} | 87 +++--- .../test_dynamics.py} | 91 ++++++- .../test_f110_env.py} | 0 .../test_observation.py} | 0 .../scan_sim.py => test/test_scan_sim.py} | 187 ++----------- .../track_test.py => test/test_track.py} | 29 +- .../utils_test.py => test/test_utils.py} | 0 gym/f110_gym/unittest/pyglet_test_camera.py | 131 --------- setup.py | 39 +-- 22 files changed, 872 insertions(+), 686 deletions(-) rename gym/f110_gym/{unittest => test}/__init__.py (100%) rename gym/f110_gym/{unittest => test}/legacy_scan.npz (100%) rename gym/f110_gym/{unittest => test}/legacy_scan_gen.py (58%) rename gym/f110_gym/{unittest/pyglet_test.py => test/pyglet_test_.py} (72%) create mode 100644 gym/f110_gym/test/pyglet_test_camera.py rename gym/f110_gym/{unittest/collision_checks.py => test/test_collision_checks.py} (82%) rename gym/f110_gym/{unittest/dynamics_test.py => test/test_dynamics.py} (96%) rename gym/f110_gym/{unittest/f110_env_test.py => test/test_f110_env.py} (100%) rename gym/f110_gym/{unittest/observation_test.py => test/test_observation.py} (100%) rename gym/f110_gym/{unittest/scan_sim.py => test/test_scan_sim.py} (65%) rename gym/f110_gym/{unittest/track_test.py => test/test_track.py} (89%) rename gym/f110_gym/{unittest/utils_test.py => test/test_utils.py} (100%) delete mode 100644 gym/f110_gym/unittest/pyglet_test_camera.py diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py index 415886e5..5a0ffcea 100644 --- a/examples/waypoint_follow.py +++ b/examples/waypoint_follow.py @@ -340,7 +340,15 @@ def render_callback(env_renderer): env.add_render_callback(render_callback) - poses = np.array([[env.track.raceline.xs[0], env.track.raceline.ys[0], env.track.raceline.yaws[0]]]) + poses = np.array( + [ + [ + env.track.raceline.xs[0], + env.track.raceline.ys[0], + env.track.raceline.yaws[0], + ] + ] + ) obs, info = env.reset(options={"poses": poses}) done = False env.render() diff --git a/gym/f110_gym/__init__.py b/gym/f110_gym/__init__.py index 362e9035..20052799 100644 --- a/gym/f110_gym/__init__.py +++ b/gym/f110_gym/__init__.py @@ -1,6 +1,6 @@ import gymnasium as gym gym.register( - id='f110-v0', - entry_point='f110_gym.envs:F110Env', -) \ No newline at end of file + id="f110-v0", + entry_point="f110_gym.envs:F110Env", +) diff --git a/gym/f110_gym/envs/action.py b/gym/f110_gym/envs/action.py index e168cd74..a4372f3b 100644 --- a/gym/f110_gym/envs/action.py +++ b/gym/f110_gym/envs/action.py @@ -48,7 +48,6 @@ def __init__(self, params: Dict) -> None: self.steering_low, self.steering_high = params["sv_min"], params["sv_max"] self.acc_low, self.acc_high = -params["a_max"], params["a_max"] - def act(self, action: Tuple[float, float], state, params) -> Tuple[float, float]: return action @@ -68,7 +67,6 @@ def __init__(self, params: Dict) -> None: self.steering_low, self.steering_high = params["s_min"], params["s_max"] self.velocity_low, self.velocity_high = params["v_min"], params["v_max"] - def act( self, action: Tuple[float, float], state: np.ndarray, params: Dict ) -> Tuple[float, float]: diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py index 112e84ce..b2156d1d 100644 --- a/gym/f110_gym/envs/base_classes.py +++ b/gym/f110_gym/envs/base_classes.py @@ -303,7 +303,6 @@ def update_pose(self, raw_steer, vel): u_np = np.array([sv, accl]) - f_dynamics = self.model.f_dynamics self.state = self.integrator.integrate( f=f_dynamics, x=self.state, u=u_np, dt=self.time_step, params=self.params diff --git a/gym/f110_gym/envs/collision_models.py b/gym/f110_gym/envs/collision_models.py index 45885e7c..8961cdc8 100644 --- a/gym/f110_gym/envs/collision_models.py +++ b/gym/f110_gym/envs/collision_models.py @@ -21,7 +21,6 @@ # SOFTWARE. - """ Prototype of Utility functions and GJK algorithm for Collision checks between vehicles Originally from https://github.com/kroitor/gjk.c @@ -31,6 +30,7 @@ import numpy as np from numba import njit + @njit(cache=True) def perpendicular(pt): """ @@ -44,7 +44,7 @@ def perpendicular(pt): """ temp = pt[0] pt[0] = pt[1] - pt[1] = -1*temp + pt[1] = -1 * temp return pt @@ -61,7 +61,7 @@ def tripleProduct(a, b, c): """ ac = a.dot(c) bc = b.dot(c) - return b*ac - a*bc + return b * ac - a * bc @njit(cache=True) @@ -75,7 +75,7 @@ def avgPoint(vertices): Returns: avg (np.ndarray, (2,)): average point of the vertices """ - return np.sum(vertices, axis=0)/vertices.shape[0] + return np.sum(vertices, axis=0) / vertices.shape[0] @njit(cache=True) @@ -153,7 +153,7 @@ def collision(vertices1, vertices2): if index < 2: b = simplex[0, :] - ab = b-a + ab = b - a d = tripleProduct(ab, ao, ab) if np.linalg.norm(d) < 1e-10: d = perpendicular(ab) @@ -161,8 +161,8 @@ def collision(vertices1, vertices2): b = simplex[1, :] c = simplex[0, :] - ab = b-a - ac = c-a + ab = b - a + ac = c - a acperp = tripleProduct(ab, ac, ac) @@ -181,6 +181,7 @@ def collision(vertices1, vertices2): iter_count += 1 return False + @njit(cache=True) def collision_multiple(vertices): """ @@ -193,28 +194,30 @@ def collision_multiple(vertices): collisions (np.ndarray (num_vertices, )): whether each body is in collision collision_idx (np.ndarray (num_vertices, )): which index of other body is each index's body is in collision, -1 if not in collision """ - collisions = np.zeros((vertices.shape[0], )) - collision_idx = -1 * np.ones((vertices.shape[0], )) + collisions = np.zeros((vertices.shape[0],)) + collision_idx = -1 * np.ones((vertices.shape[0],)) # looping over all pairs - for i in range(vertices.shape[0]-1): - for j in range(i+1, vertices.shape[0]): + for i in range(vertices.shape[0] - 1): + for j in range(i + 1, vertices.shape[0]): # check collision vi = np.ascontiguousarray(vertices[i, :, :]) vj = np.ascontiguousarray(vertices[j, :, :]) ij_collision = collision(vi, vj) # fill in results if ij_collision: - collisions[i] = 1. - collisions[j] = 1. + collisions[i] = 1.0 + collisions[j] = 1.0 collision_idx[i] = j collision_idx[j] = i return collisions, collision_idx + """ Utility functions for getting vertices by pose and shape """ + @njit(cache=True) def get_trmtx(pose): """ @@ -231,9 +234,17 @@ def get_trmtx(pose): th = pose[2] cos = np.cos(th) sin = np.sin(th) - H = np.array([[cos, -sin, 0., x], [sin, cos, 0., y], [0., 0., 1., 0.], [0., 0., 0., 1.]]) + H = np.array( + [ + [cos, -sin, 0.0, x], + [sin, cos, 0.0, y], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) return H + @njit(cache=True) def get_vertices(pose, length, width): """ @@ -248,48 +259,52 @@ def get_vertices(pose, length, width): vertices (np.ndarray, (4, 2)): corner vertices of the vehicle body """ H = get_trmtx(pose) - rl = H.dot(np.asarray([[-length/2],[width/2],[0.], [1.]])).flatten() - rr = H.dot(np.asarray([[-length/2],[-width/2],[0.], [1.]])).flatten() - fl = H.dot(np.asarray([[length/2],[width/2],[0.], [1.]])).flatten() - fr = H.dot(np.asarray([[length/2],[-width/2],[0.], [1.]])).flatten() - rl = rl/rl[3] - rr = rr/rr[3] - fl = fl/fl[3] - fr = fr/fr[3] - vertices = np.asarray([[rl[0], rl[1]], [rr[0], rr[1]], [fr[0], fr[1]], [fl[0], fl[1]]]) + rl = H.dot(np.asarray([[-length / 2], [width / 2], [0.0], [1.0]])).flatten() + rr = H.dot(np.asarray([[-length / 2], [-width / 2], [0.0], [1.0]])).flatten() + fl = H.dot(np.asarray([[length / 2], [width / 2], [0.0], [1.0]])).flatten() + fr = H.dot(np.asarray([[length / 2], [-width / 2], [0.0], [1.0]])).flatten() + rl = rl / rl[3] + rr = rr / rr[3] + fl = fl / fl[3] + fr = fr / fr[3] + vertices = np.asarray( + [[rl[0], rl[1]], [rr[0], rr[1]], [fr[0], fr[1]], [fl[0], fl[1]]] + ) return vertices """ -Unit tests for GJK collision checks +Unit test for GJK collision checks Author: Hongrui Zheng """ import time import unittest + class CollisionTests(unittest.TestCase): def setUp(self): # test params np.random.seed(1234) # Collision check body - self.vertices1 = np.asarray([[4,11.],[5,5],[9,9],[10,10]]) + self.vertices1 = np.asarray([[4, 11.0], [5, 5], [9, 9], [10, 10]]) # car size self.length = 0.32 self.width = 0.22 - + def test_get_vert(self): test_pose = np.array([2.3, 6.7, 0.8]) vertices = get_vertices(test_pose, self.length, self.width) - rect = np.vstack((vertices, vertices[0,:])) + rect = np.vstack((vertices, vertices[0, :])) import matplotlib.pyplot as plt - plt.scatter(test_pose[0], test_pose[1], c='red') + + plt.scatter(test_pose[0], test_pose[1], c="red") plt.plot(rect[:, 0], rect[:, 1]) plt.xlim([1, 4]) plt.ylim([5, 8]) - plt.axes().set_aspect('equal') + plt.axes().set_aspect("equal") plt.show() self.assertTrue(vertices.shape == (4, 2)) @@ -299,41 +314,46 @@ def test_get_vert_fps(self): for _ in range(1000): vertices = get_vertices(test_pose, self.length, self.width) elapsed = time.time() - start - fps = 1000/elapsed - print('get vertices fps:', fps) - self.assertTrue(fps>500) + fps = 1000 / elapsed + print("get vertices fps:", fps) + self.assertTrue(fps > 500) def test_random_collision(self): # perturb the body by a small amount and make sure it all collides with the original body for _ in range(1000): - a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - self.assertTrue(collision(a,b)) + a = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + b = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + self.assertTrue(collision(a, b)) def test_multiple_collisions(self): - a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - c = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - d = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - e = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - f = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - g = self.vertices1 + 10. - allv = np.stack((a,b,c,d,e,f,g)) + a = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + b = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + c = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + d = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + e = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + f = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + g = self.vertices1 + 10.0 + allv = np.stack((a, b, c, d, e, f, g)) collisions, collision_idx = collision_multiple(allv) - self.assertTrue(np.all(collisions == np.array([1., 1., 1., 1., 1., 1., 0.]))) - self.assertTrue(np.all(collision_idx == np.array([5., 5., 5., 5., 5., 4., -1.]))) + self.assertTrue( + np.all(collisions == np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0])) + ) + self.assertTrue( + np.all(collision_idx == np.array([5.0, 5.0, 5.0, 5.0, 5.0, 4.0, -1.0])) + ) def test_fps(self): # also perturb the body but mainly want to test GJK speed start = time.time() for _ in range(1000): - a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. + a = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + b = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 collision(a, b) elapsed = time.time() - start - fps = 1000/elapsed - print('gjk fps:', fps) - self.assertTrue(fps>500) + fps = 1000 / elapsed + print("gjk fps:", fps) + self.assertTrue(fps > 500) + -if __name__ == '__main__': - unittest.main() \ No newline at end of file +if __name__ == "__main__": + unittest.main() diff --git a/gym/f110_gym/envs/f110_env_backup.py b/gym/f110_gym/envs/f110_env_backup.py index 6744d0a0..5fd37721 100644 --- a/gym/f110_gym/envs/f110_env_backup.py +++ b/gym/f110_gym/envs/f110_env_backup.py @@ -20,9 +20,9 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. -''' +""" Author: Hongrui Zheng -''' +""" # gym imports import gym @@ -53,6 +53,7 @@ # from matplotlib.pyplot import imshow # import matplotlib.pyplot as plt + class F110Env(gym.Env, utils.EzPickle): """ OpenAI gym environment for F1/10 simulator @@ -62,7 +63,8 @@ class F110Env(gym.Env, utils.EzPickle): should be initialized with a map, a timestep, and number of agents """ - metadata = {'render.modes': []} + + metadata = {"render.modes": []} def __init__(self): # simualtor params @@ -152,7 +154,7 @@ def __init__(self): self.socket = self.context.socket(zmq.PAIR) while tries < max_tries: try: - self.socket.bind('tcp://*:%s' % str(min_port + tries)) + self.socket.bind("tcp://*:%s" % str(min_port + tries)) # self.socket.connect('tcp://localhost:6666') self.port = min_port + tries break @@ -160,7 +162,7 @@ def __init__(self): tries = tries + 1 # print('Gym env - retrying for ' + str(tries) + ' times') - print('Gym env - Connected env to port: ' + str(self.port)) + print("Gym env - Connected env to port: " + str(self.port)) # create cpp instance if create then need to pass port number # subprocess call assumes directory structure @@ -190,7 +192,19 @@ def _start_executable(self, path): cs_r = self.params[4] I_z = self.params[5] mass = self.params[6] - args = [path+'sim_server', str(self.timestep), str(self.num_agents), str(self.port), str(mu), str(h_cg), str(l_r), str(cs_f), str(cs_r), str(I_z), str(mass)] + args = [ + path + "sim_server", + str(self.timestep), + str(self.num_agents), + str(self.port), + str(mu), + str(h_cg), + str(l_r), + str(cs_f), + str(cs_r), + str(I_z), + str(mass), + ] self.sim_p = subprocess.Popen(args) def _set_map(self): @@ -198,11 +212,13 @@ def _set_map(self): Sets the map for the simulator instance """ if not self.map_inited: - print('Gym env - Sim map not initialized, call env.init_map() to init map.') + print("Gym env - Sim map not initialized, call env.init_map() to init map.") # create and fill in protobuf map_request_proto = sim_requests_pb2.SimRequest() map_request_proto.type = 1 - map_request_proto.map_request.map.extend((1. - self.map_img/255.).flatten().tolist()) + map_request_proto.map_request.map.extend( + (1.0 - self.map_img / 255.0).flatten().tolist() + ) map_request_proto.map_request.origin_x = self.origin[0] map_request_proto.map_request.origin_y = self.origin[1] map_request_proto.map_request.map_resolution = self.map_resolution @@ -224,7 +240,7 @@ def _set_map(self): # get results set_map_result = sim_response_proto.map_result.result if set_map_result == 1: - print('Gym env - Set map failed, exiting...') + print("Gym env - Set map failed, exiting...") sys.exit() def _check_done(self): @@ -243,17 +259,17 @@ def _check_done(self): right_t = 2 timeout = self.current_time >= self.timeout if self.double_finish: - poses_x = np.array(self.all_x)-self.start_xs - poses_y = np.array(self.all_y)-self.start_ys + poses_x = np.array(self.all_x) - self.start_xs + poses_y = np.array(self.all_y) - self.start_ys delta_pt = np.dot(self.start_rot, np.stack((poses_x, poses_y), axis=0)) - temp_y = delta_pt[1,:] + temp_y = delta_pt[1, :] idx1 = temp_y > left_t idx2 = temp_y < -right_t temp_y[idx1] -= left_t temp_y[idx2] = -right_t - temp_y[idx2] temp_y[np.invert(np.logical_or(idx1, idx2))] = 0 - dist2 = delta_pt[0,:]**2 + temp_y**2 + dist2 = delta_pt[0, :] ** 2 + temp_y**2 closes = dist2 <= 0.1 for i in range(self.num_agents): if closes[i] and not self.near_starts[i]: @@ -262,7 +278,7 @@ def _check_done(self): elif not closes[i] and self.near_starts[i]: self.near_starts[i] = False self.toggle_list[i] += 1 - done = (self.in_collision | (timeout) | np.all(self.toggle_list >= 4)) + done = self.in_collision | (timeout) | np.all(self.toggle_list >= 4) # only for two cars atm self.lap_counts[0] = np.floor(self.toggle_list[0] / 2) self.lap_counts[1] = np.floor(self.toggle_list[1] / 2) @@ -272,14 +288,16 @@ def _check_done(self): self.lap_times[1] = self.current_time return done, self.toggle_list >= 4 - delta_pt = np.dot(self.start_rot, np.array([self.x-self.start_x, self.y-self.start_y])) - if delta_pt[1] > left_t: # left - temp_y = delta_pt[1]-left_t - elif delta_pt[1] < -right_t: # right + delta_pt = np.dot( + self.start_rot, np.array([self.x - self.start_x, self.y - self.start_y]) + ) + if delta_pt[1] > left_t: # left + temp_y = delta_pt[1] - left_t + elif delta_pt[1] < -right_t: # right temp_y = -right_t - delta_pt[1] else: temp_y = 0 - dist2 = delta_pt[0]**2 + temp_y**2 + dist2 = delta_pt[0] ** 2 + temp_y**2 close = dist2 <= 0.1 # close = dist_to_start <= self.start_thresh if close and not self.near_start: @@ -288,7 +306,7 @@ def _check_done(self): elif not close and self.near_start: self.near_start = False self.num_toggles += 1 - done = (self.in_collision | (timeout) | (self.num_toggles >= 4)) + done = self.in_collision | (timeout) | (self.num_toggles >= 4) return done def _check_passed(self): @@ -302,15 +320,15 @@ def _update_state(self, obs_dict): Update the env's states according to observations obs is observation dictionary """ - self.x = obs_dict['poses_x'][obs_dict['ego_idx']] - self.y = obs_dict['poses_y'][obs_dict['ego_idx']] + self.x = obs_dict["poses_x"][obs_dict["ego_idx"]] + self.y = obs_dict["poses_y"][obs_dict["ego_idx"]] if self.double_finish: - self.all_x = obs_dict['poses_x'] - self.all_y = obs_dict['poses_y'] + self.all_x = obs_dict["poses_x"] + self.all_y = obs_dict["poses_y"] - self.theta = obs_dict['poses_theta'][obs_dict['ego_idx']] - self.in_collision = obs_dict['collisions'][obs_dict['ego_idx']] - self.collision_angle = obs_dict['collision_angles'][obs_dict['ego_idx']] + self.theta = obs_dict["poses_theta"][obs_dict["ego_idx"]] + self.in_collision = obs_dict["collisions"][obs_dict["ego_idx"]] + self.collision_angle = obs_dict["collision_angles"][obs_dict["ego_idx"]] # TODO: do we do the ray casting here or in C++? # if speed is a concern do it in C++? @@ -327,16 +345,18 @@ def _raycast_opponents(self, obs_dict): def step(self, action): # can't step if params not set if not self.params_set: - print('ERROR - Gym Env - Params not set, call update params before stepping.') + print( + "ERROR - Gym Env - Params not set, call update params before stepping." + ) sys.exit() # action is a list of steering angles + command velocities # also a ego car index # action should a DICT with {'ego_idx': int, 'speed':[], 'steer':[]} step_request_proto = sim_requests_pb2.SimRequest() step_request_proto.type = 0 - step_request_proto.step_request.ego_idx = action['ego_idx'] - step_request_proto.step_request.requested_vel.extend(action['speed']) - step_request_proto.step_request.requested_ang.extend(action['steer']) + step_request_proto.step_request.ego_idx = action["ego_idx"] + step_request_proto.step_request.requested_vel.extend(action["speed"]) + step_request_proto.step_request.requested_ang.extend(action["steer"]) # serialization step_request_string = step_request_proto.SerializeToString() # send step request @@ -352,34 +372,47 @@ def step(self, action): response_type = sim_response_proto.type # TODO: also check for stepping fail if not response_type == 0: - print('Gym env - Wrong response type for stepping, exiting...') + print("Gym env - Wrong response type for stepping, exiting...") sys.exit() observations_proto = sim_response_proto.sim_obs # make sure the ego idx matches - if not observations_proto.ego_idx == action['ego_idx']: - print('Gym env - Ego index mismatch, exiting...') + if not observations_proto.ego_idx == action["ego_idx"]: + print("Gym env - Ego index mismatch, exiting...") sys.exit() # get observations carobs_list = observations_proto.observations # construct observation dict # Observation DICT, assume indices consistent: {'ego_idx':int, 'scans':[[]], 'poses_x':[], 'poses_y':[], 'poses_theta':[], 'linear_vels_x':[], 'linear_vels_y':[], 'ang_vels_z':[], 'collisions':[], 'collision_angles':[]} - obs = {'ego_idx': observations_proto.ego_idx, 'scans': [], 'poses_x': [], 'poses_y': [], 'poses_theta': [], 'linear_vels_x': [], 'linear_vels_y': [], 'ang_vels_z': [], 'collisions': [], 'collision_angles': [], 'lap_times': [], 'lap_counts': []} + obs = { + "ego_idx": observations_proto.ego_idx, + "scans": [], + "poses_x": [], + "poses_y": [], + "poses_theta": [], + "linear_vels_x": [], + "linear_vels_y": [], + "ang_vels_z": [], + "collisions": [], + "collision_angles": [], + "lap_times": [], + "lap_counts": [], + } for car_obs in carobs_list: - obs['scans'].append(car_obs.scan) - obs['poses_x'].append(car_obs.pose_x) - obs['poses_y'].append(car_obs.pose_y) + obs["scans"].append(car_obs.scan) + obs["poses_x"].append(car_obs.pose_x) + obs["poses_y"].append(car_obs.pose_y) if abs(car_obs.theta) < np.pi: - obs['poses_theta'].append(car_obs.theta) + obs["poses_theta"].append(car_obs.theta) else: - obs['poses_theta'].append(-((2 * np.pi) - car_obs.theta)) - obs['linear_vels_x'].append(car_obs.linear_vel_x) - obs['linear_vels_y'].append(car_obs.linear_vel_y) - obs['ang_vels_z'].append(car_obs.ang_vel_z) - obs['collisions'].append(car_obs.collision) - obs['collision_angles'].append(car_obs.collision_angle) + obs["poses_theta"].append(-((2 * np.pi) - car_obs.theta)) + obs["linear_vels_x"].append(car_obs.linear_vel_x) + obs["linear_vels_y"].append(car_obs.linear_vel_y) + obs["ang_vels_z"].append(car_obs.ang_vel_z) + obs["collisions"].append(car_obs.collision) + obs["collision_angles"].append(car_obs.collision_angle) - obs['lap_times'] = self.lap_times - obs['lap_counts'] = self.lap_counts + obs["lap_times"] = self.lap_times + obs["lap_counts"] = self.lap_counts # TODO: do we need step reward? reward = self.timestep @@ -389,7 +422,7 @@ def step(self, action): self._update_state(obs) if self.double_finish: done, temp = self._check_done() - info = {'checkpoint_done': temp} + info = {"checkpoint_done": temp} else: done = self._check_done() info = {} @@ -398,26 +431,29 @@ def step(self, action): return obs, reward, done, info def reset(self, poses=None): - self.current_time = 0.0 self.in_collision = False self.collision_angles = None self.num_toggles = 0 self.near_start = True - self.near_starts = np.array([True]*self.num_agents) + self.near_starts = np.array([True] * self.num_agents) self.toggle_list = np.zeros((self.num_agents,)) if poses: - pose_x = poses['x'] - pose_y = poses['y'] - pose_theta = poses['theta'] + pose_x = poses["x"] + pose_y = poses["y"] + pose_theta = poses["theta"] self.start_x = pose_x[0] self.start_y = pose_y[0] self.start_theta = pose_theta[0] self.start_xs = np.array(pose_x) self.start_ys = np.array(pose_y) self.start_thetas = np.array(pose_theta) - self.start_rot = np.array([[np.cos(-self.start_theta), -np.sin(-self.start_theta)], - [np.sin(-self.start_theta), np.cos(-self.start_theta)]]) + self.start_rot = np.array( + [ + [np.cos(-self.start_theta), -np.sin(-self.start_theta)], + [np.sin(-self.start_theta), np.cos(-self.start_theta)], + ] + ) # create reset by pose proto reset_request_proto = sim_requests_pb2.SimRequest() reset_request_proto.type = 4 @@ -433,8 +469,12 @@ def reset(self, poses=None): self.start_x = 0.0 self.start_y = 0.0 self.start_theta = 0.0 - self.start_rot = np.array([[np.cos(-self.start_theta), -np.sin(-self.start_theta)], - [np.sin(-self.start_theta), np.cos(-self.start_theta)]]) + self.start_rot = np.array( + [ + [np.cos(-self.start_theta), -np.sin(-self.start_theta)], + [np.sin(-self.start_theta), np.cos(-self.start_theta)], + ] + ) reset_request_proto = sim_requests_pb2.SimRequest() reset_request_proto.type = 2 reset_request_proto.reset_request.num_cars = self.num_agents @@ -448,13 +488,13 @@ def reset(self, poses=None): reset_response_proto = sim_requests_pb2.SimResponse() reset_response_proto.ParseFromString(reset_response_string) if reset_response_proto.reset_resp.result: - print('Gym env - Reset failed') + print("Gym env - Reset failed") # TODO: failure handling return None # TODO: return with gym convention, one step? vels = [0.0] * self.num_agents angs = [0.0] * self.num_agents - action = {'ego_idx': self.ego_idx, 'speed': vels, 'steer': angs} + action = {"ego_idx": self.ego_idx, "speed": vels, "steer": angs} # print('Gym env - Reset done') obs, reward, done, info = self.step(action) # print('Gym env - step done for reset') @@ -462,21 +502,23 @@ def reset(self, poses=None): def init_map(self, map_path, img_ext, rgb, flip): """ - init a map for the gym env - map_path: full path for the yaml, same as ROS, img and yaml in same dir - rgb: map grayscale or rgb - flip: if map needs flipping + init a map for the gym env + map_path: full path for the yaml, same as ROS, img and yaml in same dir + rgb: map grayscale or rgb + flip: if map needs flipping """ self.map_path = map_path - if not map_path.endswith('.yaml'): - print('Gym env - Please use a yaml file for map initialization.') - print('Exiting...') + if not map_path.endswith(".yaml"): + print("Gym env - Please use a yaml file for map initialization.") + print("Exiting...") sys.exit() # split yaml ext name map_img_path = os.path.splitext(self.map_path)[0] + img_ext - self.map_img = np.array(Image.open(map_img_path).transpose(Image.FLIP_TOP_BOTTOM)) + self.map_img = np.array( + Image.open(map_img_path).transpose(Image.FLIP_TOP_BOTTOM) + ) self.map_img = self.map_img.astype(np.float64) if flip: self.map_img = self.map_img[::-1] @@ -488,11 +530,11 @@ def init_map(self, map_path, img_ext, rgb, flip): self.map_height = self.map_img.shape[0] self.map_width = self.map_img.shape[1] self.free_thresh = 0.6 # TODO: double check - with open(self.map_path, 'r') as yaml_stream: + with open(self.map_path, "r") as yaml_stream: try: map_metadata = yaml.safe_load(yaml_stream) - self.map_resolution = map_metadata['resolution'] - self.origin = map_metadata['origin'] + self.map_resolution = map_metadata["resolution"] + self.origin = map_metadata["origin"] except yaml.YAMLError as ex: print(ex) self.map_inited = True @@ -504,8 +546,7 @@ def init_map(self, map_path, img_ext, rgb, flip): # # waypoints are [x, y, speed, theta] # self.waypoints = np.array([(float(pt[0]), float(pt[1]), float(pt[2]), float(pt[3])) for pt in self.waypoints]) - - def render(self, mode='human', close=False): + def render(self, mode="human", close=False): return # def get_min_dist(self, position): @@ -538,7 +579,9 @@ def render(self, mode='human', close=False): # min_dist_segment = np.argmin(dists) # return projections[min_dist_segment], dists[min_dist_segment], t[min_dist_segment], min_dist_segment - def update_params(self, mu, h_cg, l_r, cs_f, cs_r, I_z, mass, exe_path, double_finish=False): + def update_params( + self, mu, h_cg, l_r, cs_f, cs_r, I_z, mass, exe_path, double_finish=False + ): # if not self.sim_p is None: # print('Gym env - Sim server exists, killing...') # self.socket.send(b'dead') @@ -577,7 +620,7 @@ def update_params(self, mu, h_cg, l_r, cs_f, cs_r, I_z, mass, exe_path, double_f update_response_proto = sim_requests_pb2.SimResponse() update_response_proto.ParseFromString(update_response_string) if update_response_proto.update_resp.result: - print('Gym env - Update param failed') + print("Gym env - Update param failed") return None # print('Gym env - params updated.') diff --git a/gym/f110_gym/envs/laser_models.py b/gym/f110_gym/envs/laser_models.py index 616653c0..450a2967 100644 --- a/gym/f110_gym/envs/laser_models.py +++ b/gym/f110_gym/envs/laser_models.py @@ -21,7 +21,6 @@ # SOFTWARE. - """ Prototype of Utility functions and classes for simulating 2D LIDAR scans Author: Hongrui Zheng @@ -55,6 +54,7 @@ def get_dt(bitmap, resolution): dt = resolution * edt(bitmap) return dt + @njit(cache=True) def xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution): """ @@ -65,7 +65,7 @@ def xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution): y (float): coordinate in y (m) orig_x (float): x coordinate of the map origin (m) orig_y (float): y coordinate of the map origin (m) - + Returns: r (int): row number in the transform matrix of the given point c (int): column number in the transform matrix of the given point @@ -79,17 +79,25 @@ def xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution): y_rot = -x_trans * orig_s + y_trans * orig_c # clip the state to be a cell - if x_rot < 0 or x_rot >= width * resolution or y_rot < 0 or y_rot >= height * resolution: + if ( + x_rot < 0 + or x_rot >= width * resolution + or y_rot < 0 + or y_rot >= height * resolution + ): c = -1 r = -1 else: - c = int(x_rot/resolution) - r = int(y_rot/resolution) + c = int(x_rot / resolution) + r = int(y_rot / resolution) return r, c + @njit(cache=True) -def distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt): +def distance_transform( + x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt +): """ Look up corresponding distance in the distance matrix @@ -106,8 +114,25 @@ def distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, reso distance = dt[r, c] return distance + @njit(cache=True) -def trace_ray(x, y, theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt, max_range): +def trace_ray( + x, + y, + theta_index, + sines, + cosines, + eps, + orig_x, + orig_y, + orig_c, + orig_s, + height, + width, + resolution, + dt, + max_range, +): """ Find the length of a specific ray at a specific scan angle theta Purely math calculation and loops, should be JITted. @@ -122,14 +147,16 @@ def trace_ray(x, y, theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, or Returns: total_distance (float): the distance to first obstacle on the current scan beam """ - + # int casting, and index precal trigs theta_index_ = int(theta_index) s = sines[theta_index_] c = cosines[theta_index_] # distance to nearest initialization - dist_to_nearest = distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt) + dist_to_nearest = distance_transform( + x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt + ) total_dist = dist_to_nearest # ray tracing iterations @@ -140,16 +167,37 @@ def trace_ray(x, y, theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, or # update dist_to_nearest for current point on ray # also keeps track of total ray length - dist_to_nearest = distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt) + dist_to_nearest = distance_transform( + x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt + ) total_dist += dist_to_nearest if total_dist > max_range: total_dist = max_range - + return total_dist + @njit(cache=True) -def get_scan(pose, theta_dis, fov, num_beams, theta_index_increment, sines, cosines, eps, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt, max_range): +def get_scan( + pose, + theta_dis, + fov, + num_beams, + theta_index_increment, + sines, + cosines, + eps, + orig_x, + orig_y, + orig_c, + orig_s, + height, + width, + resolution, + dt, + max_range, +): """ Perform the scan for each discretized angle of each beam of the laser, loop heavy, should be JITted @@ -167,17 +215,33 @@ def get_scan(pose, theta_dis, fov, num_beams, theta_index_increment, sines, cosi scan = np.empty((num_beams,)) # make theta discrete by mapping the range [-pi, pi] onto [0, theta_dis] - theta_index = theta_dis * (pose[2] - fov/2.)/(2. * np.pi) + theta_index = theta_dis * (pose[2] - fov / 2.0) / (2.0 * np.pi) # make sure it's wrapped properly theta_index = np.fmod(theta_index, theta_dis) - while (theta_index < 0): + while theta_index < 0: theta_index += theta_dis # sweep through each beam for i in range(0, num_beams): # trace the current beam - scan[i] = trace_ray(pose[0], pose[1], theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt, max_range) + scan[i] = trace_ray( + pose[0], + pose[1], + theta_index, + sines, + cosines, + eps, + orig_x, + orig_y, + orig_c, + orig_s, + height, + width, + resolution, + dt, + max_range, + ) # increment the beam index theta_index += theta_index_increment @@ -188,7 +252,8 @@ def get_scan(pose, theta_dis, fov, num_beams, theta_index_increment, sines, cosi return scan -@njit(cache=True, error_model='numpy') + +@njit(cache=True, error_model="numpy") def check_ttc_jit(scan, vel, scan_angles, cosines, side_distances, ttc_thresh): """ Checks the iTTC of each beam in a scan for collision with environment @@ -209,8 +274,8 @@ def check_ttc_jit(scan, vel, scan_angles, cosines, side_distances, ttc_thresh): if vel != 0.0: num_beams = scan.shape[0] for i in range(num_beams): - proj_vel = vel*cosines[i] - ttc = (scan[i] - side_distances[i])/proj_vel + proj_vel = vel * cosines[i] + ttc = (scan[i] - side_distances[i]) / proj_vel if (ttc < ttc_thresh) and (ttc >= 0.0): in_collision = True break @@ -219,6 +284,7 @@ def check_ttc_jit(scan, vel, scan_angles, cosines, side_distances, ttc_thresh): return in_collision + @njit(cache=True) def cross(v1, v2): """ @@ -230,7 +296,8 @@ def cross(v1, v2): Returns: crossproduct (float): cross product """ - return v1[0]*v2[1]-v1[1]*v2[0] + return v1[0] * v2[1] - v1[1] * v2[0] + @njit(cache=True) def are_collinear(pt_a, pt_b, pt_c): @@ -249,6 +316,7 @@ def are_collinear(pt_a, pt_b, pt_c): col = np.fabs(cross(ba, ca)) < tol return col + @njit(cache=True) def get_range(pose, beam_theta, va, vb): """ @@ -265,7 +333,7 @@ def get_range(pose, beam_theta, va, vb): o = pose[0:2] v1 = o - va v2 = vb - va - v3 = np.array([np.cos(beam_theta + np.pi/2.), np.sin(beam_theta + np.pi/2.)]) + v3 = np.array([np.cos(beam_theta + np.pi / 2.0), np.sin(beam_theta + np.pi / 2.0)]) denom = v2.dot(v3) distance = np.inf @@ -282,6 +350,7 @@ def get_range(pose, beam_theta, va, vb): return distance + @njit(cache=True) def get_blocked_view_indices(pose, vertices, scan_angles): """ @@ -300,14 +369,16 @@ def get_blocked_view_indices(pose, vertices, scan_angles): # find angles between all four and pose vector ego_x_vec = np.array([[np.cos(pose[2])], [np.sin(pose[2])]]) - - angles_with_x = np.empty((4, )) + + angles_with_x = np.empty((4,)) for i in range(4): - angle = np.arctan2(ego_x_vec[1], ego_x_vec[0]) - np.arctan2(unit_vecs[i, 1], unit_vecs[i, 0]) + angle = np.arctan2(ego_x_vec[1], ego_x_vec[0]) - np.arctan2( + unit_vecs[i, 1], unit_vecs[i, 0] + ) if angle > np.pi: - angle = angle - 2*np.pi + angle = angle - 2 * np.pi elif angle < -np.pi: - angle = angle + 2*np.pi + angle = angle + 2 * np.pi angles_with_x[i] = -angle[0] ind1 = int(np.argmin(np.abs(scan_angles - angles_with_x[0]))) @@ -328,7 +399,7 @@ def ray_cast(pose, scan, scan_angles, vertices): scan (np.ndarray(num_beams, )): original scan to modify scan_angles (np.ndarray(num_beams, )): corresponding beam angles vertices (np.ndarray(4, 2)): four vertices of a vehicle pose - + Returns: new_scan (np.ndarray(num_beams, )): modified scan """ @@ -343,11 +414,17 @@ def ray_cast(pose, scan, scan_angles, vertices): # looping over vertices for j in range(4): # check if original scan is longer than ray casted distance - scan_range = get_range(pose, pose[2]+scan_angles[i], looped_vertices[j,:], looped_vertices[j+1,:]) + scan_range = get_range( + pose, + pose[2] + scan_angles[i], + looped_vertices[j, :], + looped_vertices[j + 1, :], + ) if scan_range < scan[i]: scan[i] = scan_range return scan + class ScanSimulator2D(object): """ 2D LIDAR scan simulator class @@ -361,14 +438,14 @@ class ScanSimulator2D(object): """ def __init__(self, num_beams, fov, eps=0.0001, theta_dis=2000, max_range=30.0): - # initialization + # initialization self.num_beams = num_beams self.fov = fov self.eps = eps self.theta_dis = theta_dis self.max_range = max_range self.angle_increment = self.fov / (self.num_beams - 1) - self.theta_index_increment = theta_dis * self.angle_increment / (2. * np.pi) + self.theta_index_increment = theta_dis * self.angle_increment / (2.0 * np.pi) self.orig_c = None self.orig_s = None self.orig_x = None @@ -377,12 +454,12 @@ def __init__(self, num_beams, fov, eps=0.0001, theta_dis=2000, max_range=30.0): self.map_width = None self.map_resolution = None self.dt = None - + # precomputing corresponding cosines and sines of the angle array - theta_arr = np.linspace(0.0, 2*np.pi, num=theta_dis) + theta_arr = np.linspace(0.0, 2 * np.pi, num=theta_dis) self.sines = np.sin(theta_arr) self.cosines = np.cos(theta_arr) - + def set_map(self, map_name: str): """ Set the bitmap of the scan simulator by path @@ -429,16 +506,34 @@ def scan(self, pose, rng, std_dev=0.01): Raises: ValueError: when scan is called before a map is set """ - + if self.map_height is None: - raise ValueError('Map is not set for scan simulator.') - - scan = get_scan(pose, self.theta_dis, self.fov, self.num_beams, self.theta_index_increment, self.sines, self.cosines, self.eps, self.orig_x, self.orig_y, self.orig_c, self.orig_s, self.map_height, self.map_width, self.map_resolution, self.dt, self.max_range) + raise ValueError("Map is not set for scan simulator.") + + scan = get_scan( + pose, + self.theta_dis, + self.fov, + self.num_beams, + self.theta_index_increment, + self.sines, + self.cosines, + self.eps, + self.orig_x, + self.orig_y, + self.orig_c, + self.orig_s, + self.map_height, + self.map_width, + self.map_resolution, + self.dt, + self.max_range, + ) if rng is not None: - noise = rng.normal(0., std_dev, size=self.num_beams) + noise = rng.normal(0.0, std_dev, size=self.num_beams) scan += noise - + return scan def get_increment(self): @@ -446,7 +541,7 @@ def get_increment(self): """ -Unit tests for the 2D scan simulator class +Unit test for the 2D scan simulator class Author: Hongrui Zheng Test cases: @@ -463,7 +558,7 @@ def setUp(self): self.num_test = 10 self.test_poses = np.zeros((self.num_test, 3)) - self.test_poses[:, 2] = np.linspace(-1., 1., num=self.num_test) + self.test_poses[:, 2] = np.linspace(-1.0, 1.0, num=self.num_test) # # legacy gym data # sample_scan = np.load('legacy_scan.npz') @@ -524,44 +619,45 @@ def test_fps(self): scan_rng = np.random.default_rng(seed=12345) scan_sim = ScanSimulator2D(self.num_beams, self.fov) - map_path = '../maps/Berlin/Berlin_map.yaml' - map_ext = '.png' + map_path = "../maps/Berlin/Berlin_map.yaml" + map_ext = ".png" scan_sim.set_map(map_path, map_ext) - + import time + start = time.time() for i in range(10000): - x_test = i/10000 - scan = scan_sim.scan(np.array([x_test, 0., 0.]), scan_rng) + x_test = i / 10000 + scan = scan_sim.scan(np.array([x_test, 0.0, 0.0]), scan_rng) end = time.time() - fps = 10000/(end-start) + fps = 10000 / (end - start) # print('FPS test') # print('Elapsed time: ' + str(end-start) + ' , FPS: ' + str(1/fps)) - self.assertGreater(fps, 500.) + self.assertGreater(fps, 500.0) def test_rng(self): num_beams = 1080 fov = 4.7 - map_path = '../maps/Berlin/Berlin_map.yaml' - map_ext = '.png' + map_path = "../maps/Berlin/Berlin_map.yaml" + map_ext = ".png" it = 100 scan_rng = np.random.default_rng(seed=12345) scan_sim = ScanSimulator2D(num_beams, fov) scan_sim.set_map(map_path, map_ext) - scan1 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng) - scan2 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng) + scan1 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng) + scan2 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng) for i in range(it): - scan3 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng) - scan4 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng) - + scan3 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng) + scan4 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng) + scan_rng = np.random.default_rng(seed=12345) - scan5 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng) - scan2 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng) + scan5 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng) + scan2 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng) for i in range(it): - _ = scan_sim.scan(np.array([0., 0., 0.]), scan_rng) - scan6 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng) - + _ = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng) + scan6 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng) + self.assertTrue(np.allclose(scan1, scan5)) self.assertFalse(np.allclose(scan1, scan2)) self.assertFalse(np.allclose(scan1, scan3)) @@ -572,49 +668,54 @@ def main(): num_beams = 1080 fov = 4.7 # map_path = '../envs/maps/Berlin_map.yaml' - map_path = '../maps/Example/Example_map.yaml' - map_ext = '.png' + map_path = "../maps/Example/Example_map.yaml" + map_ext = ".png" scan_rng = np.random.default_rng(seed=12345) scan_sim = ScanSimulator2D(num_beams, fov) scan_sim.set_map(map_path, map_ext) - scan = scan_sim.scan(np.array([0., 0., 0.]), scan_rng) + scan = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng) # fps test import time + start = time.time() for i in range(10000): - x_test = i/10000 - scan = scan_sim.scan(np.array([x_test, 0., 0.]), scan_rng) + x_test = i / 10000 + scan = scan_sim.scan(np.array([x_test, 0.0, 0.0]), scan_rng) end = time.time() - fps = (end-start)/10000 - print('FPS test') - print('Elapsed time: ' + str(end-start) + ' , FPS: ' + str(1/fps)) + fps = (end - start) / 10000 + print("FPS test") + print("Elapsed time: " + str(end - start) + " , FPS: " + str(1 / fps)) # visualization import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation + num_iter = 100 - theta = np.linspace(-fov/2., fov/2., num=num_beams) + theta = np.linspace(-fov / 2.0, fov / 2.0, num=num_beams) fig = plt.figure() - ax = fig.add_subplot(111, projection='polar') + ax = fig.add_subplot(111, projection="polar") ax.set_ylim(0, 31) - line, = ax.plot([], [], '.', lw=0) + (line,) = ax.plot([], [], ".", lw=0) + def update(i): # x_ani = i * 3. / num_iter theta_ani = -i * 2 * np.pi / num_iter - x_ani = 0. - current_scan = scan_sim.scan(np.array([x_ani, 0., theta_ani]), scan_rng) + x_ani = 0.0 + current_scan = scan_sim.scan(np.array([x_ani, 0.0, theta_ani]), scan_rng) print(np.max(current_scan)) line.set_data(theta, current_scan) - return line, + return (line,) + ani = FuncAnimation(fig, update, frames=num_iter, blit=True) plt.show() -if __name__ == '__main__': + +if __name__ == "__main__": unittest.main() - #main() + # main() - # import time + # import time # pt_a = np.array([1., 1.]) # pt_b = np.array([1., 2.]) # pt_c = np.array([1., 3.]) diff --git a/gym/f110_gym/envs/rendering.py b/gym/f110_gym/envs/rendering.py index 374c3a98..44e06e4c 100644 --- a/gym/f110_gym/envs/rendering.py +++ b/gym/f110_gym/envs/rendering.py @@ -21,7 +21,6 @@ # SOFTWARE. - """ Rendering engine for f1tenth gym env based on pyglet and OpenGL Author: Hongrui Zheng @@ -37,21 +36,24 @@ import yaml from f110_gym.envs.track import Track + # helpers from f110_gym.envs.collision_models import get_vertices # zooming constants ZOOM_IN_FACTOR = 1.2 -ZOOM_OUT_FACTOR = 1/ZOOM_IN_FACTOR +ZOOM_OUT_FACTOR = 1 / ZOOM_IN_FACTOR # vehicle shape constants CAR_LENGTH = 0.58 CAR_WIDTH = 0.31 + class EnvRenderer(pyglet.window.Window): """ A window class inherited from pyglet.window.Window, handles the camera/projection interaction, resizing window, and rendering the environment """ + def __init__(self, width, height, *args, **kwargs): """ Class constructor @@ -63,20 +65,19 @@ def __init__(self, width, height, *args, **kwargs): Returns: None """ - conf = Config(sample_buffers=1, - samples=4, - depth_size=16, - double_buffer=True) - super().__init__(width, height, config=conf, resizable=True, vsync=False, *args, **kwargs) + conf = Config(sample_buffers=1, samples=4, depth_size=16, double_buffer=True) + super().__init__( + width, height, config=conf, resizable=True, vsync=False, *args, **kwargs + ) # gl init - glClearColor(9/255, 32/255, 87/255, 1.) + glClearColor(9 / 255, 32 / 255, 87 / 255, 1.0) # initialize camera values - self.left = -width/2 - self.right = width/2 - self.bottom = -height/2 - self.top = height/2 + self.left = -width / 2 + self.right = width / 2 + self.bottom = -height / 2 + self.top = height / 2 self.zoom_level = 1.2 self.zoomed_width = width self.zoomed_height = height @@ -86,7 +87,7 @@ def __init__(self, width, height, *args, **kwargs): # current env map self.map_points = None - + # current env agent poses, (num_agents, 3), columns are (x, y, theta) self.poses = None @@ -95,17 +96,19 @@ def __init__(self, width, height, *args, **kwargs): # current score label self.score_label = pyglet.text.Label( - 'Lap Time: {laptime:.2f}, Ego Lap Count: {count:.0f}'.format( - laptime=0.0, count=0.0), - font_size=36, - x=0, - y=-800, - anchor_x='center', - anchor_y='center', - # width=0.01, - # height=0.01, - color=(255, 255, 255, 255), - batch=self.batch) + "Lap Time: {laptime:.2f}, Ego Lap Count: {count:.0f}".format( + laptime=0.0, count=0.0 + ), + font_size=36, + x=0, + y=-800, + anchor_x="center", + anchor_y="center", + # width=0.01, + # height=0.01, + color=(255, 255, 255, 255), + batch=self.batch, + ) self.fps_display = pyglet.window.FPSDisplay(self) @@ -142,9 +145,15 @@ def update_map(self, track: Track): # mask and only leave the obstacle points map_mask = map_img == 0.0 map_mask_flat = map_mask.flatten() - map_points = 50. * map_coords[:, map_mask_flat].T + map_points = 50.0 * map_coords[:, map_mask_flat].T for i in range(map_points.shape[0]): - self.batch.add(1, GL_POINTS, None, ('v3f/stream', [map_points[i, 0], map_points[i, 1], map_points[i, 2]]), ('c3B/stream', [183, 193, 222])) + self.batch.add( + 1, + GL_POINTS, + None, + ("v3f/stream", [map_points[i, 0], map_points[i, 1], map_points[i, 2]]), + ("c3B/stream", [183, 193, 222]), + ) self.map_points = map_points def on_resize(self, width, height): @@ -166,10 +175,10 @@ def on_resize(self, width, height): # update camera value (width, height) = self.get_size() - self.left = -self.zoom_level * width/2 - self.right = self.zoom_level * width/2 - self.bottom = -self.zoom_level * height/2 - self.top = self.zoom_level * height/2 + self.left = -self.zoom_level * width / 2 + self.right = self.zoom_level * width / 2 + self.bottom = -self.zoom_level * height / 2 + self.top = self.zoom_level * height / 2 self.zoomed_width = self.zoom_level * width self.zoomed_height = self.zoom_level * height @@ -213,17 +222,16 @@ def on_mouse_scroll(self, x, y, dx, dy): f = ZOOM_IN_FACTOR if dy > 0 else ZOOM_OUT_FACTOR if dy < 0 else 1 # If zoom_level is in the proper range - if .01 < self.zoom_level * f < 10: - + if 0.01 < self.zoom_level * f < 10: self.zoom_level *= f (width, height) = self.get_size() - mouse_x = x/width - mouse_y = y/height + mouse_x = x / width + mouse_y = y / height - mouse_x_in_world = self.left + mouse_x*self.zoomed_width - mouse_y_in_world = self.bottom + mouse_y*self.zoomed_height + mouse_x_in_world = self.left + mouse_x * self.zoomed_width + mouse_y_in_world = self.bottom + mouse_y * self.zoomed_height self.zoomed_width *= f self.zoomed_height *= f @@ -248,12 +256,12 @@ def on_close(self): """ super().on_close() - raise Exception('Rendering window was closed.') + raise Exception("Rendering window was closed.") def on_draw(self): """ Function when the pyglet is drawing. The function draws the batch created that includes the map points, the agent polygons, and the information text, and the fps display. - + Args: None @@ -263,9 +271,9 @@ def on_draw(self): # if map and poses doesn't exist, raise exception if self.map_points is None: - raise Exception('Map not set for renderer.') + raise Exception("Map not set for renderer.") if self.poses is None: - raise Exception('Agent poses not updated for renderer.') + raise Exception("Agent poses not updated for renderer.") # Initialize Projection matrix glMatrixMode(GL_PROJECTION) @@ -300,31 +308,54 @@ def update_obs(self, obs): None """ - self.ego_idx = obs['ego_idx'] - poses_x = obs['poses_x'] - poses_y = obs['poses_y'] - poses_theta = obs['poses_theta'] + self.ego_idx = obs["ego_idx"] + poses_x = obs["poses_x"] + poses_y = obs["poses_y"] + poses_theta = obs["poses_theta"] num_agents = len(poses_x) if self.poses is None: self.cars = [] for i in range(num_agents): if i == self.ego_idx: - vertices_np = get_vertices(np.array([0., 0., 0.]), CAR_LENGTH, CAR_WIDTH) + vertices_np = get_vertices( + np.array([0.0, 0.0, 0.0]), CAR_LENGTH, CAR_WIDTH + ) vertices = list(vertices_np.flatten()) - car = self.batch.add(4, GL_QUADS, None, ('v2f', vertices), ('c3B', [172, 97, 185, 172, 97, 185, 172, 97, 185, 172, 97, 185])) + car = self.batch.add( + 4, + GL_QUADS, + None, + ("v2f", vertices), + ( + "c3B", + [172, 97, 185, 172, 97, 185, 172, 97, 185, 172, 97, 185], + ), + ) self.cars.append(car) else: - vertices_np = get_vertices(np.array([0., 0., 0.]), CAR_LENGTH, CAR_WIDTH) + vertices_np = get_vertices( + np.array([0.0, 0.0, 0.0]), CAR_LENGTH, CAR_WIDTH + ) vertices = list(vertices_np.flatten()) - car = self.batch.add(4, GL_QUADS, None, ('v2f', vertices), ('c3B', [99, 52, 94, 99, 52, 94, 99, 52, 94, 99, 52, 94])) + car = self.batch.add( + 4, + GL_QUADS, + None, + ("v2f", vertices), + ("c3B", [99, 52, 94, 99, 52, 94, 99, 52, 94, 99, 52, 94]), + ) self.cars.append(car) poses = np.stack((poses_x, poses_y, poses_theta)).T for j in range(poses.shape[0]): - vertices_np = 50. * get_vertices(poses[j, :], CAR_LENGTH, CAR_WIDTH) + vertices_np = 50.0 * get_vertices(poses[j, :], CAR_LENGTH, CAR_WIDTH) vertices = list(vertices_np.flatten()) self.cars[j].vertices = vertices self.poses = poses - self.score_label.text = 'Lap Time: {laptime:.2f}, Ego Lap Count: {count:.0f}'.format(laptime=obs['lap_times'][0], count=obs['lap_counts'][obs['ego_idx']]) \ No newline at end of file + self.score_label.text = ( + "Lap Time: {laptime:.2f}, Ego Lap Count: {count:.0f}".format( + laptime=obs["lap_times"][0], count=obs["lap_counts"][obs["ego_idx"]] + ) + ) diff --git a/gym/f110_gym/unittest/__init__.py b/gym/f110_gym/test/__init__.py similarity index 100% rename from gym/f110_gym/unittest/__init__.py rename to gym/f110_gym/test/__init__.py diff --git a/gym/f110_gym/unittest/legacy_scan.npz b/gym/f110_gym/test/legacy_scan.npz similarity index 100% rename from gym/f110_gym/unittest/legacy_scan.npz rename to gym/f110_gym/test/legacy_scan.npz diff --git a/gym/f110_gym/unittest/legacy_scan_gen.py b/gym/f110_gym/test/legacy_scan_gen.py similarity index 58% rename from gym/f110_gym/unittest/legacy_scan_gen.py rename to gym/f110_gym/test/legacy_scan_gen.py index 20aa69db..96beed5a 100644 --- a/gym/f110_gym/unittest/legacy_scan_gen.py +++ b/gym/f110_gym/test/legacy_scan_gen.py @@ -21,12 +21,11 @@ # SOFTWARE. - """ Utility functions to generate sample scan data from legacy C++ backend Author: Hongrui Zheng -The script generates sample scan data for 3 different maps used in the unit tests. +The script generates sample scan data for 3 different maps used in the unit test. Map 1: Levine @@ -42,8 +41,8 @@ thetas = np.linspace(-2.35, 2.35, num=1080) # init -executable_dir = '../../../build/' -mass= 3.74 +executable_dir = "../../../build/" +mass = 3.74 l_r = 0.17145 I_z = 0.04712 mu = 0.523 @@ -54,49 +53,57 @@ # test poses num_test = 10 test_poses = np.zeros((num_test, 3)) -test_poses[:, 2] = np.linspace(-1., 1., num=num_test) +test_poses[:, 2] = np.linspace(-1.0, 1.0, num=num_test) # map 1: vegas -map_path = '../../../maps/Vegas_map.yaml' -map_ext = '.png' -racecar_env = gym.make('f110_gym:f110-v0') +map_path = "../../../maps/Vegas_map.yaml" +map_ext = ".png" +racecar_env = gym.make("f110_gym:f110-v0") racecar_env.init_map(map_path, map_ext, False, False) -racecar_env.update_params(mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True) +racecar_env.update_params( + mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True +) vegas_scan = np.empty((num_test, 1080)) for i in range(test_poses.shape[0]): - x = [test_poses[i, 0], 200.] - y = [test_poses[i, 1], 200.] - theta = [test_poses[i, 2], 0.] - obs,_,_,_ = racecar_env.reset({'x': x, 'y': y, 'theta': theta}) - vegas_scan[i,:] = obs['scans'][0] + x = [test_poses[i, 0], 200.0] + y = [test_poses[i, 1], 200.0] + theta = [test_poses[i, 2], 0.0] + obs, _, _, _ = racecar_env.reset({"x": x, "y": y, "theta": theta}) + vegas_scan[i, :] = obs["scans"][0] # map 2: berlin -map_path = '../../../maps/Berlin_map.yaml' -map_ext = '.png' -racecar_env = gym.make('f110_gym:f110-v0') +map_path = "../../../maps/Berlin_map.yaml" +map_ext = ".png" +racecar_env = gym.make("f110_gym:f110-v0") racecar_env.init_map(map_path, map_ext, False, False) -racecar_env.update_params(mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True) +racecar_env.update_params( + mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True +) berlin_scan = np.empty((num_test, 1080)) for i in range(test_poses.shape[0]): - x = [test_poses[i, 0], 200.] - y = [test_poses[i, 1], 200.] - theta = [test_poses[i, 2], 0.] - obs,_,_,_ = racecar_env.reset({'x': x, 'y': y, 'theta': theta}) - berlin_scan[i,:] = obs['scans'][0] + x = [test_poses[i, 0], 200.0] + y = [test_poses[i, 1], 200.0] + theta = [test_poses[i, 2], 0.0] + obs, _, _, _ = racecar_env.reset({"x": x, "y": y, "theta": theta}) + berlin_scan[i, :] = obs["scans"][0] # map 3: skirk -map_path = '../../../maps/Skirk_map.yaml' -map_ext = '.png' -racecar_env = gym.make('f110_gym:f110-v0') +map_path = "../../../maps/Skirk_map.yaml" +map_ext = ".png" +racecar_env = gym.make("f110_gym:f110-v0") racecar_env.init_map(map_path, map_ext, False, False) -racecar_env.update_params(mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True) +racecar_env.update_params( + mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True +) skirk_scan = np.empty((num_test, 1080)) for i in range(test_poses.shape[0]): - x = [test_poses[i, 0], 200.] - y = [test_poses[i, 1], 200.] - theta = [test_poses[i, 2], 0.] - obs,_,_,_ = racecar_env.reset({'x': x, 'y': y, 'theta': theta}) - skirk_scan[i,:] = obs['scans'][0] + x = [test_poses[i, 0], 200.0] + y = [test_poses[i, 1], 200.0] + theta = [test_poses[i, 2], 0.0] + obs, _, _, _ = racecar_env.reset({"x": x, "y": y, "theta": theta}) + skirk_scan[i, :] = obs["scans"][0] # package data -np.savez_compressed('legacy_scan.npz', vegas=vegas_scan, berlin=berlin_scan, skirk=skirk_scan) \ No newline at end of file +np.savez_compressed( + "legacy_scan.npz", vegas=vegas_scan, berlin=berlin_scan, skirk=skirk_scan +) diff --git a/gym/f110_gym/unittest/pyglet_test.py b/gym/f110_gym/test/pyglet_test_.py similarity index 72% rename from gym/f110_gym/unittest/pyglet_test.py rename to gym/f110_gym/test/pyglet_test_.py index 43449019..8d0d355d 100644 --- a/gym/f110_gym/unittest/pyglet_test.py +++ b/gym/f110_gym/test/pyglet_test_.py @@ -8,11 +8,16 @@ import argparse -class Camera: - """ A simple 2D camera that contains the speed and offset.""" - def __init__(self, window: pyglet.window.Window, scroll_speed=1, min_zoom=1, max_zoom=4): - assert min_zoom <= max_zoom, "Minimum zoom must not be greater than maximum zoom" +class Camera: + """A simple 2D camera that contains the speed and offset.""" + + def __init__( + self, window: pyglet.window.Window, scroll_speed=1, min_zoom=1, max_zoom=4 + ): + assert ( + min_zoom <= max_zoom + ), "Minimum zoom must not be greater than maximum zoom" self._window = window self.scroll_speed = scroll_speed self.max_zoom = max_zoom @@ -27,7 +32,7 @@ def zoom(self): @zoom.setter def zoom(self, value): - """ Here we set zoom, clamp value to minimum of min_zoom and max of max_zoom.""" + """Here we set zoom, clamp value to minimum of min_zoom and max of max_zoom.""" self._zoom = max(min(value, self.max_zoom), self.min_zoom) @property @@ -41,9 +46,9 @@ def position(self, value): self.offset_x, self.offset_y = value def move(self, axis_x, axis_y): - """ Move axis direction with scroll_speed. - Example: Move left -> move(-1, 0) - """ + """Move axis direction with scroll_speed. + Example: Move left -> move(-1, 0) + """ self.offset_x += self.scroll_speed * axis_x self.offset_y += self.scroll_speed * axis_y @@ -51,7 +56,9 @@ def begin(self): # Set the current camera offset so you can draw your scene. # Translate using the offset. - view_matrix = self._window.view.translate(-self.offset_x * self._zoom, -self.offset_y * self._zoom, 0) + view_matrix = self._window.view.translate( + -self.offset_x * self._zoom, -self.offset_y * self._zoom, 0 + ) # Scale by zoom level. view_matrix = view_matrix.scale(self._zoom, self._zoom, 1) @@ -64,7 +71,9 @@ def end(self): # Reverse scale, since that was the last transform. view_matrix = self._window.view.scale(1 / self._zoom, 1 / self._zoom, 1) # Reverse translate. - view_matrix = view_matrix.translate(self.offset_x * self._zoom, self.offset_y * self._zoom, 0) + view_matrix = view_matrix.translate( + self.offset_x * self._zoom, self.offset_y * self._zoom, 0 + ) self._window.view = view_matrix @@ -95,25 +104,30 @@ def end(self): self._window.view = view_matrix - parser = argparse.ArgumentParser() -parser.add_argument('--map_path', type=str, required=True, help='Path to the map without extensions') -parser.add_argument('--map_ext', type=str, required=True, help='Extension of the map image file') +parser.add_argument( + "--map_path", type=str, required=True, help="Path to the map without extensions" +) +parser.add_argument( + "--map_ext", type=str, required=True, help="Extension of the map image file" +) args = parser.parse_args() # load map yaml -with open(args.map_path + '.yaml', 'r') as yaml_stream: +with open(args.map_path + ".yaml", "r") as yaml_stream: try: map_metada = yaml.safe_load(yaml_stream) - map_resolution = map_metada['resolution'] - origin = map_metada['origin'] + map_resolution = map_metada["resolution"] + origin = map_metada["origin"] origin_x = origin[0] origin_y = origin[1] except yaml.YAMLError as ex: print(ex) # load map image -map_img = np.array(Image.open(args.map_path + args.map_ext).transpose(Image.FLIP_TOP_BOTTOM)).astype(np.float64) +map_img = np.array( + Image.open(args.map_path + args.map_ext).transpose(Image.FLIP_TOP_BOTTOM) +).astype(np.float64) map_height = map_img.shape[0] map_width = map_img.shape[1] @@ -134,47 +148,61 @@ def end(self): # prep opengl try: # Try and create a window with multisampling (antialiasing) - config = Config(sample_buffers=1, samples=4, - depth_size=16, double_buffer=True, ) + config = Config( + sample_buffers=1, + samples=4, + depth_size=16, + double_buffer=True, + ) window = window.Window(resizable=True, config=config) except window.NoSuchConfigException: # Fall back to no multisampling for old hardware window = window.Window(resizable=True) -glClearColor(18/255, 4/255, 88/255, 1.) +glClearColor(18 / 255, 4 / 255, 88 / 255, 1.0) glEnable(GL_DEPTH_TEST) glTranslatef(25, -5, -60) cam = Camera(window) + @window.event def on_resize(width, height): # Override the default on_resize handler to create a 3D projection glViewport(0, 0, width, height) glMatrixMode(GL_PROJECTION) glLoadIdentity() - gluPerspective(60., width / float(height), .1, 1000.) + gluPerspective(60.0, width / float(height), 0.1, 1000.0) glMatrixMode(GL_MODELVIEW) return pyglet.event.EVENT_HANDLED + batch = graphics.Batch() points = [] for i in range(map_points.shape[0]): - particle = batch.add(1, GL_POINTS, None, ('v3f/stream', [map_points[i, 0], map_points[i, 1], map_points[i, 2]])) + particle = batch.add( + 1, + GL_POINTS, + None, + ("v3f/stream", [map_points[i, 0], map_points[i, 1], map_points[i, 2]]), + ) points.append(particle) + def loop(dt): print(pyglet.clock.get_fps()) pass + @window.event def on_draw(): glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) - glColor3f(254/255, 117/255, 254/255) + glColor3f(254 / 255, 117 / 255, 254 / 255) cam.begin() batch.draw() cam.end() + pyglet.clock.schedule(loop) -pyglet.app.run() \ No newline at end of file +pyglet.app.run() diff --git a/gym/f110_gym/test/pyglet_test_camera.py b/gym/f110_gym/test/pyglet_test_camera.py new file mode 100644 index 00000000..6f5f973c --- /dev/null +++ b/gym/f110_gym/test/pyglet_test_camera.py @@ -0,0 +1,127 @@ +import pyglet +from pyglet.gl import * + +# Zooming constants +ZOOM_IN_FACTOR = 1.2 +ZOOM_OUT_FACTOR = 1 / ZOOM_IN_FACTOR + + +class App(pyglet.window.Window): + def __init__(self, width, height, *args, **kwargs): + conf = Config(sample_buffers=1, samples=4, depth_size=16, double_buffer=True) + super().__init__(width, height, config=conf, *args, **kwargs) + + # Initialize camera values + self.left = 0 + self.right = width + self.bottom = 0 + self.top = height + self.zoom_level = 1 + self.zoomed_width = width + self.zoomed_height = height + + def init_gl(self, width, height): + # Set clear color + glClearColor(0 / 255, 0 / 255, 0 / 255, 0 / 255) + + # Set antialiasing + glEnable(GL_LINE_SMOOTH) + glEnable(GL_POLYGON_SMOOTH) + glHint(GL_LINE_SMOOTH_HINT, GL_NICEST) + + # Set alpha blending + glEnable(GL_BLEND) + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) + + # Set viewport + glViewport(0, 0, width, height) + + def on_resize(self, width, height): + super().on_resize(width, height) + size = self.get_size() + self.left = 0 + self.right = size[0] + self.bottom = 0 + self.top = size[1] + self.zoomed_width = size[0] + self.zoomed_height = size[1] + + # # Set window values + # self.width = width + # self.height = height + # # Initialize OpenGL context + # self.init_gl(width, height) + # self.width = width + # self.height = height + # pass + + def on_mouse_drag(self, x, y, dx, dy, buttons, modifiers): + # Move camera + self.left -= dx * self.zoom_level + self.right -= dx * self.zoom_level + self.bottom -= dy * self.zoom_level + self.top -= dy * self.zoom_level + + def on_mouse_scroll(self, x, y, dx, dy): + # Get scale factor + f = ZOOM_IN_FACTOR if dy > 0 else ZOOM_OUT_FACTOR if dy < 0 else 1 + # If zoom_level is in the proper range + if 0.2 < self.zoom_level * f < 5: + self.zoom_level *= f + + size = self.get_size() + + mouse_x = x / size[0] + mouse_y = y / size[1] + + mouse_x_in_world = self.left + mouse_x * self.zoomed_width + mouse_y_in_world = self.bottom + mouse_y * self.zoomed_height + + self.zoomed_width *= f + self.zoomed_height *= f + + self.left = mouse_x_in_world - mouse_x * self.zoomed_width + self.right = mouse_x_in_world + (1 - mouse_x) * self.zoomed_width + self.bottom = mouse_y_in_world - mouse_y * self.zoomed_height + self.top = mouse_y_in_world + (1 - mouse_y) * self.zoomed_height + + def on_draw(self): + # Initialize Projection matrix + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + + # Initialize Modelview matrix + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + # Save the default modelview matrix + glPushMatrix() + + # Clear window with ClearColor + glClear(GL_COLOR_BUFFER_BIT) + + # Set orthographic projection matrix + glOrtho(self.left, self.right, self.bottom, self.top, 1, -1) + + # Draw quad + glBegin(GL_QUADS) + glColor3ub(0xFF, 0, 0) + glVertex2i(10, 10) + + glColor3ub(0xFF, 0xFF, 0) + glVertex2i(110, 10) + + glColor3ub(0, 0xFF, 0) + glVertex2i(110, 110) + + glColor3ub(0, 0, 0xFF) + glVertex2i(10, 110) + glEnd() + + # Remove default modelview matrix + glPopMatrix() + + def run(self): + pyglet.app.run() + + +App(800, 800, resizable=True).run() diff --git a/gym/f110_gym/unittest/collision_checks.py b/gym/f110_gym/test/test_collision_checks.py similarity index 82% rename from gym/f110_gym/unittest/collision_checks.py rename to gym/f110_gym/test/test_collision_checks.py index c5c86f0e..14d9cb9a 100644 --- a/gym/f110_gym/unittest/collision_checks.py +++ b/gym/f110_gym/test/test_collision_checks.py @@ -21,7 +21,6 @@ # SOFTWARE. - """ Prototype of Utility functions and GJK algorithm for Collision checks between vehicles Originally from https://github.com/kroitor/gjk.c @@ -31,6 +30,7 @@ import numpy as np from numba import njit + @njit(cache=True) def perpendicular(pt): """ @@ -44,7 +44,7 @@ def perpendicular(pt): """ temp = pt[0] pt[0] = pt[1] - pt[1] = -1*temp + pt[1] = -1 * temp return pt @@ -61,7 +61,7 @@ def tripleProduct(a, b, c): """ ac = a.dot(c) bc = b.dot(c) - return b*ac - a*bc + return b * ac - a * bc @njit(cache=True) @@ -75,7 +75,7 @@ def avgPoint(vertices): Returns: avg (np.ndarray, (2,)): average point of the vertices """ - return np.sum(vertices, axis=0)/vertices.shape[0] + return np.sum(vertices, axis=0) / vertices.shape[0] @njit(cache=True) @@ -153,7 +153,7 @@ def collision(vertices1, vertices2): if index < 2: b = simplex[0, :] - ab = b-a + ab = b - a d = tripleProduct(ab, ao, ab) if np.linalg.norm(d) < 1e-10: d = perpendicular(ab) @@ -161,8 +161,8 @@ def collision(vertices1, vertices2): b = simplex[1, :] c = simplex[0, :] - ab = b-a - ac = c-a + ab = b - a + ac = c - a acperp = tripleProduct(ab, ac, ac) @@ -181,10 +181,12 @@ def collision(vertices1, vertices2): iter_count += 1 return False + """ Utility functions for getting vertices by pose and shape """ + @njit(cache=True) def get_trmtx(pose): """ @@ -201,9 +203,17 @@ def get_trmtx(pose): th = pose[2] cos = np.cos(th) sin = np.sin(th) - H = np.array([[cos, -sin, 0., x], [sin, cos, 0., y], [0., 0., 1., 0.], [0., 0., 0., 1.]]) + H = np.array( + [ + [cos, -sin, 0.0, x], + [sin, cos, 0.0, y], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) return H + @njit(cache=True) def get_vertices(pose, length, width): """ @@ -218,48 +228,52 @@ def get_vertices(pose, length, width): vertices (np.ndarray, (4, 2)): corner vertices of the vehicle body """ H = get_trmtx(pose) - rl = H.dot(np.asarray([[-length/2],[width/2],[0.], [1.]])).flatten() - rr = H.dot(np.asarray([[-length/2],[-width/2],[0.], [1.]])).flatten() - fl = H.dot(np.asarray([[length/2],[width/2],[0.], [1.]])).flatten() - fr = H.dot(np.asarray([[length/2],[-width/2],[0.], [1.]])).flatten() - rl = rl/rl[3] - rr = rr/rr[3] - fl = fl/fl[3] - fr = fr/fr[3] - vertices = np.asarray([[rl[0], rl[1]], [rr[0], rr[1]], [fr[0], fr[1]], [fl[0], fl[1]]]) + rl = H.dot(np.asarray([[-length / 2], [width / 2], [0.0], [1.0]])).flatten() + rr = H.dot(np.asarray([[-length / 2], [-width / 2], [0.0], [1.0]])).flatten() + fl = H.dot(np.asarray([[length / 2], [width / 2], [0.0], [1.0]])).flatten() + fr = H.dot(np.asarray([[length / 2], [-width / 2], [0.0], [1.0]])).flatten() + rl = rl / rl[3] + rr = rr / rr[3] + fl = fl / fl[3] + fr = fr / fr[3] + vertices = np.asarray( + [[rl[0], rl[1]], [rr[0], rr[1]], [fr[0], fr[1]], [fl[0], fl[1]]] + ) return vertices """ -Unit tests for GJK collision checks +Unit test for GJK collision checks Author: Hongrui Zheng """ import time import unittest + class CollisionTests(unittest.TestCase): def setUp(self): # test params np.random.seed(1234) # Collision check body - self.vertices1 = np.asarray([[4,11.],[5,5],[9,9],[10,10]]) + self.vertices1 = np.asarray([[4, 11.0], [5, 5], [9, 9], [10, 10]]) # car size self.length = 0.32 self.width = 0.22 - + def test_get_vert(self): test_pose = np.array([2.3, 6.7, 0.8]) vertices = get_vertices(test_pose, self.length, self.width) - rect = np.vstack((vertices, vertices[0,:])) + rect = np.vstack((vertices, vertices[0, :])) import matplotlib.pyplot as plt - plt.scatter(test_pose[0], test_pose[1], c='red') + + plt.scatter(test_pose[0], test_pose[1], c="red") plt.plot(rect[:, 0], rect[:, 1]) plt.xlim([1, 4]) plt.ylim([5, 8]) - plt.axes().set_aspect('equal') + plt.axes().set_aspect("equal") plt.show() self.assertTrue(vertices.shape == (4, 2)) @@ -269,28 +283,29 @@ def test_get_vert_fps(self): for _ in range(1000): vertices = get_vertices(test_pose, self.length, self.width) elapsed = time.time() - start - fps = 1000/elapsed - print('get vertices fps:', fps) - self.assertTrue(fps>500) + fps = 1000 / elapsed + print("get vertices fps:", fps) + self.assertTrue(fps > 500) def test_random_collision(self): # perturb the body by a small amount and make sure it all collides with the original body for _ in range(1000): - a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - self.assertTrue(collision(a,b)) + a = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + b = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + self.assertTrue(collision(a, b)) def test_fps(self): # also perturb the body but mainly want to test GJK speed start = time.time() for _ in range(1000): - a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. - b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. + a = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 + b = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0 collision(a, b) elapsed = time.time() - start - fps = 1000/elapsed - print('gjk fps:', fps) - self.assertTrue(fps>500) + fps = 1000 / elapsed + print("gjk fps:", fps) + self.assertTrue(fps > 500) + -if __name__ == '__main__': - unittest.main() \ No newline at end of file +if __name__ == "__main__": + unittest.main() diff --git a/gym/f110_gym/unittest/dynamics_test.py b/gym/f110_gym/test/test_dynamics.py similarity index 96% rename from gym/f110_gym/unittest/dynamics_test.py rename to gym/f110_gym/test/test_dynamics.py index e2ffa72f..7bce8f43 100644 --- a/gym/f110_gym/unittest/dynamics_test.py +++ b/gym/f110_gym/test/test_dynamics.py @@ -28,19 +28,94 @@ from f110_gym.envs import vehicle_dynamics_ks, vehicle_dynamics_st -def func_KS(x, t, u, mu, C_Sf, C_Sr, lf, lr, h, m, I, s_min, s_max, sv_min, sv_max, v_switch, a_max, v_min, - v_max): - f = vehicle_dynamics_ks(x, u, mu, C_Sf, C_Sr, lf, lr, h, m, I, s_min, s_max, sv_min, sv_max, v_switch, - a_max, v_min, v_max) +def func_KS( + x, + t, + u, + mu, + C_Sf, + C_Sr, + lf, + lr, + h, + m, + I, + s_min, + s_max, + sv_min, + sv_max, + v_switch, + a_max, + v_min, + v_max, +): + f = vehicle_dynamics_ks( + x, + u, + mu, + C_Sf, + C_Sr, + lf, + lr, + h, + m, + I, + s_min, + s_max, + sv_min, + sv_max, + v_switch, + a_max, + v_min, + v_max, + ) return f -def func_ST(x, t, u, mu, C_Sf, C_Sr, lf, lr, h, m, I, s_min, s_max, sv_min, sv_max, v_switch, a_max, v_min, - v_max): - f = vehicle_dynamics_st(x, u, mu, C_Sf, C_Sr, lf, lr, h, m, I, s_min, s_max, sv_min, sv_max, v_switch, - a_max, v_min, v_max) +def func_ST( + x, + t, + u, + mu, + C_Sf, + C_Sr, + lf, + lr, + h, + m, + I, + s_min, + s_max, + sv_min, + sv_max, + v_switch, + a_max, + v_min, + v_max, +): + f = vehicle_dynamics_st( + x, + u, + mu, + C_Sf, + C_Sr, + lf, + lr, + h, + m, + I, + s_min, + s_max, + sv_min, + sv_max, + v_switch, + a_max, + v_min, + v_max, + ) return f + class DynamicsTest(unittest.TestCase): def setUp(self): # test params diff --git a/gym/f110_gym/unittest/f110_env_test.py b/gym/f110_gym/test/test_f110_env.py similarity index 100% rename from gym/f110_gym/unittest/f110_env_test.py rename to gym/f110_gym/test/test_f110_env.py diff --git a/gym/f110_gym/unittest/observation_test.py b/gym/f110_gym/test/test_observation.py similarity index 100% rename from gym/f110_gym/unittest/observation_test.py rename to gym/f110_gym/test/test_observation.py diff --git a/gym/f110_gym/unittest/scan_sim.py b/gym/f110_gym/test/test_scan_sim.py similarity index 65% rename from gym/f110_gym/unittest/scan_sim.py rename to gym/f110_gym/test/test_scan_sim.py index bb1ca344..41d8b49c 100644 --- a/gym/f110_gym/unittest/scan_sim.py +++ b/gym/f110_gym/test/test_scan_sim.py @@ -36,6 +36,8 @@ import unittest import timeit +from f110_gym.envs import ScanSimulator2D + def get_dt(bitmap, resolution): """ @@ -248,152 +250,13 @@ def get_scan( return scan -class ScanSimulator2D(object): - """ - 2D LIDAR scan simulator class - - Init params: - num_beams (int): number of beams in the scan - fov (float): field of view of the laser scan - std_dev (float, default=0.01): standard deviation of the generated whitenoise in the scan - eps (float, default=0.0001): ray tracing iteration termination condition - theta_dis (int, default=2000): number of steps to discretize the angles between 0 and 2pi for look up - max_range (float, default=30.0): maximum range of the laser - seed (int, default=123): seed for random number generator for the whitenoise in scan - """ - - def __init__( - self, - num_beams, - fov, - std_dev=0.01, - eps=0.0001, - theta_dis=2000, - max_range=30.0, - seed=123, - ): - # initialization - self.num_beams = num_beams - self.fov = fov - self.std_dev = std_dev - self.eps = eps - self.theta_dis = theta_dis - self.max_range = max_range - self.angle_increment = self.fov / (self.num_beams - 1) - self.theta_index_increment = theta_dis * self.angle_increment / (2.0 * np.pi) - self.orig_c = None - self.orig_s = None - self.orig_x = None - self.orig_y = None - self.map_height = None - self.map_width = None - self.map_resolution = None - self.dt = None - - # white noise generator - self.rng = np.random.default_rng(seed=seed) - - # precomputing corresponding cosines and sines of the angle array - theta_arr = np.linspace(0.0, 2 * np.pi, num=theta_dis) - self.sines = np.sin(theta_arr) - self.cosines = np.cos(theta_arr) - - def set_map(self, map_path, map_ext): - """ - Set the bitmap of the scan simulator by path - - Args: - map_path (str): path to the map yaml file - map_ext (str): extension (image type) of the map image - - Returns: - flag (bool): if image reading and loading is successful - """ - # TODO: do we open the option to flip the images, and turn rgb into grayscale? or specify the exact requirements in documentation. - # TODO: throw error if image specification isn't met - - # load map image - map_img_path = os.path.splitext(map_path)[0] + map_ext - self.map_img = np.array( - Image.open(map_img_path).transpose(Image.FLIP_TOP_BOTTOM) - ) - self.map_img = self.map_img.astype(np.float64) - - # grayscale -> binary - self.map_img[self.map_img <= 128.0] = 0.0 - self.map_img[self.map_img > 128.0] = 255.0 - - self.map_height = self.map_img.shape[0] - self.map_width = self.map_img.shape[1] - - # load map yaml - with open(map_path, "r") as yaml_stream: - try: - map_metadata = yaml.safe_load(yaml_stream) - self.map_resolution = map_metadata["resolution"] - self.origin = map_metadata["origin"] - except yaml.YAMLError as ex: - print(ex) - - # calculate map parameters - self.orig_x = self.origin[0] - self.orig_y = self.origin[1] - self.orig_s = np.sin(self.origin[2]) - self.orig_c = np.cos(self.origin[2]) - - # get the distance transform - self.dt = get_dt(self.map_img, self.map_resolution) - - return True - - def scan(self, pose): - """ - Perform simulated 2D scan by pose on the given map - - Args: - pose (numpy.ndarray (3, )): pose of the scan frame (x, y, theta) - - Returns: - scan (numpy.ndarray (n, )): data array of the laserscan, n=num_beams - - Raises: - ValueError: when scan is called before a map is set - """ - if self.map_height is None: - raise ValueError("Map is not set for scan simulator.") - scan = get_scan( - pose, - self.theta_dis, - self.fov, - self.num_beams, - self.theta_index_increment, - self.sines, - self.cosines, - self.eps, - self.orig_x, - self.orig_y, - self.orig_c, - self.orig_s, - self.map_height, - self.map_width, - self.map_resolution, - self.dt, - self.max_range, - ) - noise = self.rng.normal(0.0, self.std_dev, size=self.num_beams) - final_scan = scan + noise - return final_scan - - def get_increment(self): - return self.angle_increment - - """ -Unit tests for the 2D scan simulator class +Unit test for the 2D scan simulator class Author: Hongrui Zheng Test cases: - 1, 2: Comparison between generated scan array of the new simulator and the legacy C++ simulator, generated data used, MSE is used as the metric + 1, 2: Comparison between generated scan array of the new simulator and the legacy C++ simulator, + generated data used, MSE is used as the metric 2. FPS test, should be greater than 500 """ @@ -415,18 +278,14 @@ def setUp(self): self.skirk_scan = sample_scan["skirk"] def test_map_berlin(self): + scan_rng = np.random.default_rng(seed=12345) scan_sim = ScanSimulator2D(self.num_beams, self.fov) new_berlin = np.empty((self.num_test, self.num_beams)) - map_path = ( - os.path.dirname(os.path.abspath(__file__)) - + "/../maps/Berlin/Berlin_map.yaml" - ) - map_ext = ".png" - scan_sim.set_map(map_path, map_ext) + scan_sim.set_map(map_name="Berlin") # scan gen loop for i in range(self.num_test): test_pose = self.test_poses[i] - new_berlin[i, :] = scan_sim.scan(test_pose) + new_berlin[i, :] = scan_sim.scan(pose=test_pose, rng=scan_rng) diff = self.berlin_scan - new_berlin mse = np.mean(diff**2) # print('Levine distance test, norm: ' + str(norm)) @@ -442,18 +301,15 @@ def test_map_berlin(self): self.assertLess(mse, 2.0) def test_map_skirk(self): + scan_rng = np.random.default_rng(seed=12345) scan_sim = ScanSimulator2D(self.num_beams, self.fov) new_skirk = np.empty((self.num_test, self.num_beams)) - map_path = ( - os.path.dirname(os.path.abspath(__file__)) + "/../maps/Skirk/Skirk_map.yaml" - ) - map_ext = ".png" - scan_sim.set_map(map_path, map_ext) + scan_sim.set_map(map_name="Skirk") print("map set") # scan gen loop for i in range(self.num_test): test_pose = self.test_poses[i] - new_skirk[i, :] = scan_sim.scan(test_pose) + new_skirk[i, :] = scan_sim.scan(pose=test_pose, rng=scan_rng) diff = self.skirk_scan - new_skirk mse = np.mean(diff**2) print("skirk distance test, mse: " + str(mse)) @@ -470,23 +326,19 @@ def test_map_skirk(self): def test_fps(self): # scan fps should be greater than 500 + scan_rng = np.random.default_rng(seed=12345) scan_sim = ScanSimulator2D(self.num_beams, self.fov) - map_path = ( - os.path.dirname(os.path.abspath(__file__)) + "/../maps/Skirk/Skirk_map.yaml" - ) - map_ext = ".png" - scan_sim.set_map(map_path, map_ext) + scan_sim.set_map(map_name="Skirk") import time start = time.time() for i in range(10000): x_test = i / 10000 - scan = scan_sim.scan(np.array([x_test, 0.0, 0.0])) + scan = scan_sim.scan(pose=np.array([x_test, 0.0, 0.0]), rng=scan_rng) end = time.time() fps = 10000 / (end - start) - # print('FPS test') - # print('Elapsed time: ' + str(end-start) + ' , FPS: ' + str(1/fps)) + self.assertGreater(fps, 500.0) @@ -494,10 +346,9 @@ def main(): num_beams = 1080 fov = 4.7 # map_path = '../envs/maps/Berlin_map.yaml' - map_path = "/home/f1tenth-eval/tunercar/es/maps/map0.yaml" - map_ext = ".png" + scan_rng = np.random.default_rng(seed=12345) scan_sim = ScanSimulator2D(num_beams, fov) - scan_sim.set_map(map_path, map_ext) + scan_sim.set_map(map_name="Example") scan = scan_sim.scan(np.array([0.0, 0.0, 0.0])) # fps test @@ -506,7 +357,7 @@ def main(): start = time.time() for i in range(10000): x_test = i / 10000 - scan = scan_sim.scan(np.array([x_test, 0.0, 0.0])) + scan = scan_sim.scan(np.array([x_test, 0.0, 0.0]), rng=scan_rng) end = time.time() fps = (end - start) / 10000 print("FPS test") @@ -537,5 +388,5 @@ def update(i): if __name__ == "__main__": - # unittest.main() + # test.main() main() diff --git a/gym/f110_gym/unittest/track_test.py b/gym/f110_gym/test/test_track.py similarity index 89% rename from gym/f110_gym/unittest/track_test.py rename to gym/f110_gym/test/test_track.py index 2ed23a50..06c66d58 100644 --- a/gym/f110_gym/unittest/track_test.py +++ b/gym/f110_gym/test/test_track.py @@ -112,22 +112,36 @@ def test_download_racetrack(self): track = Track.from_track_name(track_name) # check the two tracks' specs are the same - for spec_attr in ["name", "image", "resolution", "origin", "negate", "occupied_thresh", "free_thresh"]: + for spec_attr in [ + "name", + "image", + "resolution", + "origin", + "negate", + "occupied_thresh", + "free_thresh", + ]: self.assertEqual( getattr(track.spec, spec_attr), getattr(track_backup.spec, spec_attr) ) # check the two tracks' racelines are the same for raceline_attr in ["ss", "xs", "ys", "yaws", "ks", "vxs", "axs"]: - self.assertTrue(np.isclose( - getattr(track.raceline, raceline_attr), getattr(track_backup.raceline, raceline_attr) - ).all()) + self.assertTrue( + np.isclose( + getattr(track.raceline, raceline_attr), + getattr(track_backup.raceline, raceline_attr), + ).all() + ) # check the two tracks' centerlines are the same for centerline_attr in ["ss", "xs", "ys", "yaws", "ks", "vxs", "axs"]: - self.assertTrue(np.isclose( - getattr(track.centerline, centerline_attr), getattr(track_backup.centerline, centerline_attr) - ).all()) + self.assertTrue( + np.isclose( + getattr(track.centerline, centerline_attr), + getattr(track_backup.centerline, centerline_attr), + ).all() + ) # remove the newly created track dir track_dir = find_track_dir(track_name) @@ -136,4 +150,3 @@ def test_download_racetrack(self): # rename the backup track dir to its original name track_backup_dir = find_track_dir(tmp_dir.stem) track_backup_dir.rename(track_dir) - diff --git a/gym/f110_gym/unittest/utils_test.py b/gym/f110_gym/test/test_utils.py similarity index 100% rename from gym/f110_gym/unittest/utils_test.py rename to gym/f110_gym/test/test_utils.py diff --git a/gym/f110_gym/unittest/pyglet_test_camera.py b/gym/f110_gym/unittest/pyglet_test_camera.py deleted file mode 100644 index 46ba26a3..00000000 --- a/gym/f110_gym/unittest/pyglet_test_camera.py +++ /dev/null @@ -1,131 +0,0 @@ -import pyglet -from pyglet.gl import * - -# Zooming constants -ZOOM_IN_FACTOR = 1.2 -ZOOM_OUT_FACTOR = 1/ZOOM_IN_FACTOR - -class App(pyglet.window.Window): - - def __init__(self, width, height, *args, **kwargs): - conf = Config(sample_buffers=1, - samples=4, - depth_size=16, - double_buffer=True) - super().__init__(width, height, config=conf, *args, **kwargs) - - #Initialize camera values - self.left = 0 - self.right = width - self.bottom = 0 - self.top = height - self.zoom_level = 1 - self.zoomed_width = width - self.zoomed_height = height - - def init_gl(self, width, height): - # Set clear color - glClearColor(0/255, 0/255, 0/255, 0/255) - - # Set antialiasing - glEnable( GL_LINE_SMOOTH ) - glEnable( GL_POLYGON_SMOOTH ) - glHint( GL_LINE_SMOOTH_HINT, GL_NICEST ) - - # Set alpha blending - glEnable( GL_BLEND ) - glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA ) - - # Set viewport - glViewport( 0, 0, width, height ) - - def on_resize(self, width, height): - super().on_resize(width, height) - size = self.get_size() - self.left = 0 - self.right = size[0] - self.bottom = 0 - self.top = size[1] - self.zoomed_width = size[0] - self.zoomed_height = size[1] - - # # Set window values - # self.width = width - # self.height = height - # # Initialize OpenGL context - # self.init_gl(width, height) - # self.width = width - # self.height = height - # pass - - def on_mouse_drag(self, x, y, dx, dy, buttons, modifiers): - # Move camera - self.left -= dx*self.zoom_level - self.right -= dx*self.zoom_level - self.bottom -= dy*self.zoom_level - self.top -= dy*self.zoom_level - - def on_mouse_scroll(self, x, y, dx, dy): - # Get scale factor - f = ZOOM_IN_FACTOR if dy > 0 else ZOOM_OUT_FACTOR if dy < 0 else 1 - # If zoom_level is in the proper range - if .2 < self.zoom_level*f < 5: - - self.zoom_level *= f - - size = self.get_size() - - mouse_x = x/size[0] - mouse_y = y/size[1] - - mouse_x_in_world = self.left + mouse_x*self.zoomed_width - mouse_y_in_world = self.bottom + mouse_y*self.zoomed_height - - self.zoomed_width *= f - self.zoomed_height *= f - - self.left = mouse_x_in_world - mouse_x*self.zoomed_width - self.right = mouse_x_in_world + (1 - mouse_x)*self.zoomed_width - self.bottom = mouse_y_in_world - mouse_y*self.zoomed_height - self.top = mouse_y_in_world + (1 - mouse_y)*self.zoomed_height - - def on_draw(self): - # Initialize Projection matrix - glMatrixMode( GL_PROJECTION ) - glLoadIdentity() - - # Initialize Modelview matrix - glMatrixMode( GL_MODELVIEW ) - glLoadIdentity() - # Save the default modelview matrix - glPushMatrix() - - # Clear window with ClearColor - glClear( GL_COLOR_BUFFER_BIT ) - - # Set orthographic projection matrix - glOrtho( self.left, self.right, self.bottom, self.top, 1, -1 ) - - # Draw quad - glBegin( GL_QUADS ) - glColor3ub( 0xFF, 0, 0 ) - glVertex2i( 10, 10 ) - - glColor3ub( 0xFF, 0xFF, 0 ) - glVertex2i( 110, 10 ) - - glColor3ub( 0, 0xFF, 0 ) - glVertex2i( 110, 110 ) - - glColor3ub( 0, 0, 0xFF ) - glVertex2i( 10, 110 ) - glEnd() - - # Remove default modelview matrix - glPopMatrix() - - def run(self): - pyglet.app.run() - - -App(800, 800, resizable=True).run() \ No newline at end of file diff --git a/setup.py b/setup.py index a87457ea..85b33204 100644 --- a/setup.py +++ b/setup.py @@ -1,21 +1,22 @@ from setuptools import setup -setup(name='f110_gym', - version='0.2.1', - author='Hongrui Zheng', - author_email='billyzheng.bz@gmail.com', - url='https://f1tenth.org', - package_dir={'': 'gym'}, - install_requires=[ - 'gymnasium', - 'numpy<=1.22.0,>=1.18.0', - 'Pillow>=9.0.1', - 'scipy>=1.7.3', - 'numba>=0.55.2', - 'pyyaml>=5.3.1', - 'pyglet<1.5', - 'pyopengl', - 'yamldataclassconfig', - 'requests' - ] - ) +setup( + name="f110_gym", + version="0.2.1", + author="Hongrui Zheng", + author_email="billyzheng.bz@gmail.com", + url="https://f1tenth.org", + package_dir={"": "gym"}, + install_requires=[ + "gymnasium", + "numpy<=1.22.0,>=1.18.0", + "Pillow>=9.0.1", + "scipy>=1.7.3", + "numba>=0.55.2", + "pyyaml>=5.3.1", + "pyglet<1.5", + "pyopengl", + "yamldataclassconfig", + "requests", + ], +)