Add priority system for vehicle control input so client always overwrites other inputs

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/DataRouter.h

@@ -46,10 +46,10 @@ public:
     return Agents;
   }
 
-  void ApplyVehicleControl(const FVehicleControl &VehicleControl)
+  void ApplyVehicleControl(const FVehicleControl &VehicleControl, EVehicleInputPriority Priority)
   {
     check((Player != nullptr) && (Player->IsPossessingAVehicle()));
-    Player->GetPossessedVehicle()->ApplyVehicleControl(VehicleControl);
+    Player->GetPossessedVehicle()->ApplyVehicleControl(VehicleControl, Priority);
   }
 
   void RestartLevel();

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/ServerGameController.cpp

@@ -137,7 +137,7 @@ void FServerGameController::Tick(float DeltaSeconds)
     FVehicleControl Control;
     if (Errc::Error != Server->ReadControl(Control, bShouldBlock))
     {
-      DataRouter.ApplyVehicleControl(Control);
+      DataRouter.ApplyVehicleControl(Control, EVehicleInputPriority::Client);
     } // Here we ignore the error too.
   }
 }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp

@@ -531,7 +531,7 @@ void FTheNewCarlaServer::FPimpl::BindActions()
     {
       RESPOND_ERROR("unable to apply control: actor is not a vehicle");
     }
-    Vehicle->ApplyVehicleControl(Control);
+    Vehicle->ApplyVehicleControl(Control, EVehicleInputPriority::Client);
     return R<void>::Success();
   };
 

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

@@ -75,63 +75,65 @@ float ACarlaWheeledVehicle::GetMaximumSteerAngle() const
 // -- Set functions ------------------------------------------------------------
 // =============================================================================
 
-void ACarlaWheeledVehicle::ApplyVehicleControl(const FVehicleControl &VehicleControl)
+void ACarlaWheeledVehicle::FlushVehicleControl()
 {
   auto *MovementComponent = GetVehicleMovementComponent();
-  MovementComponent->SetThrottleInput(VehicleControl.Throttle);
-  MovementComponent->SetSteeringInput(VehicleControl.Steer);
-  MovementComponent->SetBrakeInput(VehicleControl.Brake);
-  MovementComponent->SetHandbrakeInput(VehicleControl.bHandBrake);
-  if (Control.bReverse != VehicleControl.bReverse)
+  MovementComponent->SetThrottleInput(InputControl.Control.Throttle);
+  MovementComponent->SetSteeringInput(InputControl.Control.Steer);
+  MovementComponent->SetBrakeInput(InputControl.Control.Brake);
+  MovementComponent->SetHandbrakeInput(InputControl.Control.bHandBrake);
+  if (LastAppliedControl.bReverse != InputControl.Control.bReverse)
   {
-    MovementComponent->SetUseAutoGears(!VehicleControl.bReverse);
-    MovementComponent->SetTargetGear(VehicleControl.bReverse ? -1 : 1, true);
+    MovementComponent->SetUseAutoGears(!InputControl.Control.bReverse);
+    MovementComponent->SetTargetGear(InputControl.Control.bReverse ? -1 : 1, true);
   }
   else
   {
-    MovementComponent->SetUseAutoGears(!VehicleControl.bManualGearShift);
-    if (VehicleControl.bManualGearShift)
+    MovementComponent->SetUseAutoGears(!InputControl.Control.bManualGearShift);
+    if (InputControl.Control.bManualGearShift)
     {
-      MovementComponent->SetTargetGear(VehicleControl.Gear, true);
+      MovementComponent->SetTargetGear(InputControl.Control.Gear, true);
     }
   }
-  Control = VehicleControl;
-  Control.Gear = MovementComponent->GetCurrentGear();
-  Control.bReverse = Control.Gear < 0;
+  InputControl.Control.Gear = MovementComponent->GetCurrentGear();
+  InputControl.Control.bReverse = InputControl.Control.Gear < 0;
+  LastAppliedControl = InputControl.Control;
+  InputControl.Priority = EVehicleInputPriority::INVALID;
 }
 
 void ACarlaWheeledVehicle::SetThrottleInput(const float Value)
 {
-  GetVehicleMovementComponent()->SetThrottleInput(Value);
+  FVehicleControl Control = InputControl.Control;
   Control.Throttle = Value;
+  ApplyVehicleControl(Control, EVehicleInputPriority::User);
 }
 
 void ACarlaWheeledVehicle::SetSteeringInput(const float Value)
 {
-  GetVehicleMovementComponent()->SetSteeringInput(Value);
+  FVehicleControl Control = InputControl.Control;
   Control.Steer = Value;
+  ApplyVehicleControl(Control, EVehicleInputPriority::User);
 }
 
 void ACarlaWheeledVehicle::SetBrakeInput(const float Value)
 {
-  GetVehicleMovementComponent()->SetBrakeInput(Value);
+  FVehicleControl Control = InputControl.Control;
   Control.Brake = Value;
+  ApplyVehicleControl(Control, EVehicleInputPriority::User);
 }
 
 void ACarlaWheeledVehicle::SetReverse(const bool Value)
 {
-  if (Value != Control.bReverse) {
-    Control.bReverse = Value;
-    auto MovementComponent = GetVehicleMovementComponent();
-    MovementComponent->SetUseAutoGears(!Control.bReverse);
-    MovementComponent->SetTargetGear(Control.bReverse ? -1 : 1, true);
-  }
+  FVehicleControl Control = InputControl.Control;
+  Control.bReverse = Value;
+  ApplyVehicleControl(Control, EVehicleInputPriority::User);
 }
 
 void ACarlaWheeledVehicle::SetHandbrakeInput(const bool Value)
 {
-  GetVehicleMovementComponent()->SetHandbrakeInput(Value);
+  FVehicleControl Control = InputControl.Control;
   Control.bHandBrake = Value;
+  ApplyVehicleControl(Control, EVehicleInputPriority::User);
 }
 
 FVehiclePhysicsControl ACarlaWheeledVehicle::GetVehiclePhysicsControl()

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

