diff --git a/gym/f110_gym/envs/dynamic_models.py b/gym/f110_gym/envs/dynamic_models.py index 3012e3e7..bc0618e0 100644 --- a/gym/f110_gym/envs/dynamic_models.py +++ b/gym/f110_gym/envs/dynamic_models.py @@ -423,3 +423,7 @@ def func_KS(x, t, u, mu, C_Sf, C_Sr, lf, lr, h, m, I, s_min, s_max, sv_min, sv_m 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 + + +if __name__ == '__main__': + unittest.main() \ No newline at end of file diff --git a/tests/test_collision_checks.py b/tests/test_collision_checks.py index 14d9cb9a..3d7b3cdd 100644 --- a/tests/test_collision_checks.py +++ b/tests/test_collision_checks.py @@ -20,7 +20,6 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. - """ Prototype of Utility functions and GJK algorithm for Collision checks between vehicles Originally from https://github.com/kroitor/gjk.c @@ -203,6 +202,7 @@ def get_trmtx(pose): th = pose[2] cos = np.cos(th) sin = np.sin(th) + H = np.array( [ [cos, -sin, 0.0, x], @@ -236,6 +236,7 @@ def get_vertices(pose, length, width): 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]]] ) diff --git a/tests/test_scan_sim.py b/tests/test_scan_sim.py index 41d8b49c..393b96d8 100644 --- a/tests/test_scan_sim.py +++ b/tests/test_scan_sim.py @@ -20,7 +20,6 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. - """ Prototype of Utility functions and classes for simulating 2D LIDAR scans Author: Hongrui Zheng @@ -250,6 +249,134 @@ 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 test for the 2D scan simulator class Author: Hongrui Zheng @@ -260,7 +387,6 @@ def get_scan( 2. FPS test, should be greater than 500 """ - class ScanTests(unittest.TestCase): def setUp(self): # test params @@ -389,4 +515,4 @@ def update(i): if __name__ == "__main__": # test.main() - main() + main() \ No newline at end of file