class TransFuserAgent(autonomous_agent.AutonomousAgent): def setup(self, path_to_conf_file): self.track = autonomous_agent.Track.SENSORS self.config_path = path_to_conf_file self.step = -1 self.wall_start = time.time() self.initialized = False self.input_buffer = { 'rgb': deque(), 'rgb_left': deque(), 'rgb_right': deque(), 'rgb_rear': deque(), 'lidar': deque(), 'gps': deque(), 'thetas': deque() } self.config = GlobalConfig() self.net = TransFuser(self.config, 'cuda') self.net.load_state_dict( torch.load(os.path.join(path_to_conf_file, 'best_model.pth'))) self.net.cuda() self.net.eval() self.save_path = None if SAVE_PATH is not None: now = datetime.datetime.now() string = pathlib.Path(os.environ['ROUTES']).stem + '_' string += '_'.join( map(lambda x: '%02d' % x, (now.month, now.day, now.hour, now.minute, now.second))) print(string) self.save_path = pathlib.Path(os.environ['SAVE_PATH']) / string self.save_path.mkdir(parents=True, exist_ok=False) (self.save_path / 'rgb').mkdir(parents=True, exist_ok=False) (self.save_path / 'meta').mkdir(parents=True, exist_ok=False) def _init(self): self._route_planner = RoutePlanner(4.0, 50.0) self._route_planner.set_route(self._global_plan, True) self.initialized = True def _get_position(self, tick_data): gps = tick_data['gps'] gps = (gps - self._route_planner.mean) * self._route_planner.scale return gps def sensors(self): return [{ 'type': 'sensor.camera.rgb', 'x': 1.3, 'y': 0.0, 'z': 2.3, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, 'width': 400, 'height': 300, 'fov': 100, 'id': 'rgb' }, { 'type': 'sensor.camera.rgb', 'x': 1.3, 'y': 0.0, 'z': 2.3, 'roll': 0.0, 'pitch': 0.0, 'yaw': -60.0, 'width': 400, 'height': 300, 'fov': 100, 'id': 'rgb_left' }, { 'type': 'sensor.camera.rgb', 'x': 1.3, 'y': 0.0, 'z': 2.3, 'roll': 0.0, 'pitch': 0.0, 'yaw': 60.0, 'width': 400, 'height': 300, 'fov': 100, 'id': 'rgb_right' }, { 'type': 'sensor.camera.rgb', 'x': -1.3, 'y': 0.0, 'z': 2.3, 'roll': 0.0, 'pitch': 0.0, 'yaw': -180.0, 'width': 400, 'height': 300, 'fov': 100, 'id': 'rgb_rear' }, { 'type': 'sensor.lidar.ray_cast', 'x': 1.3, 'y': 0.0, 'z': 2.5, 'roll': 0.0, 'pitch': 0.0, 'yaw': -90.0, 'id': 'lidar' }, { 'type': 'sensor.other.imu', 'x': 0.0, 'y': 0.0, 'z': 0.0, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, 'sensor_tick': 0.05, 'id': 'imu' }, { 'type': 'sensor.other.gnss', 'x': 0.0, 'y': 0.0, 'z': 0.0, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, 'sensor_tick': 0.01, 'id': 'gps' }, { 'type': 'sensor.speedometer', 'reading_frequency': 20, 'id': 'speed' }] def tick(self, input_data): self.step += 1 rgb = cv2.cvtColor(input_data['rgb'][1][:, :, :3], cv2.COLOR_BGR2RGB) rgb_left = cv2.cvtColor(input_data['rgb_left'][1][:, :, :3], cv2.COLOR_BGR2RGB) rgb_right = cv2.cvtColor(input_data['rgb_right'][1][:, :, :3], cv2.COLOR_BGR2RGB) rgb_rear = cv2.cvtColor(input_data['rgb_rear'][1][:, :, :3], cv2.COLOR_BGR2RGB) gps = input_data['gps'][1][:2] speed = input_data['speed'][1]['speed'] compass = input_data['imu'][1][-1] lidar = input_data['lidar'][1][:, :3] result = { 'rgb': rgb, 'rgb_left': rgb_left, 'rgb_right': rgb_right, 'rgb_rear': rgb_rear, 'lidar': lidar, 'gps': gps, 'speed': speed, 'compass': compass, } pos = self._get_position(result) result['gps'] = pos next_wp, next_cmd = self._route_planner.run_step(pos) result['next_command'] = next_cmd.value theta = compass + np.pi / 2 R = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) local_command_point = np.array( [next_wp[0] - pos[0], next_wp[1] - pos[1]]) local_command_point = R.T.dot(local_command_point) result['target_point'] = tuple(local_command_point) return result @torch.no_grad() def run_step(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) if self.step < self.config.seq_len: rgb = torch.from_numpy( scale_and_crop_image( Image.fromarray(tick_data['rgb']), crop=self.config.input_resolution)).unsqueeze(0) self.input_buffer['rgb'].append(rgb.to('cuda', dtype=torch.float32)) if not self.config.ignore_sides: rgb_left = torch.from_numpy( scale_and_crop_image( Image.fromarray(tick_data['rgb_left']), crop=self.config.input_resolution)).unsqueeze(0) self.input_buffer['rgb_left'].append( rgb_left.to('cuda', dtype=torch.float32)) rgb_right = torch.from_numpy( scale_and_crop_image( Image.fromarray(tick_data['rgb_right']), crop=self.config.input_resolution)).unsqueeze(0) self.input_buffer['rgb_right'].append( rgb_right.to('cuda', dtype=torch.float32)) if not self.config.ignore_rear: rgb_rear = torch.from_numpy( scale_and_crop_image( Image.fromarray(tick_data['rgb_rear']), crop=self.config.input_resolution)).unsqueeze(0) self.input_buffer['rgb_rear'].append( rgb_rear.to('cuda', dtype=torch.float32)) self.input_buffer['lidar'].append(tick_data['lidar']) self.input_buffer['gps'].append(tick_data['gps']) self.input_buffer['thetas'].append(tick_data['compass']) control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 0.0 return control gt_velocity = torch.FloatTensor([tick_data['speed'] ]).to('cuda', dtype=torch.float32) command = torch.FloatTensor([tick_data['next_command'] ]).to('cuda', dtype=torch.float32) tick_data['target_point'] = [ torch.FloatTensor([tick_data['target_point'][0]]), torch.FloatTensor([tick_data['target_point'][1]]) ] target_point = torch.stack(tick_data['target_point'], dim=1).to('cuda', dtype=torch.float32) encoding = [] rgb = torch.from_numpy( scale_and_crop_image( Image.fromarray(tick_data['rgb']), crop=self.config.input_resolution)).unsqueeze(0) self.input_buffer['rgb'].popleft() self.input_buffer['rgb'].append(rgb.to('cuda', dtype=torch.float32)) if not self.config.ignore_sides: rgb_left = torch.from_numpy( scale_and_crop_image( Image.fromarray(tick_data['rgb_left']), crop=self.config.input_resolution)).unsqueeze(0) self.input_buffer['rgb_left'].popleft() self.input_buffer['rgb_left'].append( rgb_left.to('cuda', dtype=torch.float32)) rgb_right = torch.from_numpy( scale_and_crop_image( Image.fromarray(tick_data['rgb_right']), crop=self.config.input_resolution)).unsqueeze(0) self.input_buffer['rgb_right'].popleft() self.input_buffer['rgb_right'].append( rgb_right.to('cuda', dtype=torch.float32)) if not self.config.ignore_rear: rgb_rear = torch.from_numpy( scale_and_crop_image( Image.fromarray(tick_data['rgb_rear']), crop=self.config.input_resolution)).unsqueeze(0) self.input_buffer['rgb_rear'].popleft() self.input_buffer['rgb_rear'].append( rgb_rear.to('cuda', dtype=torch.float32)) self.input_buffer['lidar'].popleft() self.input_buffer['lidar'].append(tick_data['lidar']) self.input_buffer['gps'].popleft() self.input_buffer['gps'].append(tick_data['gps']) self.input_buffer['thetas'].popleft() self.input_buffer['thetas'].append(tick_data['compass']) lidar_processed = list() # transform the lidar point clouds to local coordinate frame ego_theta = self.input_buffer['thetas'][-1] ego_x, ego_y = self.input_buffer['gps'][-1] for i, lidar_point_cloud in enumerate(self.input_buffer['lidar']): curr_theta = self.input_buffer['thetas'][i] curr_x, curr_y = self.input_buffer['gps'][i] lidar_point_cloud[:, 1] *= -1 # inverts x, y lidar_transformed = transform_2d_points(lidar_point_cloud, np.pi / 2 - curr_theta, -curr_x, -curr_y, np.pi / 2 - ego_theta, -ego_x, -ego_y) lidar_transformed = torch.from_numpy( lidar_to_histogram_features( lidar_transformed, crop=self.config.input_resolution)).unsqueeze(0) lidar_processed.append( lidar_transformed.to('cuda', dtype=torch.float32)) pred_wp = self.net(self.input_buffer['rgb'] + self.input_buffer['rgb_left'] + \ self.input_buffer['rgb_right']+self.input_buffer['rgb_rear'], \ lidar_processed, target_point, gt_velocity) steer, throttle, brake, metadata = self.net.control_pid( pred_wp, gt_velocity) self.pid_metadata = metadata if brake < 0.05: brake = 0.0 if throttle > brake: brake = 0.0 control = carla.VehicleControl() control.steer = float(steer) control.throttle = float(throttle) control.brake = float(brake) if SAVE_PATH is not None and self.step % 10 == 0: self.save(tick_data) return control def save(self, tick_data): frame = self.step // 10 Image.fromarray(tick_data['rgb']).save(self.save_path / 'rgb' / ('%04d.png' % frame)) outfile = open(self.save_path / 'meta' / ('%04d.json' % frame), 'w') json.dump(self.pid_metadata, outfile, indent=4) outfile.close() def destroy(self): del self.net
class CILRSAgent(autonomous_agent.AutonomousAgent): def setup(self, path_to_conf_file): self.track = autonomous_agent.Track.SENSORS self.config_path = path_to_conf_file self.step = -1 self.wall_start = time.time() self.initialized = False self.input_buffer = { 'rgb': deque(), 'rgb_left': deque(), 'rgb_right': deque(), 'rgb_rear': deque() } self.config = GlobalConfig() self.net = CILRS(self.config, 'cuda') self.net.load_state_dict( torch.load(os.path.join(path_to_conf_file, 'best_model.pth'))) self.net.cuda() self.net.eval() self.save_path = None if SAVE_PATH is not None: now = datetime.datetime.now() string = pathlib.Path(os.environ['ROUTES']).stem + '_' string += '_'.join( map(lambda x: '%02d' % x, (now.month, now.day, now.hour, now.minute, now.second))) print(string) self.save_path = pathlib.Path(os.environ['SAVE_PATH']) / string self.save_path.mkdir(parents=True, exist_ok=False) (self.save_path / 'rgb').mkdir() def _init(self): self._route_planner = RoutePlanner(4.0, 50.0) self._route_planner.set_route(self._global_plan, True) self.initialized = True def _get_position(self, tick_data): gps = tick_data['gps'] gps = (gps - self._route_planner.mean) * self._route_planner.scale return gps def sensors(self): return [{ 'type': 'sensor.camera.rgb', 'x': 1.3, 'y': 0.0, 'z': 2.3, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, 'width': 400, 'height': 300, 'fov': 100, 'id': 'rgb' }, { 'type': 'sensor.camera.rgb', 'x': 1.3, 'y': 0.0, 'z': 2.3, 'roll': 0.0, 'pitch': 0.0, 'yaw': -60.0, 'width': 400, 'height': 300, 'fov': 100, 'id': 'rgb_left' }, { 'type': 'sensor.camera.rgb', 'x': 1.3, 'y': 0.0, 'z': 2.3, 'roll': 0.0, 'pitch': 0.0, 'yaw': 60.0, 'width': 400, 'height': 300, 'fov': 100, 'id': 'rgb_right' }, { 'type': 'sensor.camera.rgb', 'x': -1.3, 'y': 0.0, 'z': 2.3, 'roll': 0.0, 'pitch': 0.0, 'yaw': -180.0, 'width': 400, 'height': 300, 'fov': 100, 'id': 'rgb_rear' }, { 'type': 'sensor.other.imu', 'x': 0.0, 'y': 0.0, 'z': 0.0, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, 'sensor_tick': 0.05, 'id': 'imu' }, { 'type': 'sensor.other.gnss', 'x': 0.0, 'y': 0.0, 'z': 0.0, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, 'sensor_tick': 0.01, 'id': 'gps' }, { 'type': 'sensor.speedometer', 'reading_frequency': 20, 'id': 'speed' }] def tick(self, input_data): self.step += 1 rgb = cv2.cvtColor(input_data['rgb'][1][:, :, :3], cv2.COLOR_BGR2RGB) rgb_left = cv2.cvtColor(input_data['rgb_left'][1][:, :, :3], cv2.COLOR_BGR2RGB) rgb_right = cv2.cvtColor(input_data['rgb_right'][1][:, :, :3], cv2.COLOR_BGR2RGB) rgb_rear = cv2.cvtColor(input_data['rgb_rear'][1][:, :, :3], cv2.COLOR_BGR2RGB) gps = input_data['gps'][1][:2] speed = input_data['speed'][1]['speed'] compass = input_data['imu'][1][-1] result = { 'rgb': rgb, 'rgb_left': rgb_left, 'rgb_right': rgb_right, 'rgb_rear': rgb_rear, 'gps': gps, 'speed': speed, 'compass': compass, } pos = self._get_position(result) next_wp, next_cmd = self._route_planner.run_step(pos) result['next_command'] = next_cmd.value return result @torch.no_grad() def run_step(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) if self.step < self.config.seq_len: rgb = torch.from_numpy( scale_and_crop_image(Image.fromarray( tick_data['rgb']))).unsqueeze(0) self.input_buffer['rgb'].append(rgb.to('cuda', dtype=torch.float32)) if not self.config.ignore_sides: rgb_left = torch.from_numpy( scale_and_crop_image(Image.fromarray( tick_data['rgb_left']))).unsqueeze(0) self.input_buffer['rgb_left'].append( rgb_left.to('cuda', dtype=torch.float32)) rgb_right = torch.from_numpy( scale_and_crop_image( Image.fromarray(tick_data['rgb_right']))).unsqueeze(0) self.input_buffer['rgb_right'].append( rgb_right.to('cuda', dtype=torch.float32)) if not self.config.ignore_rear: rgb_rear = torch.from_numpy( scale_and_crop_image(Image.fromarray( tick_data['rgb_rear']))).unsqueeze(0) self.input_buffer['rgb_rear'].append( rgb_rear.to('cuda', dtype=torch.float32)) control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 0.0 return control gt_velocity = torch.FloatTensor([tick_data['speed'] ]).to('cuda', dtype=torch.float32) command = torch.FloatTensor([tick_data['next_command'] ]).to('cuda', dtype=torch.float32) encoding = [] rgb = torch.from_numpy( scale_and_crop_image(Image.fromarray( tick_data['rgb']))).unsqueeze(0) self.input_buffer['rgb'].popleft() self.input_buffer['rgb'].append(rgb.to('cuda', dtype=torch.float32)) encoding.append(self.net.encoder(list(self.input_buffer['rgb']))) if not self.config.ignore_sides: rgb_left = torch.from_numpy( scale_and_crop_image(Image.fromarray( tick_data['rgb_left']))).unsqueeze(0) self.input_buffer['rgb_left'].popleft() self.input_buffer['rgb_left'].append( rgb_left.to('cuda', dtype=torch.float32)) encoding.append( self.net.encoder(list(self.input_buffer['rgb_left']))) rgb_right = torch.from_numpy( scale_and_crop_image(Image.fromarray( tick_data['rgb_right']))).unsqueeze(0) self.input_buffer['rgb_right'].popleft() self.input_buffer['rgb_right'].append( rgb_right.to('cuda', dtype=torch.float32)) encoding.append( self.net.encoder(list(self.input_buffer['rgb_right']))) if not self.config.ignore_rear: rgb_rear = torch.from_numpy( scale_and_crop_image(Image.fromarray( tick_data['rgb_rear']))).unsqueeze(0) self.input_buffer['rgb_rear'].popleft() self.input_buffer['rgb_rear'].append( rgb_rear.to('cuda', dtype=torch.float32)) encoding.append( self.net.encoder(list(self.input_buffer['rgb_rear']))) steer, throttle, brake, velocity = self.net(encoding, gt_velocity, command) steer = steer.squeeze(0).item() throttle = throttle.squeeze(0).item() brake = brake.squeeze(0).item() if brake < 0.05: brake = 0.0 if throttle > brake: brake = 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = brake if SAVE_PATH is not None and self.step % 10 == 0: self.save(tick_data) return control def save(self, tick_data): frame = self.step // 10 Image.fromarray(tick_data['rgb']).save(self.save_path / 'rgb' / ('%04d.png' % frame)) def destroy(self): del self.net