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