def update_obs(self, obs):
"""
Updates the renderer with the latest observation from the gym environment, including the agent poses, and the information text.
Args:
obs (dict): observation dict from the gym env
Returns:
None
"""
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 = 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
]))
self.cars.append(car)
else:
vertices_np = get_vertices(np.array([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]))
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 = 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']])
def check_collision(self):
"""
Checks for collision between agents using GJK and agents' body vertices
Args:
None
Returns:
None
"""
# get vertices of all agents
all_vertices = np.empty((self.num_agents, 4, 2))
for i in range(self.num_agents):
all_vertices[i, :, :] = get_vertices(np.append(self.agents[i].state[0:2],self.agents[i].state[4]), self.params['length'], self.params['width'])
self.collisions, self.collision_idx = collision_multiple(all_vertices)
def 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
"""
# starting from original scan
new_scan = scan
# loop over all opponent vehicle poses
for opp_pose in self.opp_poses:
# get vertices of current oppoenent
opp_vertices = get_vertices(opp_pose, self.params['length'], self.params['width'])
new_scan = ray_cast(np.append(self.state[0:2], self.state[4]), new_scan, self.scan_angles, opp_vertices)
return new_scan