First version of the Constant velocity mode

This mode can be enabled in the API and it will keep
a velocity constant.
For now, it needs to be disable automatically but this
can be improved in the future.

LibCarla/source/carla/client/Actor.cpp

@@ -48,6 +48,14 @@ namespace client {
     GetEpisode().Lock()->SetActorTargetAngularVelocity(*this, vector);
   }
 
+  void Actor::EnableConstantVelocity(const geom::Vector3D &vector) {
+    GetEpisode().Lock()->EnableActorConstantVelocity(*this, vector);
+  }
+
+  void Actor::DisableConstantVelocity() {
+    GetEpisode().Lock()->DisableActorConstantVelocity(*this);
+  }
+
   void Actor::AddImpulse(const geom::Vector3D &vector) {
     GetEpisode().Lock()->AddActorImpulse(*this, vector);
   }

LibCarla/source/carla/client/Actor.h

@@ -69,6 +69,11 @@ namespace client {
 
     /// Set the angular velocity of the actor before applying physics.
     void SetTargetAngularVelocity(const geom::Vector3D &vector);
+    /// Enable a constant velocity mode
+    void EnableConstantVelocity(const geom::Vector3D &vector);
+    /// Disable the constant velocity mode
+    void DisableConstantVelocity();
+
 
     /// Add impulse to the actor.
     void AddImpulse(const geom::Vector3D &vector);

LibCarla/source/carla/client/detail/Client.cpp

@@ -271,6 +271,13 @@ namespace detail {
   void Client::SetActorTargetAngularVelocity(rpc::ActorId actor, const geom::Vector3D &vector) {
     _pimpl->AsyncCall("set_actor_target_angular_velocity", actor, vector);
   }
+  void Client::EnableActorConstantVelocity(rpc::ActorId actor, const geom::Vector3D &vector) {
+    _pimpl->AsyncCall("enable_actor_constant_velocity", actor, vector);
+  }
+
+  void Client::DisableActorConstantVelocity(rpc::ActorId actor) {
+    _pimpl->AsyncCall("disable_actor_constant_velocity", actor);
+  }
 
   void Client::AddActorImpulse(rpc::ActorId actor, const geom::Vector3D &vector) {
     _pimpl->AsyncCall("add_actor_impulse", actor, vector);

LibCarla/source/carla/client/detail/Client.h

@@ -153,6 +153,12 @@ namespace detail {
     void SetActorTargetAngularVelocity(
         rpc::ActorId actor,
         const geom::Vector3D &vector);
+    void EnableActorConstantVelocity(
+        rpc::ActorId actor,
+        const geom::Vector3D &vector);
+
+    void DisableActorConstantVelocity(
+        rpc::ActorId actor);
 
     void AddActorImpulse(
         rpc::ActorId actor,

LibCarla/source/carla/client/detail/Simulator.h

@@ -330,6 +330,13 @@ namespace detail {
     void SetActorTargetAngularVelocity(const Actor &actor, const geom::Vector3D &vector) {
       _client.SetActorTargetAngularVelocity(actor.GetId(), vector);
     }
+    void EnableActorConstantVelocity(const Actor &actor, const geom::Vector3D &vector) {
+      _client.EnableActorConstantVelocity(actor.GetId(), vector);
+    }
+
+    void DisableActorConstantVelocity(const Actor &actor) {
+      _client.DisableActorConstantVelocity(actor.GetId());
+    }
 
     void AddActorImpulse(const Actor &actor, const geom::Vector3D &vector) {
       _client.AddActorImpulse(actor.GetId(), vector);

PythonAPI/carla/source/libcarla/Actor.cpp

@@ -87,6 +87,8 @@ void export_actor() {
       .def("set_transform", &cc::Actor::SetTransform, (arg("transform")))
       .def("set_target_velocity", &cc::Actor::SetTargetVelocity, (arg("vector")))
       .def("set_target_angular_velocity", &cc::Actor::SetTargetAngularVelocity, (arg("vector")))
+      .def("enable_constant_velocity", &cc::Actor::EnableConstantVelocity, (arg("vector")))
+      .def("disable_constant_velocity", &cc::Actor::DisableConstantVelocity)
       .def("add_impulse", &cc::Actor::AddImpulse, (arg("vector")))
       .def("add_angular_impulse", &cc::Actor::AddAngularImpulse, (arg("vector")))
       .def("set_simulate_physics", &cc::Actor::SetSimulatePhysics, (arg("enabled") = true))

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

@@ -557,6 +557,48 @@ void FCarlaServer::FPimpl::BindActions()
     return R<void>::Success();
   };
 
+  BIND_SYNC(enable_actor_constant_velocity) << [this](
+      cr::ActorId ActorId,
+      cr::Vector3D vector) -> R<void>
+  {
+    REQUIRE_CARLA_EPISODE();
+    auto ActorView = Episode->FindActor(ActorId);
+    if (!ActorView.IsValid())
+    {
+      RESPOND_ERROR("unable to set actor velocity: actor not found");
+    }
+    auto CarlaVehicle = Cast<ACarlaWheeledVehicle>(ActorView.GetActor());
+    if (CarlaVehicle == nullptr)
+    {
+      RESPOND_ERROR("unable to set actor velocity: not supported by actor");
+    }
+
+    CarlaVehicle->ActivateVelocityControl(vector.ToCentimeters().ToFVector());
+
+    return R<void>::Success();
+  };
+
+  BIND_SYNC(disable_actor_constant_velocity) << [this](
+      cr::ActorId ActorId,
+      cr::Vector3D vector) -> R<void>
+  {
+    REQUIRE_CARLA_EPISODE();
+    auto ActorView = Episode->FindActor(ActorId);
+    if (!ActorView.IsValid())
+    {
+      RESPOND_ERROR("unable to set actor velocity: actor not found");
+    }
+    auto CarlaVehicle = Cast<ACarlaWheeledVehicle>(ActorView.GetActor());
+    if (CarlaVehicle == nullptr)
+    {
+      RESPOND_ERROR("unable to set actor velocity: not supported by actor");
+    }
+
+    CarlaVehicle->DeactivateVelocityControl();
+
+    return R<void>::Success();
+  };
+
   BIND_SYNC(add_actor_impulse) << [this](
       cr::ActorId ActorId,
       cr::Vector3D vector) -> R<void>

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

@@ -30,6 +30,9 @@ ACarlaWheeledVehicle::ACarlaWheeledVehicle(const FObjectInitializer& ObjectIniti
   VehicleBounds->SetHiddenInGame(true);
   VehicleBounds->SetCollisionProfileName(UCollisionProfile::NoCollision_ProfileName);
 
+  VelocityControl = CreateDefaultSubobject<UVehicleVelocityControl>(TEXT("VelocityControl"));
+  VelocityControl->Deactivate();
+
   GetVehicleMovementComponent()->bReverseAsBrake = false;
 }
 
@@ -432,6 +435,16 @@ void ACarlaWheeledVehicle::ApplyVehiclePhysicsControl(const FVehiclePhysicsContr
   }
 }
 
+void ACarlaWheeledVehicle::ActivateVelocityControl(const FVector &Velocity)
+{
+  VelocityControl->Activate(Velocity);
+}
+
+void ACarlaWheeledVehicle::DeactivateVelocityControl()
+{
+  VelocityControl->Deactivate();
+}
+
 void ACarlaWheeledVehicle::SetVehicleLightState(const FVehicleLightState &LightState)
 {
   InputControl.LightState = LightState;

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

@@ -13,6 +13,7 @@
 #include "Vehicle/VehicleLightState.h"
 #include "Vehicle/VehicleInputPriority.h"
 #include "Vehicle/VehiclePhysicsControl.h"
+#include "VehicleVelocityControl.h"
 #include "WheeledVehicleMovementComponent4W.h"
 
 #include "CoreMinimal.h"
@@ -136,6 +137,12 @@ public:
     }
   }
 
+  UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
+  void ActivateVelocityControl(const FVector &Velocity);
+
+  UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
+  void DeactivateVelocityControl();
+
   /// @todo This function should be private to AWheeledVehicleAIController.
   void FlushVehicleControl();
 
@@ -208,6 +215,9 @@ private:
   UPROPERTY(Category = "CARLA Wheeled Vehicle", EditAnywhere)
   UBoxComponent *VehicleBounds;
 
+  UPROPERTY(Category = "CARLA Wheeled Vehicle", EditAnywhere)
+  UVehicleVelocityControl* VelocityControl;
+
   struct
   {
     EVehicleInputPriority Priority = EVehicleInputPriority::INVALID;

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

@@ -0,0 +1,55 @@
+// Copyright (c) 2020 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>.
+
+#include "Kismet/KismetMathLibrary.h"
+
+#include "VehicleVelocityControl.h"
+
+
+UVehicleVelocityControl::UVehicleVelocityControl()
+{
+  PrimaryComponentTick.bCanEverTick = true;
+}
+
+void UVehicleVelocityControl::BeginPlay()
+{
+  Super::BeginPlay();
+
+  SetComponentTickEnabled(false);
+  SetTickGroup(ETickingGroup::TG_PrePhysics);
+
+  OwnerVehicle = GetOwner();
+  PrimitiveComponent = Cast<UPrimitiveComponent>(OwnerVehicle->GetRootComponent());
+}
+
+void UVehicleVelocityControl::Activate(bool bReset)
+{
+  Super::Activate(bReset);
+
+  TargetVelocity = FVector();
+  SetComponentTickEnabled(true);
+}
+
+void UVehicleVelocityControl::Activate(FVector Velocity, bool bReset)
+{
+  Super::Activate(bReset);
+
+  TargetVelocity = Velocity;
+  SetComponentTickEnabled(true);
+}
+
+void UVehicleVelocityControl::Deactivate()
+{
+  Super::Deactivate();
+  SetComponentTickEnabled(false);
+}
+
+void UVehicleVelocityControl::TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction *ThisTickFunction)
+{
+  FTransform Transf = OwnerVehicle->GetActorTransform();
+  const FVector LocVel = Transf.TransformVector(TargetVelocity);
+  PrimitiveComponent->SetPhysicsLinearVelocity(LocVel, false, "None");
+}

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

@@ -0,0 +1,56 @@
+// Copyright (c) 2020 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 "Components/ActorComponent.h"
+#include "Components/PrimitiveComponent.h"
+#include "CoreMinimal.h"
+
+#include "VehicleVelocityControl.generated.h"
+
+/// Component that controls that the velocity of an actor is constant.
+UCLASS(Blueprintable, BlueprintType, ClassGroup=(Custom), meta=(BlueprintSpawnableComponent))
+class CARLA_API UVehicleVelocityControl : public UActorComponent
+{
+  GENERATED_BODY()
+
+  // ===========================================================================
+  /// @name Constructor and destructor
+  // ===========================================================================
+  /// @{
+public:
+  UVehicleVelocityControl();
+
+  /// @}
+  // ===========================================================================
+  /// @name Get functions
+  // ===========================================================================
+  /// @{
+public:
+
+  void BeginPlay() override;
+
+  virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction *ThisTickFunction) override;
+
+  // Activate the component setting the target velocity
+  virtual void Activate(bool bReset=false) override;
+
+  // Activate the component setting the target velocity
+  virtual void Activate(FVector Velocity, bool bReset=false);
+
+  // Deactivate the component
+	virtual void Deactivate() override;
+
+private:
+  ///
+  UPROPERTY(Category = "Vehicle Velocity Control", VisibleAnywhere)
+  FVector TargetVelocity;
+
+  UPrimitiveComponent* PrimitiveComponent;
+  AActor* OwnerVehicle;
+
+};