Predicts vehicle control with a PID controller. Args: waypoints (tensor): output of self.plan() velocity (tensor): speedometer input
(self, waypoints, velocity, target)
| 248 | return x |
| 249 | |
| 250 | def control_pid(self, waypoints, velocity, target): |
| 251 | ''' Predicts vehicle control with a PID controller. |
| 252 | Args: |
| 253 | waypoints (tensor): output of self.plan() |
| 254 | velocity (tensor): speedometer input |
| 255 | ''' |
| 256 | assert(waypoints.size(0)==1) |
| 257 | waypoints = waypoints[0].data.cpu().numpy() |
| 258 | target = target.squeeze().data.cpu().numpy() |
| 259 | |
| 260 | # flip y (forward is negative in our waypoints) |
| 261 | waypoints[:,1] *= -1 |
| 262 | target[1] *= -1 |
| 263 | |
| 264 | # iterate over vectors between predicted waypoints |
| 265 | num_pairs = len(waypoints) - 1 |
| 266 | best_norm = 1e5 |
| 267 | desired_speed = 0 |
| 268 | aim = waypoints[0] |
| 269 | for i in range(num_pairs): |
| 270 | # magnitude of vectors, used for speed |
| 271 | desired_speed += np.linalg.norm( |
| 272 | waypoints[i+1] - waypoints[i]) * 2.0 / num_pairs |
| 273 | |
| 274 | # norm of vector midpoints, used for steering |
| 275 | norm = np.linalg.norm((waypoints[i+1] + waypoints[i]) / 2.0) |
| 276 | if abs(self.config.aim_dist-best_norm) > abs(self.config.aim_dist-norm): |
| 277 | aim = waypoints[i] |
| 278 | best_norm = norm |
| 279 | |
| 280 | aim_last = waypoints[-1] - waypoints[-2] |
| 281 | |
| 282 | angle = np.degrees(np.pi / 2 - np.arctan2(aim[1], aim[0])) / 90 |
| 283 | angle_last = np.degrees(np.pi / 2 - np.arctan2(aim_last[1], aim_last[0])) / 90 |
| 284 | angle_target = np.degrees(np.pi / 2 - np.arctan2(target[1], target[0])) / 90 |
| 285 | |
| 286 | # choice of point to aim for steering, removing outlier predictions |
| 287 | # use target point if it has a smaller angle or if error is large |
| 288 | # predicted point otherwise |
| 289 | # (reduces noise in eg. straight roads, helps with sudden turn commands) |
| 290 | use_target_to_aim = np.abs(angle_target) < np.abs(angle) |
| 291 | use_target_to_aim = use_target_to_aim or (np.abs(angle_target-angle_last) > self.config.angle_thresh and target[1] < self.config.dist_thresh) |
| 292 | if use_target_to_aim: |
| 293 | angle_final = angle_target |
| 294 | else: |
| 295 | angle_final = angle |
| 296 | |
| 297 | steer = self.turn_controller.step(angle_final) |
| 298 | steer = np.clip(steer, -1.0, 1.0) |
| 299 | |
| 300 | speed = velocity[0].data.cpu().numpy() |
| 301 | brake = desired_speed < self.config.brake_speed or (speed / desired_speed) > self.config.brake_ratio |
| 302 | |
| 303 | delta = np.clip(desired_speed - speed, 0.0, self.config.clip_delta) |
| 304 | throttle = self.speed_controller.step(delta) |
| 305 | throttle = np.clip(throttle, 0.0, self.config.max_throttle) |
| 306 | throttle = throttle if not brake else 0.0 |
| 307 |