Base Classes
Base classes are the underlying classes that handles the physical simulation and interaction between vehicles.
The RaceCar class handles the simulation of the dynamics model and the generation of the laser scan.
The Simulator class handles the interaction between agents including collision checking.
- module base_classes
- RaceCar : public object
Base level race car class, handles the physics and laser scan of a single vehicle Data Members: params (dict): vehicle parameters dictionary is_ego (bool): ego identifier time_step (float): physics timestep num_beams (int): number of beams in laser fov (float): field of view of laser state (np.ndarray (7, )): state vector [x, y, theta, vel, steer_angle, ang_vel, slip_angle] odom (np.ndarray(13, )): odometry vector [x, y, z, qx, qy, qz, qw, linear_x, linear_y, linear_z, angular_x, angular_y, angular_z] accel (float): current acceleration input steer_angle_vel (float): current steering velocity input in_collision (bool): collision indicator
Public Functions
- __init__(self, params, is_ego=False, time_step=0.01, num_beams=1080, fov=4.7)
Init function Args: params (dict): vehicle parameter dictionary, includes {'mu', 'C_Sf', 'C_Sr', 'lf', 'lr', 'h', 'm', 'I', 's_min', 's_max', 'sv_min', 'sv_max', 'v_switch', 'a_max': 9.51, 'v_min', 'v_max', 'length', 'width'} is_ego (bool, default=False): ego identifier time_step (float, default=0.01): physics sim time step num_beams (int, default=1080): number of beams in the laser scan fov (float, default=4.7): field of view of the laser Returns: None
- update_params(self, params)
Updates the physical parameters of the vehicle Note that does not need to be called at initialization of class anymore Args: params (dict): new parameters for the vehicle Returns: None
- set_map(self, map_path, map_ext)
Sets the map for scan simulator Args: map_path (str): absolute path to the map yaml file map_ext (str): extension of the map image file
- reset(self, pose)
Resets the vehicle to a pose Args: pose (np.ndarray (3, )): pose to reset the vehicle to Returns: None
- ray_cast_agents(self, scan)
Ray cast onto other agents in the env, modify original scan Args: scan (np.ndarray, (n, )): original scan range array Returns: new_scan (np.ndarray, (n, )): modified scan
- check_ttc(self)
Check iTTC against the environment, sets vehicle states accordingly if collision occurs. Note that this does NOT check collision with other agents. state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle] Args: None Returns: None
- update_pose(self, raw_steer, vel)
Steps the vehicle's physical simulation Args: steer (float): desired steering angle vel (float): desired longitudinal velocity Returns: None
- update_opp_poses(self, opp_poses)
Updates the vehicle's information on other vehicles Args: opp_poses (np.ndarray(num_other_agents, 3)): updated poses of other agents Returns: None
- update_scan(self)
Steps the vehicle's laser scan simulation Separated from update_pose because needs to update scan based on NEW poses of agents in the environment Args: None Returns: None
- Simulator : public object
Simulator class, handles the interaction and update of all vehicles in the environment Data Members: num_agents (int): number of agents in the environment time_step (float): physics time step agent_poses (np.ndarray(num_agents, 3)): all poses of all agents agents (list[RaceCar]): container for RaceCar objects collisions (np.ndarray(num_agents, )): array of collision indicator for each agent collision_idx (np.ndarray(num_agents, )): which agent is each agent in collision with
Public Functions
- __init__(self, params, num_agents, time_step=0.01, ego_idx=0)
Init function Args: params (dict): vehicle parameter dictionary, includes {'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', 'length', 'width'} num_agents (int): number of agents in the environment time_step (float, default=0.01): physics time step ego_idx (int, default=0): ego vehicle's index in list of agents Returns: None
- set_map(self, map_path, map_ext)
Sets the map of the environment and sets the map for scan simulator of each agent Args: map_path (str): path to the map yaml file map_ext (str): extension for the map image file Returns: None
- update_params(self, params, agent_idx=-1)
Updates the params of agents, if an index of an agent is given, update only that agent's params Args: params (dict): dictionary of params, see details in docstring of __init__ agent_idx (int, default=-1): index for agent that needs param update, if negative, update all agents Returns: None
- check_collision(self)
Checks for collision between agents using GJK and agents' body vertices Args: None Returns: None
- step(self, control_inputs)
Steps the simulation environment Args: control_inputs (np.ndarray (num_agents, 2)): control inputs of all agents, first column is desired steering angle, second column is desired velocity Returns: observations (dict): dictionary for observations: poses of agents, current laser scan of each agent, collision indicators, etc.
- reset(self, poses)
Resets the simulation environment by given poses Arges: poses (np.ndarray (num_agents, 3)): poses to reset agents to Returns: None