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)