Laser Scan Simulator Models
This file contains all numba just-in-time compiled function for the 2D laser scan models. The core of the laser scan simulation is the Euclidean distance transform of the map image provided. See more details about the algorithm here: https://docs.scipy.org/doc/scipy/reference/generated/scipy.ndimage.distance_transform_edt.html#scipy.ndimage.distance_transform_edt. Then, ray tracing is used to create the beams of the 2D lasers scan.
- module laser_models
Functions
- get_dt(bitmap, resolution)
Distance transformation, returns the distance matrix from the input bitmap. Uses scipy.ndimage, cannot be JITted. Args: bitmap (numpy.ndarray, (n, m)): input binary bitmap of the environment, where 0 is obstacles, and 255 (or anything > 0) is freespace resolution (float): resolution of the input bitmap (m/cell) Returns: dt (numpy.ndarray, (n, m)): output distance matrix, where each cell has the corresponding distance (in meters) to the closest obstacle
- xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution)
Translate (x, y) coordinate into (r, c) in the matrix Args: x (float): coordinate in x (m) 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
- distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt)
Look up corresponding distance in the distance matrix Args: x (float): x coordinate of the lookup point y (float): y coordinate of the lookup point orig_x (float): x coordinate of the map origin (m) orig_y (float): y coordinate of the map origin (m) Returns: distance (float): corresponding shortest distance to obstacle in meters
- 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. Args: x (float): current x coordinate of the ego (scan) frame y (float): current y coordinate of the ego (scan) frame theta_index(int): current index of the scan beam in the scan range sines (numpy.ndarray (n, )): pre-calculated sines of the angle array cosines (numpy.ndarray (n, )): pre-calculated cosines ... Returns: total_distance (float): the distance to first obstacle on the current scan beam
- 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 Args: pose (numpy.ndarray(3, )): current pose of the scan frame in the map theta_dis (int): number of steps to discretize the angles between 0 and 2pi for look up fov (float): field of view of the laser scan num_beams (int): number of beams in the scan theta_index_increment (float): increment between angle indices after discretization Returns: scan (numpy.ndarray(n, )): resulting laser scan at the pose, n=num_beams
- 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 Args: scan (np.ndarray(num_beams, )): current scan to check vel (float): current velocity scan_angles (np.ndarray(num_beams, )): precomped angles of each beam cosines (np.ndarray(num_beams, )): precomped cosines of the scan angles side_distances (np.ndarray(num_beams, )): precomped distances at each beam from the laser to the sides of the car ttc_thresh (float): threshold for iTTC for collision Returns: in_collision (bool): whether vehicle is in collision with environment collision_angle (float): at which angle the collision happened
- cross(v1, v2)
Cross product of two 2-vectors Args: v1, v2 (np.ndarray(2, )): input vectors Returns: crossproduct (float): cross product
- are_collinear(pt_a, pt_b, pt_c)
Checks if three points are collinear in 2D Args: pt_a, pt_b, pt_c (np.ndarray(2, )): points to check in 2D Returns: col (bool): whether three points are collinear
- get_range(pose, beam_theta, va, vb)
Get the distance at a beam angle to the vector formed by two of the four vertices of a vehicle Args: pose (np.ndarray(3, )): pose of the scanning vehicle beam_theta (float): angle of the current beam (world frame) va, vb (np.ndarray(2, )): the two vertices forming an edge Returns: distance (float): smallest distance at beam theta from scanning pose to edge
- ray_cast(pose, scan, scan_angles, vertices)
Modify a scan by ray casting onto another agent's four vertices Args: pose (np.ndarray(3, )): pose of the vehicle performing scan 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
- main()
- ScanSimulator2D : public 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
Public Functions
- __init__(self, num_beams, fov, std_dev=0.01, eps=0.0001, theta_dis=2000, max_range=30.0, seed=123)
- 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
- 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
- get_increment(self)
- ScanTests : public TestCase