@@ -10,6 +10,7 @@
 
 #include "Vehicle/CarlaWheeledVehicleState.h"
 #include "Vehicle/VehicleControl.h"
+#include "Vehicle/VehicleInputPriority.h"
 #include "Vehicle/VehiclePhysicsControl.h"
 #include "WheeledVehicleMovementComponent4W.h"
 
@@ -17,7 +18,6 @@
 
 #include "CarlaWheeledVehicle.generated.h"
 
-
 class UBoxComponent;
 class UVehicleAgentComponent;
 
@@ -48,7 +48,7 @@ public:
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   const FVehicleControl &GetVehicleControl() const
   {
-    return Control;
+    return LastAppliedControl;
   }
 
   /// Transform of the vehicle. Location is shifted so it matches center of the
@@ -90,6 +90,19 @@ public:
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   float GetMaximumSteerAngle() const;
 
+  /// @}
+  // ===========================================================================
+  /// @name AI debug state
+  // ===========================================================================
+  /// @{
+public:
+
+  /// @todo This function should be private to AWheeledVehicleAIController.
+  void SetAIVehicleState(ECarlaWheeledVehicleState InState)
+  {
+    State = InState;
+  }
+
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   ECarlaWheeledVehicleState GetAIVehicleState() const
   {
@@ -102,14 +115,30 @@ public:
 
   /// @}
   // ===========================================================================
-  /// @name Set functions
-  /// @todo Keep only ApplyVehicleControl.
+  /// @name Vehicle input control
   // ===========================================================================
   /// @{
 public:
 
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
-  void ApplyVehicleControl(const FVehicleControl &VehicleControl);
+  void ApplyVehicleControl(const FVehicleControl &Control, EVehicleInputPriority Priority)
+  {
+    if (InputControl.Priority <= Priority)
+    {
+      InputControl.Control = Control;
+      InputControl.Priority = Priority;
+    }
+  }
+
+  /// @todo This function should be private to AWheeledVehicleAIController.
+  void FlushVehicleControl();
+
+  /// @}
+  // ===========================================================================
+  /// @name DEPRECATED Set functions
+  // ===========================================================================
+  /// @{
+public:
 
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   void SetThrottleInput(float Value);
@@ -126,7 +155,7 @@ public:
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   void ToggleReverse()
   {
-    SetReverse(!Control.bReverse);
+    SetReverse(!LastAppliedControl.bReverse);
   }
 
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
@@ -144,11 +173,6 @@ public:
     SetHandbrakeInput(false);
   }
 
-  void SetAIVehicleState(ECarlaWheeledVehicleState InState)
-  {
-    State = InState;
-  }
-
 private:
 
   /// Current state of the vehicle controller (for debugging purposes).
@@ -161,5 +185,12 @@ private:
   UPROPERTY(Category = "CARLA Wheeled Vehicle", VisibleAnywhere)
   UVehicleAgentComponent *VehicleAgentComponent;
 
-  FVehicleControl Control;
+  struct
+  {
+    EVehicleInputPriority Priority = EVehicleInputPriority::INVALID;
+    FVehicleControl Control;
+  }
+  InputControl;
+
+  FVehicleControl LastAppliedControl;
 };

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

@@ -0,0 +1,30 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include <type_traits>
+
+#include "VehicleInputPriority.generated.h"
+
+UENUM(BlueprintType)
+enum class EVehicleInputPriority : uint8
+{
+  INVALID = 0u  UMETA(Hidden),
+
+  Lowest        UMETA(DisplayName = "Lowest Priority", ToolTip = "Use for debugging purposes only"),
+  Relaxation    UMETA(DisplayName = "Control Relaxation Input", ToolTip = "Control relaxation when no other input is provided (non-sticky control)"),
+  Autopilot     UMETA(DisplayName = "Autopilot Input", ToolTip = "Input provided by the built-in autopilot"),
+  User          UMETA(DisplayName = "User Input", ToolTip = "Input provided by an user playing in the simulator"),
+  Client        UMETA(DisplayName = "Client Input", ToolTip = "Input provided by an RPC client connected to the simulator"),
+  Highest       UMETA(DisplayName = "Highest Priority", ToolTip = "Use for debugging purposes only")
+};
+
+inline static bool operator<=(EVehicleInputPriority Lhs, EVehicleInputPriority Rhs)
+{
+  constexpr auto Cast = [](auto e) { return static_cast<typename std::underlying_type<decltype(e)>::type>(e); };
+  return Cast(Lhs) <= Cast(Rhs);
+}

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

@@ -138,12 +138,14 @@ void AWheeledVehicleAIController::Tick(const float DeltaTime)
 
   if (bAutopilotEnabled)
   {
-    Vehicle->ApplyVehicleControl(AutopilotControl);
+    Vehicle->ApplyVehicleControl(AutopilotControl, EVehicleInputPriority::Autopilot);
   }
   else if (!bControlIsSticky)
   {
-    Vehicle->ApplyVehicleControl(FVehicleControl{});
+    Vehicle->ApplyVehicleControl(FVehicleControl{}, EVehicleInputPriority::Relaxation);
   }
+
+  Vehicle->FlushVehicleControl();
 }
 
 // =============================================================================