(self, input_data, timestamp)
| 168 | return result |
| 169 | @torch.no_grad() |
| 170 | def run_step(self, input_data, timestamp): |
| 171 | if not self.initialized: |
| 172 | self._init() |
| 173 | tick_data = self.tick(input_data) |
| 174 | if self.step < self.config.seq_len: |
| 175 | rgb = self._im_transform(tick_data['rgb']).unsqueeze(0) |
| 176 | |
| 177 | control = carla.VehicleControl() |
| 178 | control.steer = 0.0 |
| 179 | control.throttle = 0.0 |
| 180 | control.brake = 0.0 |
| 181 | |
| 182 | return control |
| 183 | |
| 184 | gt_velocity = torch.FloatTensor([tick_data['speed']]).to('cuda', dtype=torch.float32) |
| 185 | command = tick_data['next_command'] |
| 186 | if command < 0: |
| 187 | command = 4 |
| 188 | command -= 1 |
| 189 | assert command in [0, 1, 2, 3, 4, 5] |
| 190 | cmd_one_hot = [0] * 6 |
| 191 | cmd_one_hot[command] = 1 |
| 192 | cmd_one_hot = torch.tensor(cmd_one_hot).view(1, 6).to('cuda', dtype=torch.float32) |
| 193 | speed = torch.FloatTensor([float(tick_data['speed'])]).view(1,1).to('cuda', dtype=torch.float32) |
| 194 | speed = speed / 12 |
| 195 | rgb = self._im_transform(tick_data['rgb']).unsqueeze(0).to('cuda', dtype=torch.float32) |
| 196 | |
| 197 | tick_data['target_point'] = [torch.FloatTensor([tick_data['target_point'][0]]), |
| 198 | torch.FloatTensor([tick_data['target_point'][1]])] |
| 199 | target_point = torch.stack(tick_data['target_point'], dim=1).to('cuda', dtype=torch.float32) |
| 200 | state = torch.cat([speed, target_point, cmd_one_hot], 1) |
| 201 | |
| 202 | pred= self.net(rgb, state, target_point) |
| 203 | |
| 204 | steer_ctrl, throttle_ctrl, brake_ctrl, metadata = self.net.process_action(pred, tick_data['next_command'], gt_velocity, target_point) |
| 205 | |
| 206 | steer_traj, throttle_traj, brake_traj, metadata_traj = self.net.control_pid(pred['pred_wp'], gt_velocity, target_point) |
| 207 | if brake_traj < 0.05: brake_traj = 0.0 |
| 208 | if throttle_traj > brake_traj: brake_traj = 0.0 |
| 209 | |
| 210 | self.pid_metadata = metadata_traj |
| 211 | control = carla.VehicleControl() |
| 212 | |
| 213 | if self.status == 0: |
| 214 | self.alpha = 0.3 |
| 215 | self.pid_metadata['agent'] = 'traj' |
| 216 | control.steer = np.clip(self.alpha*steer_ctrl + (1-self.alpha)*steer_traj, -1, 1) |
| 217 | control.throttle = np.clip(self.alpha*throttle_ctrl + (1-self.alpha)*throttle_traj, 0, 0.75) |
| 218 | control.brake = np.clip(self.alpha*brake_ctrl + (1-self.alpha)*brake_traj, 0, 1) |
| 219 | else: |
| 220 | self.alpha = 0.3 |
| 221 | self.pid_metadata['agent'] = 'ctrl' |
| 222 | control.steer = np.clip(self.alpha*steer_traj + (1-self.alpha)*steer_ctrl, -1, 1) |
| 223 | control.throttle = np.clip(self.alpha*throttle_traj + (1-self.alpha)*throttle_ctrl, 0, 0.75) |
| 224 | control.brake = np.clip(self.alpha*brake_traj + (1-self.alpha)*brake_ctrl, 0, 1) |
| 225 | |
| 226 | |
| 227 | self.pid_metadata['steer_ctrl'] = float(steer_ctrl) |
no test coverage detected