def run_step(self, vehicle: Vehicle) -> float:
self.sync(vehicle=vehicle)
return float(
VehicleControl.clamp(
self.kp * (self.target_speed - Vehicle.get_speed(vehicle)), 0,
1
)
)
def run_step(self, vehicle: Vehicle, next_waypoint: Transform) -> float:
self.sync(vehicle=vehicle)
target_y = next_waypoint.location.y
target_x = next_waypoint.location.x
angle_difference = math.atan2(
target_y - self.vehicle.transform.location.y,
target_x - self.vehicle.transform.location.x,
) - np.radians(self.vehicle.transform.rotation.yaw)
curr_look_forward = (
self.look_ahead_gain * Vehicle.get_speed(vehicle=vehicle)
+ self.look_ahead_distance
)
lateral_difference = math.atan2(
2.0
* self.vehicle.wheel_base
* math.sin(angle_difference)
/ curr_look_forward,
1.0,
)
return VehicleControl.clamp(lateral_difference, -1, 1)