Handle steer speed in the ackermann controller

LibCarla/source/carla/client/Vehicle.h

@@ -55,7 +55,12 @@ namespace client {
     /// Apply @a control to this vehicle.
     void ApplyAckermannControl(const AckermannControl &control);
 
+    /// Return the last Ackermann controller settings applied to this vehicle.
+    ///
+    /// @warning This function does call the simulator.
     rpc::AckermannControllerSettings GetAckermannControllerSettings() const;
+
+    /// Apply Ackermann control settings to this vehicle
     void ApplyAckermannControllerSettings(const rpc::AckermannControllerSettings &settings);
 
     /// Apply physics control to this vehicle.

PythonAPI/examples/manual_control.py

@@ -732,10 +732,7 @@ class HUD(object):
                 self._info_text += [
                     '',
                     'Ackermann Controller:',
-                    '  Target steer: % 9.0f rad' % self._ackermann_control.steer,
                     '  Target speed: % 8.0f km/h' % (3.6*self._ackermann_control.speed),
-                    '  Target accel: % 7.0f km/h2' % self._ackermann_control.acceleration,
-                    '  Target jerk:  % 7.0f km/h3' % self._ackermann_control.jerk,
                 ]
         elif isinstance(c, carla.WalkerControl):
             self._info_text += [

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannController.cpp

@@ -44,10 +44,11 @@ void FAckermannController::ApplySettings(const FAckermannControllerSettings& Set
 void FAckermannController::SetTargetPoint(const FVehicleAckermannControl& AckermannControl) {
   UserTargetPoint = AckermannControl;
 
-  TargetSteer = UserTargetPoint.Steer;
+  TargetSteer = FMath::Clamp(UserTargetPoint.Steer, -VehicleMaxSteering, VehicleMaxSteering);
+  TargetSteerSpeed = FMath::Abs(UserTargetPoint.SteerSpeed);
   TargetSpeed = UserTargetPoint.Speed;
-  TargetAcceleration = UserTargetPoint.Acceleration;
-  TargetJerk = UserTargetPoint.Jerk;
+  TargetAcceleration = FMath::Abs(UserTargetPoint.Acceleration);
+  TargetJerk = FMath::Abs(UserTargetPoint.Jerk);
 
 }
 
@@ -58,6 +59,9 @@ void FAckermannController::Reset() {
 
   // Reset control parameters
   Steer = 0.0f;
+  Throttle = 0.0f;
+  Brake = 0.0f;
+  bReverse = false;
 
   SpeedControlAccelDelta = 0.0f;
   SpeedControlAccelTarget = 0.0f;
@@ -89,14 +93,26 @@ void FAckermannController::RunLoop(FVehicleControl& Control) {
   }
 
   // Update control command
-  Control.Steer = Steer;
+  Control.Steer = Steer / VehicleMaxSteering;
   Control.Throttle = FMath::Clamp(Throttle, 0.0f, 1.0f);
   Control.Brake = FMath::Clamp(Brake, 0.0f, 1.0f);
   Control.bReverse = bReverse;
 }
 
 void FAckermannController::RunControlSteering() {
-  Steer = TargetSteer / VehicleMaxSteering;
+  if (FMath::Abs(TargetSteerSpeed) < 0.001) {
+    Steer = TargetSteer;
+  } else {
+    float SteerDelta = TargetSteerSpeed * DeltaTime;
+    if (FMath::Abs(TargetSteer - VehicleSteer) < SteerDelta) {
+      Steer = TargetSteer;
+    } else {
+      float SteerDirection = (TargetSteer > VehicleSteer) ? 1.0f : -1.0f;
+      UE_LOG(LogCarla, Error, TEXT("Direction: %f"), SteerDirection);
+      Steer = VehicleSteer + SteerDirection * (TargetSteerSpeed * DeltaTime);
+    }
+
+  }
 }
 
 bool FAckermannController::RunControlFullStop() {
@@ -192,6 +208,7 @@ void FAckermannController::UpdateVehicleState(const ACarlaWheeledVehicle* Vehicl
   DeltaTime = Vehicle->GetWorld()->GetDeltaSeconds();
 
   // Update Vehicle state
+  VehicleSteer = Vehicle->GetVehicleControl().Steer * VehicleMaxSteering;
   VehicleSpeed = Vehicle->GetVehicleForwardSpeed() / 100.0f;  // From cm/s to m/s
   float CurrentAcceleration = (VehicleSpeed - LastVehicleSpeed) / DeltaTime;
   // Apply an average filter for the acceleration.

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannController.h

@@ -119,6 +119,7 @@ private:
 
   // Target point after aplying restrictions
   float TargetSteer = 0.0;
+  float TargetSteerSpeed = 0.0;
   float TargetSpeed = 0.0;
   float TargetAcceleration = 0.0;
   float TargetJerk = 0.0;
@@ -146,6 +147,7 @@ private:
   // Vehicle state
   float VehicleMaxSteering = 0.0f;       // [rad]
 
+  float VehicleSteer = 0.0f;             // [rad]
   float VehicleSpeed = 0.0f;             // [m/s]
   float VehicleAcceleration = 0.0f;      // [m/s2]