Added wheel control (#3868)

* First version - Wheel direction changes

* Updated CHANGELOG.md

* Fix DVSCamera include error

* Updated vehicle wheels direction commands

* Reset files back to default state

* Removed files from PR.

* Updated changelog

CHANGELOG.md

@@ -1,5 +1,5 @@
 ## Latest
-
+	
   * Fixed bug causing the RoadOptions at the BehaviorAgent to not work as intended
   * Upgrading to Unreal Engine 4.26
   * Added Lincoln 2020 vehicle dimensions for CarSim integration
@@ -14,6 +14,9 @@
   * Fixed a bug when importing a FBX map with some **_** in the FBX name
   * Fix bug in carla.Transform.get_up_vector()
   * When setting a global plan at the LocalPlanner, it is now optional to stop the automatic fill of the waypoint buffer
+  * API extensions:
+  - Added `set_wheel_steer_direction()` function to change the bone angle of each wheel of a vehicle
+  - Added `get_wheel_steer_angle()` function to get the steer angle of a vehicle whee
 
 ## CARLA 0.9.11
 

LibCarla/source/carla/client/Vehicle.cpp

@@ -59,6 +59,14 @@ namespace client {
     GetEpisode().Lock()->SetLightStateToVehicle(*this, rpc::VehicleLightState(light_state));
   }
 
+  void Vehicle::SetWheelSteerDirection(WheelLocation wheel_location, float angle_in_deg) {
+    GetEpisode().Lock()->SetWheelSteerDirection(*this, wheel_location, angle_in_deg);
+  }
+
+  float Vehicle::GetWheelSteerAngle(WheelLocation wheel_location) {
+    return GetEpisode().Lock()->GetWheelSteerAngle(*this, wheel_location);
+  }
+
   Vehicle::Control Vehicle::GetControl() const {
     return GetEpisode().Lock()->GetActorSnapshot(*this).state.vehicle_data.control;
   }

LibCarla/source/carla/client/Vehicle.h

@@ -11,6 +11,7 @@
 #include "carla/rpc/VehicleLightState.h"
 #include "carla/rpc/VehicleControl.h"
 #include "carla/rpc/VehiclePhysicsControl.h"
+#include "carla/rpc/VehicleWheels.h"
 #include "carla/trafficmanager/TrafficManager.h"
 
 using carla::traffic_manager::constants::Networking::TM_DEFAULT_PORT;
@@ -32,6 +33,8 @@ namespace client {
     using PhysicsControl = rpc::VehiclePhysicsControl;
     using LightState = rpc::VehicleLightState::LightState;
     using TM = traffic_manager::TrafficManager;
+    using WheelLocation = carla::rpc::VehicleWheelLocation;
+
 
     explicit Vehicle(ActorInitializer init);
 
@@ -47,6 +50,14 @@ namespace client {
     /// Sets a @a LightState to this vehicle.
     void SetLightState(const LightState &light_state);
 
+    /// Sets a @a Rotation to a wheel of the vehicle (affects the bone of the car skeleton, not the physics)
+    void SetWheelSteerDirection(WheelLocation wheel_location, float angle_in_deg);
+
+    /// Return a @a Rotation from a wheel of the vehicle
+    ///
+    /// @note The function returns the rotation of the vehicle based on the it's physics
+    float GetWheelSteerAngle(WheelLocation wheel_location);
+
     /// Return the control last applied to this vehicle.
     ///
     /// @note This function does not call the simulator, it returns the data
@@ -107,6 +118,6 @@ namespace client {
 
     Control _control;
   };
-
+  
 } // namespace client
 } // namespace carla

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

@@ -228,6 +228,19 @@ namespace detail {
     return _pimpl->AsyncCall("set_vehicle_light_state", vehicle, light_state);
   }
 
+  void Client::SetWheelSteerDirection(
+        rpc::ActorId vehicle, 
+        rpc::VehicleWheelLocation vehicle_wheel,
+        float angle_in_deg){
+    return _pimpl->AsyncCall("set_wheel_steer_direction", vehicle, vehicle_wheel, angle_in_deg);
+  }
+
+  float Client::GetWheelSteerAngle(
+        rpc::ActorId vehicle,
+        rpc::VehicleWheelLocation wheel_location){
+    return _pimpl->CallAndWait<float>("get_wheel_steer_angle", vehicle, wheel_location);
+  }
+
   rpc::Actor Client::SpawnActor(
       const rpc::ActorDescription &description,
       const geom::Transform &transform) {

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

@@ -29,6 +29,7 @@
 #include "carla/rpc/WeatherParameters.h"
 #include "carla/rpc/OpendriveGenerationParameters.h"
 #include "carla/rpc/VehicleLightStateList.h"
+#include "carla/rpc/VehicleWheels.h"
 
 #include <functional>
 #include <memory>
@@ -219,6 +220,17 @@ namespace detail {
         rpc::ActorId vehicle,
         bool enabled);
 
+    void SetWheelSteerDirection(
+        rpc::ActorId vehicle, 
+        rpc::VehicleWheelLocation vehicle_wheel,
+        float angle_in_deg
+    );
+
+    float GetWheelSteerAngle(
+        rpc::ActorId vehicle,
+        rpc::VehicleWheelLocation wheel_location
+    );
+    
     void EnableChronoPhysics(
         rpc::ActorId vehicle,
         uint64_t MaxSubsteps,

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

@@ -24,6 +24,7 @@
 #include "carla/rpc/TrafficLightState.h"
 #include "carla/rpc/VehicleLightStateList.h"
 #include "carla/rpc/LabelledPoint.h"
+#include "carla/rpc/VehicleWheels.h"
 
 #include <boost/optional.hpp>
 
@@ -446,6 +447,14 @@ namespace detail {
       _client.SetLightStateToVehicle(vehicle.GetId(), light_state);
     }
 
+    void SetWheelSteerDirection(Vehicle &vehicle, rpc::VehicleWheelLocation wheel_location, float angle_in_deg) {
+      _client.SetWheelSteerDirection(vehicle.GetId(), wheel_location, angle_in_deg);
+    }
+
+    float GetWheelSteerAngle(Vehicle &vehicle, rpc::VehicleWheelLocation wheel_location) {
+      return _client.GetWheelSteerAngle(vehicle.GetId(), wheel_location);
+    }
+
     void EnableCarSim(Vehicle &vehicle, std::string simfile_path) {
       _client.EnableCarSim(vehicle.GetId(), simfile_path);
     }

LibCarla/source/carla/rpc/VehicleWheels.h

@@ -0,0 +1,29 @@
+// Copyright (c) 2019 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 "carla/MsgPack.h"
+#include "carla/MsgPackAdaptors.h"
+
+namespace carla {
+namespace rpc {
+
+  enum class VehicleWheelLocation : uint8_t {
+
+    FL_Wheel = 0,
+    FR_Wheel = 1,
+    BL_Wheel = 2, 
+    BR_Wheel = 3,
+    //Use for bikes and bicycles
+    Front_Wheel = 0,
+    Back_Wheel = 1,
+  };
+  
+}
+}
+
+MSGPACK_ADD_ENUM(carla::rpc::VehicleWheelLocation);

PythonAPI/carla/source/libcarla/Actor.cpp

@@ -128,11 +128,22 @@ void export_actor() {
     .value("All", cr::VehicleLightState::LightState::All)
   ;
 
+  enum_<cr::VehicleWheelLocation>("VehicleWheelLocation")
+    .value("FL_Wheel", cr::VehicleWheelLocation::FL_Wheel)
+    .value("FR_Wheel", cr::VehicleWheelLocation::FR_Wheel)
+    .value("BL_Wheel", cr::VehicleWheelLocation::BL_Wheel)
+    .value("BR_Wheel", cr::VehicleWheelLocation::BR_Wheel)
+    .value("Front_Wheel", cr::VehicleWheelLocation::Front_Wheel)
+    .value("Back_Wheel", cr::VehicleWheelLocation::Back_Wheel)
+  ;
+  
   class_<cc::Vehicle, bases<cc::Actor>, boost::noncopyable, boost::shared_ptr<cc::Vehicle>>("Vehicle",
       no_init)
       .def("apply_control", &cc::Vehicle::ApplyControl, (arg("control")))
       .def("get_control", &cc::Vehicle::GetControl)
       .def("set_light_state", &cc::Vehicle::SetLightState, (arg("light_state")))
+      .def("set_wheel_steer_direction", &cc::Vehicle::SetWheelSteerDirection, (arg("wheel_location")), (arg("angle_in_deg")))
+      .def("get_wheel_steer_angle", &cc::Vehicle::GetWheelSteerAngle, (arg("wheel_location")))
       .def("get_light_state", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetLightState))
       .def("apply_physics_control", &cc::Vehicle::ApplyPhysicsControl, (arg("physics_control")))
       .def("get_physics_control", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetPhysicsControl))

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

@@ -47,6 +47,7 @@
 #include <carla/rpc/VehicleLightStateList.h>
 #include <carla/rpc/WalkerBoneControl.h>
 #include <carla/rpc/WalkerControl.h>
+#include <carla/rpc/VehicleWheels.h>
 #include <carla/rpc/WeatherParameters.h>
 #include <carla/streaming/Server.h>
 #include <compiler/enable-ue4-macros.h>
@@ -906,6 +907,49 @@ void FCarlaServer::FPimpl::BindActions()
     return R<void>::Success();
   };
 
+  BIND_SYNC(set_wheel_steer_direction) << [this](
+    cr::ActorId ActorId,
+    cr::VehicleWheelLocation WheelLocation,
+    float AngleInDeg) -> R<void>
+  {
+
+    REQUIRE_CARLA_EPISODE();
+    auto ActorView = Episode->FindActor(ActorId);
+    if(!ActorView.IsValid()){
+
+      RESPOND_ERROR("unable to apply actor wheel steer : actor not found");
+    }
+
+    auto Vehicle = Cast<ACarlaWheeledVehicle>(ActorView.GetActor());
+    if(Vehicle == nullptr){
+
+      RESPOND_ERROR("unable to apply actor wheel steer : actor is not a vehicle");
+    }
+
+    Vehicle->SetWheelSteerDirection((VehicleWheelLocation)WheelLocation, AngleInDeg);
+    return R<void>::Success();
+  };
+
+  BIND_SYNC(get_wheel_steer_angle) << [this](
+      const cr::ActorId ActorId,
+      cr::VehicleWheelLocation WheelLocation) -> R<float>
+  {
+    REQUIRE_CARLA_EPISODE();
+    auto ActorView = Episode->FindActor(ActorId);
+    if(!ActorView.IsValid()){
+
+      RESPOND_ERROR("unable to get actor wheel angle : actor not found");
+    }
+
+    auto Vehicle = Cast<ACarlaWheeledVehicle>(ActorView.GetActor());
+    if(Vehicle == nullptr){
+
+      RESPOND_ERROR("unable to get actor wheel angle : actor is not a vehicle");
+    }
+
+    return Vehicle->GetWheelSteerAngle((VehicleWheelLocation)WheelLocation);
+  };
+
   BIND_SYNC(set_actor_simulate_physics) << [this](
       cr::ActorId ActorId,
       bool bEnabled) -> R<void>

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/EnvironmentObject.h

@@ -5,6 +5,7 @@
 
 #pragma once
 
+#include "Carla/Util/BoundingBox.h"
 #include <compiler/disable-ue4-macros.h>
 #include <carla/rpc/ObjectLabel.h>
 #include <compiler/enable-ue4-macros.h>

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

@@ -461,6 +461,24 @@ void ACarlaWheeledVehicle::SetCarlaMovementComponent(UBaseCarlaMovementComponent
   BaseMovementComponent = MovementComponent;
 }
 
+void ACarlaWheeledVehicle::SetWheelSteerDirection(VehicleWheelLocation WheelLocation, float AngleInDeg) {
+
+  check((uint8)WheelLocation >= 0)
+  check((uint8)WheelLocation < 4)
+  UVehicleAnimInstance *VehicleAnim = Cast<UVehicleAnimInstance>(GetMesh()->GetAnimInstance());
+  check(VehicleAnim != nullptr)
+  VehicleAnim->SetWheelRotYaw((uint8)WheelLocation, AngleInDeg);
+}
+
+float ACarlaWheeledVehicle::GetWheelSteerAngle(VehicleWheelLocation WheelLocation) {
+
+  check((uint8)WheelLocation >= 0)
+  check((uint8)WheelLocation < 4)
+  UVehicleAnimInstance *VehicleAnim = Cast<UVehicleAnimInstance>(GetMesh()->GetAnimInstance());
+  check(VehicleAnim != nullptr)
+  return VehicleAnim->GetWheeledVehicleMovementComponent()->Wheels[(uint8)WheelLocation]->GetSteerAngle();
+}
+
 void ACarlaWheeledVehicle::SetSimulatePhysics(bool enabled) {
   if(!GetCarlaMovementComponent<UDefaultMovementComponent>())
   {

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

@@ -15,6 +15,7 @@
 #include "Vehicle/VehiclePhysicsControl.h"
 #include "VehicleVelocityControl.h"
 #include "WheeledVehicleMovementComponent4W.h"
+#include "VehicleAnimInstance.h"
 #include "MovementComponents/BaseCarlaMovementComponent.h"
 
 #include "CoreMinimal.h"
@@ -29,6 +30,18 @@
 
 class UBoxComponent;
 
+UENUM()
+enum class VehicleWheelLocation : uint8 {
+
+  FL_Wheel = 0,
+  FR_Wheel = 1,
+  BL_Wheel = 2,
+  BR_Wheel = 3,
+  //Use for bikes and bicycles
+  Front_Wheel = 0,
+  Back_Wheel = 1,
+};
+
 /// Base class for CARLA wheeled vehicles.
 UCLASS()
 class CARLA_API ACarlaWheeledVehicle : public AWheeledVehicle
@@ -259,6 +272,16 @@ private:
 
 public:
 
+  /// Set the rotation of the car wheels indicated by the user
+  /// 0 = FL_VehicleWheel, 1 = FR_VehicleWheel, 2 = BL_VehicleWheel, 3 = BR_VehicleWheel
+  /// An enum could be used, but this is something situational. An uint8 does the trick just fine
+  ///NOTE : This is purely aesthetic. It will not modify the physics of the car in any way
+  UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
+  void SetWheelSteerDirection(VehicleWheelLocation WheelLocation, float AngleInDeg);
+  
+  UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
+  float GetWheelSteerAngle(VehicleWheelLocation WheelLocation);
+
   virtual FVector GetVelocity() const override;
 
 //-----CARSIM--------------------------------