diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AcekrmannController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AcekrmannController.cpp index c3b31fed9..689132ea3 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AcekrmannController.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AcekrmannController.cpp @@ -35,7 +35,7 @@ void FAckermannController::ApplySettings(const FAckermannControllerSettings& Set SpeedController.Kp = Settings.SpeedKp; SpeedController.Ki = Settings.SpeedKi; SpeedController.Kd = Settings.SpeedKd; - + AccelerationController.Kp = Settings.AccelKp; AccelerationController.Ki = Settings.AccelKi; AccelerationController.Kd = Settings.AccelKd; @@ -93,35 +93,6 @@ void FAckermannController::RunLoop(FVehicleControl& Control) { Control.Throttle = FMath::Clamp(Throttle, 0.0f, 1.0f); Control.Brake = FMath::Clamp(Brake, 0.0f, 1.0f); Control.bReverse = bReverse; - - // // Debugging - // UE_LOG(LogCarla, Log, TEXT("[AckermannLog];%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%d;%f;%f;%f;%f"), - // DeltaTime, - // UserTargetPoint.Steer, - // UserTargetPoint.Speed, - // UserTargetPoint.Acceleration, - // UserTargetPoint.Jerk, - // VehicleSpeed, - // VehicleAcceleration, - // //VehiclePitch, - // Control.Steer, - // Control.Throttle, - // Control.Brake, - // Control.Gear, - // //SpeedController.Proportional, - // //SpeedController.Integral, - // //SpeedController.Derivative, - // SpeedControlAccelDelta, - // SpeedControlAccelTarget, - // //AccelerationController.Proportional, - // //AccelerationController.Integral, - // //AccelerationController.Derivative, - // AccelControlPedalDelta, - // AccelControlPedalTarget - // //BrakeUpperBorder, - // //ThrottleLowerBorder - // ); - } void FAckermannController::RunControlSteering() { @@ -137,7 +108,7 @@ bool FAckermannController::RunControlFullStop() { Throttle = 0.0; return true; } - return false; + return false; } void FAckermannController::RunControlReverse() { @@ -163,7 +134,6 @@ void FAckermannController::RunControlReverse() { } } - void FAckermannController::RunControlSpeed() { SpeedController.SetTargetPoint(TargetSpeed); SpeedControlAccelDelta = SpeedController.Run(VehicleSpeed, DeltaTime); @@ -217,16 +187,17 @@ void FAckermannController::UpdateVehicleState(const ACarlaWheeledVehicle* Vehicl LastVehicleSpeed = VehicleSpeed; LastVehicleAcceleration = VehicleAcceleration; - + // Update simulation state DeltaTime = Vehicle->GetWorld()->GetDeltaSeconds(); // Update Vehicle state VehicleSpeed = Vehicle->GetVehicleForwardSpeed() / 100.0f; // From cm/s to m/s float CurrentAcceleration = (VehicleSpeed - LastVehicleSpeed) / DeltaTime; + // Apply an average filter for the acceleration. VehicleAcceleration = (4.0f*LastVehicleAcceleration + CurrentAcceleration) / 5.0f; } void FAckermannController::UpdateVehiclePhysics(const ACarlaWheeledVehicle* Vehicle) { VehicleMaxSteering = FMath::DegreesToRadians(Vehicle->GetMaximumSteerAngle()); -} \ No newline at end of file +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannController.h index 70cf26abb..39b107c23 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannController.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannController.h @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2022 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -51,7 +51,7 @@ class PID Integral = 0.0f; Integral = FMath::Clamp(Integral, MinOutput, MaxOutput); Derivative = 0.0f; - + LastError = 0.0f; LastInput = 0.0f; } @@ -136,7 +136,7 @@ private: // Internal control parameters float SpeedControlAccelDelta = 0.0f; float SpeedControlAccelTarget = 0.0f; - + float AccelControlPedalDelta = 0.0f; float AccelControlPedalTarget = 0.0f; @@ -151,4 +151,4 @@ private: float LastVehicleSpeed = 0.0f; // [m/s] float LastVehicleAcceleration = 0.0f; // [m/s2] -}; \ No newline at end of file +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp index dd68c91e1..be6c2afa3 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp @@ -726,7 +726,7 @@ void ACarlaWheeledVehicle::ApplyVehiclePhysicsControl(const FVehiclePhysicsContr { Recorder->AddPhysicsControl(*this); } - + // Update physics in the Ackermann Controller AckermannController.UpdateVehiclePhysics(this); }