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