First iteration ackermann controller

LibCarla/source/carla/client/Vehicle.cpp

@@ -55,6 +55,18 @@ namespace client {
     }
   }
 
+  void Vehicle::ApplyAckermannControl(const AckermannControl &control) {
+    GetEpisode().Lock()->ApplyAckermannControlToVehicle(*this, control);
+  }
+
+  rpc::AckermannControllerSettings Vehicle::GetAckermannControllerSettings() const {
+    return GetEpisode().Lock()->GetAckermannControllerSettings(*this);
+  }
+
+  void Vehicle::ApplyAckermannControllerSettings(const rpc::AckermannControllerSettings &settings) {
+    GetEpisode().Lock()->ApplyAckermannControllerSettings(*this, settings);
+  }
+
   void Vehicle::ApplyPhysicsControl(const PhysicsControl &physics_control) {
     GetEpisode().Lock()->ApplyPhysicsControlToVehicle(*this, physics_control);
   }

LibCarla/source/carla/client/Vehicle.h

@@ -7,7 +7,9 @@
 #pragma once
 
 #include "carla/client/Actor.h"
+#include "carla/rpc/AckermannControllerSettings.h"
 #include "carla/rpc/TrafficLightState.h"
+#include "carla/rpc/VehicleAckermannControl.h"
 #include "carla/rpc/VehicleControl.h"
 #include "carla/rpc/VehicleDoor.h"
 #include "carla/rpc/VehicleLightState.h"
@@ -31,6 +33,7 @@ namespace client {
   public:
 
     using Control = rpc::VehicleControl;
+    using AckermannControl = rpc::VehicleAckermannControl;
     using PhysicsControl = rpc::VehiclePhysicsControl;
     using LightState = rpc::VehicleLightState::LightState;
     using TM = traffic_manager::TrafficManager;
@@ -49,6 +52,12 @@ namespace client {
     /// Apply @a control to this vehicle.
     void ApplyControl(const Control &control);
 
+    /// Apply @a control to this vehicle.
+    void ApplyAckermannControl(const AckermannControl &control);
+
+    rpc::AckermannControllerSettings GetAckermannControllerSettings() const;
+    void ApplyAckermannControllerSettings(const rpc::AckermannControllerSettings &settings);
+
     /// Apply physics control to this vehicle.
     void ApplyPhysicsControl(const PhysicsControl &physics_control);
 

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

@@ -10,11 +10,13 @@
 #include "carla/Version.h"
 #include "carla/client/FileTransfer.h"
 #include "carla/client/TimeoutException.h"
+#include "carla/rpc/AckermannControllerSettings.h"
 #include "carla/rpc/ActorDescription.h"
 #include "carla/rpc/BoneTransformDataIn.h"
 #include "carla/rpc/Client.h"
 #include "carla/rpc/DebugShape.h"
 #include "carla/rpc/Response.h"
+#include "carla/rpc/VehicleAckermannControl.h"
 #include "carla/rpc/VehicleControl.h"
 #include "carla/rpc/VehicleLightState.h"
 #include "carla/rpc/WalkerBoneControlIn.h"
@@ -424,6 +426,19 @@ namespace detail {
     _pimpl->AsyncCall("apply_control_to_vehicle", vehicle, control);
   }
 
+  void Client::ApplyAckermannControlToVehicle(rpc::ActorId vehicle, const rpc::VehicleAckermannControl &control) {
+    _pimpl->AsyncCall("apply_ackermann_control_to_vehicle", vehicle, control);
+  }
+
+  rpc::AckermannControllerSettings Client::GetAckermannControllerSettings(
+      rpc::ActorId vehicle) const {
+    return _pimpl->CallAndWait<carla::rpc::AckermannControllerSettings>("get_ackermann_controller_settings", vehicle);
+  }
+
+  void Client::ApplyAckermannControllerSettings(rpc::ActorId vehicle, const rpc::AckermannControllerSettings &settings) {
+    _pimpl->AsyncCall("apply_ackermann_controller_settings", vehicle, settings);
+  }
+
   void Client::EnableCarSim(rpc::ActorId vehicle, std::string simfile_path) {
     _pimpl->AsyncCall("enable_carsim", vehicle, simfile_path);
   }

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

@@ -43,8 +43,10 @@
 namespace carla {
   class Buffer;
 namespace rpc {
+  class AckermannControllerSettings;
   class ActorDescription;
   class DebugShape;
+  class VehicleAckermannControl;
   class VehicleControl;
   class WalkerControl;
   class WalkerBoneControlIn;
@@ -250,6 +252,16 @@ namespace detail {
         rpc::ActorId vehicle,
         const rpc::VehicleControl &control);
 
+    void ApplyAckermannControlToVehicle(
+        rpc::ActorId vehicle,
+        const rpc::VehicleAckermannControl &control);
+
+    rpc::AckermannControllerSettings GetAckermannControllerSettings(rpc::ActorId vehicle) const;
+
+    void ApplyAckermannControllerSettings(
+        rpc::ActorId vehicle,
+        const rpc::AckermannControllerSettings &settings);
+
     void EnableCarSim(
         rpc::ActorId vehicle,
         std::string simfile_path);

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

@@ -457,6 +457,18 @@ namespace detail {
       _client.ApplyControlToVehicle(vehicle.GetId(), control);
     }
 
+    void ApplyAckermannControlToVehicle(Vehicle &vehicle, const rpc::VehicleAckermannControl &control) {
+      _client.ApplyAckermannControlToVehicle(vehicle.GetId(), control);
+    }
+
+    rpc::AckermannControllerSettings GetAckermannControllerSettings(const Vehicle &vehicle) const {
+      return _client.GetAckermannControllerSettings(vehicle.GetId());
+    }
+
+    void ApplyAckermannControllerSettings(Vehicle &vehicle, const rpc::AckermannControllerSettings &settings) {
+      _client.ApplyAckermannControllerSettings(vehicle.GetId(), settings);
+    }
+
     void ApplyControlToWalker(Walker &walker, const rpc::WalkerControl &control) {
       _client.ApplyControlToWalker(walker.GetId(), control);
     }

LibCarla/source/carla/rpc/AckermannControllerSettings.h

@@ -0,0 +1,92 @@
+// Copyright (c) 2021 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"
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+#  include "Carla/Vehicle/AckermannControllerSettings.h"
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+namespace carla {
+namespace rpc {
+
+  class AckermannControllerSettings {
+  public:
+
+    AckermannControllerSettings() = default;
+
+    AckermannControllerSettings(
+        float speed_kp,
+        float speed_ki,
+        float speed_kd,
+        float accel_kp,
+        float accel_ki,
+        float accel_kd)
+      : speed_kp(speed_kp),
+        speed_ki(speed_ki),
+        speed_kd(speed_kd),
+        accel_kp(accel_kp),
+        accel_ki(accel_ki),
+        accel_kd(accel_kd) {}
+
+    float speed_kp = 0.0f;
+    float speed_ki = 0.0f;
+    float speed_kd = 0.0f;
+    float accel_kp = 0.0f;
+    float accel_ki = 0.0f;
+    float accel_kd = 0.0f;
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+
+    AckermannControllerSettings(const FAckermannControllerSettings &Settings)
+      : speed_kp(Settings.SpeedKp),
+        speed_ki(Settings.SpeedKi),
+        speed_kd(Settings.SpeedKd),
+        accel_kp(Settings.AccelKp),
+        accel_ki(Settings.AccelKi),
+        accel_kd(Settings.AccelKd) {}
+
+    operator FAckermannControllerSettings() const {
+      FAckermannControllerSettings Settings;
+      Settings.SpeedKp = speed_kp;
+      Settings.SpeedKi = speed_ki;
+      Settings.SpeedKd = speed_kd;
+      Settings.AccelKp = accel_kp;
+      Settings.AccelKi = accel_ki;
+      Settings.AccelKd = accel_kd;
+      return Settings;
+    }
+
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+    bool operator!=(const AckermannControllerSettings &rhs) const {
+      return
+          speed_kp != rhs.speed_kp ||
+          speed_ki != rhs.speed_ki ||
+          speed_kd != rhs.speed_kd ||
+          accel_kp != rhs.accel_kp ||
+          accel_ki != rhs.accel_ki ||
+          accel_kd != rhs.accel_kd;
+    }
+
+    bool operator==(const AckermannControllerSettings &rhs) const {
+      return !(*this != rhs);
+    }
+
+    MSGPACK_DEFINE_ARRAY(
+        speed_kp,
+        speed_ki,
+        speed_kd,
+        accel_kp,
+        accel_ki,
+        accel_kd
+    );
+  };
+
+} // namespace rpc
+} // namespace carla

LibCarla/source/carla/rpc/Command.h

@@ -11,6 +11,7 @@
 #include "carla/geom/Transform.h"
 #include "carla/rpc/ActorDescription.h"
 #include "carla/rpc/ActorId.h"
+#include "carla/rpc/VehicleAckermannControl.h"
 #include "carla/rpc/VehicleControl.h"
 #include "carla/rpc/VehiclePhysicsControl.h"
 #include "carla/rpc/VehicleLightState.h"
@@ -74,6 +75,16 @@ namespace rpc {
       MSGPACK_DEFINE_ARRAY(actor, control);
     };
 
+    struct ApplyVehicleAckermannControl : CommandBase<ApplyVehicleAckermannControl> {
+      ApplyVehicleAckermannControl() = default;
+      ApplyVehicleAckermannControl(ActorId id, const VehicleAckermannControl &value)
+        : actor(id),
+          control(value) {}
+      ActorId actor;
+      VehicleAckermannControl control;
+      MSGPACK_DEFINE_ARRAY(actor, control);
+    };
+
     struct ApplyWalkerControl : CommandBase<ApplyWalkerControl> {
       ApplyWalkerControl() = default;
       ApplyWalkerControl(ActorId id, const WalkerControl &value)
@@ -236,6 +247,7 @@ namespace rpc {
         SpawnActor,
         DestroyActor,
         ApplyVehicleControl,
+        ApplyVehicleAckermannControl,
         ApplyWalkerControl,
         ApplyVehiclePhysicsControl,
         ApplyTransform,

LibCarla/source/carla/rpc/VehicleAckermannControl.h

@@ -0,0 +1,84 @@
+// Copyright (c) 2021 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"
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+#  include "Carla/Vehicle/VehicleAckermannControl.h"
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+namespace carla {
+namespace rpc {
+
+  class VehicleAckermannControl {
+  public:
+
+    VehicleAckermannControl() = default;
+
+    VehicleAckermannControl(
+        float in_steer,
+        float in_steer_speed,
+        float in_speed,
+        float in_acceleration,
+        float in_jerk)
+      : steer(in_steer),
+        steer_speed(in_steer_speed),
+        speed(in_speed),
+        acceleration(in_acceleration),
+        jerk(in_jerk) {}
+
+    float steer = 0.0f;
+    float steer_speed = 0.0f;
+    float speed = 0.0f;
+    float acceleration = 0.0f;
+    float jerk = 0.0f;
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+
+    VehicleAckermannControl(const FVehicleAckermannControl &Control)
+      : steer(Control.Steer),
+        steer_speed(Control.SteerSpeed),
+        speed(Control.Speed),
+        acceleration(Control.Acceleration),
+        jerk(Control.Jerk) {}
+
+    operator FVehicleAckermannControl() const {
+      FVehicleAckermannControl Control;
+      Control.Steer = steer;
+      Control.SteerSpeed = steer_speed;
+      Control.Speed = speed;
+      Control.Acceleration = acceleration;
+      Control.Jerk = jerk;
+      return Control;
+    }
+
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+    bool operator!=(const VehicleAckermannControl &rhs) const {
+      return
+          steer != rhs.steer ||
+          steer_speed != rhs.steer_speed ||
+          speed != rhs.speed ||
+          acceleration != rhs.acceleration ||
+          jerk != rhs.jerk;
+    }
+
+    bool operator==(const VehicleAckermannControl &rhs) const {
+      return !(*this != rhs);
+    }
+
+    MSGPACK_DEFINE_ARRAY(
+        steer,
+        steer_speed,
+        speed,
+        acceleration,
+        jerk);
+  };
+
+} // namespace rpc
+} // namespace carla

PythonAPI/carla/source/libcarla/Actor.cpp

@@ -165,6 +165,7 @@ void export_actor() {
   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("apply_ackermann_control", &cc::Vehicle::ApplyAckermannControl, (arg("control")))
       .def("get_control", &cc::Vehicle::GetControl)
       .def("set_light_state", &cc::Vehicle::SetLightState, (arg("light_state")))
       .def("open_door", &cc::Vehicle::OpenDoor, (arg("door_idx")))
@@ -174,6 +175,8 @@ void export_actor() {
       .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))
+      .def("apply_ackermann_controller_settings", &cc::Vehicle::ApplyAckermannControllerSettings, (arg("settings")))
+      .def("get_ackermann_controller_settings", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetAckermannControllerSettings))
       .def("set_autopilot", CALL_WITHOUT_GIL_2(cc::Vehicle, SetAutopilot, bool, uint16_t), (arg("enabled") = true, arg("tm_port") = ctm::TM_DEFAULT_PORT))
       .def("show_debug_telemetry", &cc::Vehicle::ShowDebugTelemetry, (arg("enabled") = true))
       .def("get_speed_limit", &cc::Vehicle::GetSpeedLimit)

PythonAPI/carla/source/libcarla/Commands.cpp

@@ -101,6 +101,13 @@ void export_commands() {
     .def_readwrite("control", &cr::Command::ApplyVehicleControl::control)
   ;
 
+  class_<cr::Command::ApplyVehicleAckermannControl>("ApplyVehicleAckermannControl")
+    .def("__init__", &command_impl::CustomInit<ActorPtr, cr::VehicleAckermannControl>, (arg("actor"), arg("control")))
+    .def(init<cr::ActorId, cr::VehicleAckermannControl>((arg("actor_id"), arg("control"))))
+    .def_readwrite("actor_id", &cr::Command::ApplyVehicleAckermannControl::actor)
+    .def_readwrite("control", &cr::Command::ApplyVehicleAckermannControl::control)
+  ;
+
   class_<cr::Command::ApplyWalkerControl>("ApplyWalkerControl")
     .def("__init__", &command_impl::CustomInit<ActorPtr, cr::WalkerControl>, (arg("actor"), arg("control")))
     .def(init<cr::ActorId, cr::WalkerControl>((arg("actor_id"), arg("control"))))
@@ -211,6 +218,7 @@ void export_commands() {
   implicitly_convertible<cr::Command::SpawnActor, cr::Command>();
   implicitly_convertible<cr::Command::DestroyActor, cr::Command>();
   implicitly_convertible<cr::Command::ApplyVehicleControl, cr::Command>();
+  implicitly_convertible<cr::Command::ApplyVehicleAckermannControl, cr::Command>();
   implicitly_convertible<cr::Command::ApplyWalkerControl, cr::Command>();
   implicitly_convertible<cr::Command::ApplyVehiclePhysicsControl, cr::Command>();
   implicitly_convertible<cr::Command::ApplyTransform, cr::Command>();

PythonAPI/carla/source/libcarla/Control.cpp

@@ -5,6 +5,7 @@
 // This work is licensed under the terms of the MIT license.
 // For a copy, see <https://opensource.org/licenses/MIT>.
 
+#include <carla/rpc/VehicleAckermannControl.h>
 #include <carla/rpc/VehicleControl.h>
 #include <carla/rpc/VehiclePhysicsControl.h>
 #include <carla/rpc/WheelPhysicsControl.h>
@@ -32,6 +33,15 @@ namespace rpc {
     return out;
   }
 
+  std::ostream &operator<<(std::ostream &out, const VehicleAckermannControl &control) {
+    out << "VehicleAckermannControl(steer=" << std::to_string(control.steer)
+        << ", steer_speed=" << std::to_string(control.steer_speed)
+        << ", speed=" << std::to_string(control.speed)
+        << ", acceleration=" << std::to_string(control.acceleration)
+        << ", jerk=" << std::to_string(control.jerk) << ')';
+    return out;
+  }
+
   std::ostream &operator<<(std::ostream &out, const WalkerControl &control) {
     out << "WalkerControl(direction=" << control.direction
         << ", speed=" << std::to_string(control.speed)
@@ -105,6 +115,17 @@ namespace rpc {
     << ", use_sweep_wheel_collision=" << control.use_sweep_wheel_collision << ')';
     return out;
   }
+
+  std::ostream &operator<<(std::ostream &out, const AckermannControllerSettings &settings) {
+    out << "AckermannControllerSettings(speed_kp=" << std::to_string(settings.speed_kp)
+        << ", speed_ki=" << std::to_string(settings.speed_ki)
+        << ", speed_kd=" << std::to_string(settings.speed_kd)
+        << ", accel_kp=" << std::to_string(settings.accel_kp)
+        << ", accel_ki=" << std::to_string(settings.accel_ki)
+        << ", accel_kd=" << std::to_string(settings.accel_kd)  << ')';
+    return out;
+  }
+
 } // namespace rpc
 } // namespace carla
 
@@ -315,6 +336,42 @@ void export_control() {
     .def(self_ns::str(self_ns::self))
   ;
 
+  class_<cr::VehicleAckermannControl>("VehicleAckermannControl")
+    .def(init<float, float, float, float, float>(
+      (arg("steer") = 0.0f,
+      arg("steer_speed") = 0.0f,
+      arg("speed") = 0.0f,
+      arg("acceleration") = 0.0f,
+      arg("jerk") = 0.0f)))
+    .def_readwrite("steer", &cr::VehicleAckermannControl::steer)
+    .def_readwrite("steer_speed", &cr::VehicleAckermannControl::steer_speed)
+    .def_readwrite("speed", &cr::VehicleAckermannControl::speed)
+    .def_readwrite("acceleration", &cr::VehicleAckermannControl::acceleration)
+    .def_readwrite("jerk", &cr::VehicleAckermannControl::jerk)
+    .def("__eq__", &cr::VehicleAckermannControl::operator==)
+    .def("__ne__", &cr::VehicleAckermannControl::operator!=)
+    .def(self_ns::str(self_ns::self))
+  ;
+
+  class_<cr::AckermannControllerSettings>("AckermannControllerSettings")
+    .def(init<float, float, float, float, float, float>(
+      (arg("speed_kp") = 0.0f,
+      arg("speed_ki") = 0.0f,
+      arg("speed_kd") = 0.0f,
+      arg("accel_kp") = 0.0f,
+      arg("accel_ki") = 0.0f,
+      arg("accel_kd") = 0.0f)))
+    .def_readwrite("speed_kp", &cr::AckermannControllerSettings::speed_kp)
+    .def_readwrite("speed_ki", &cr::AckermannControllerSettings::speed_ki)
+    .def_readwrite("speed_kd", &cr::AckermannControllerSettings::speed_kd)
+    .def_readwrite("accel_kp", &cr::AckermannControllerSettings::accel_kp)
+    .def_readwrite("accel_ki", &cr::AckermannControllerSettings::accel_ki)
+    .def_readwrite("accel_kd", &cr::AckermannControllerSettings::accel_kd)
+    .def("__eq__", &cr::AckermannControllerSettings::operator==)
+    .def("__ne__", &cr::AckermannControllerSettings::operator!=)
+    .def(self_ns::str(self_ns::self))
+  ;
+
   class_<cr::WalkerControl>("WalkerControl")
     .def(init<cg::Vector3D, float, bool>(
        (arg("direction") = cg::Vector3D{1.0f, 0.0f, 0.0f},

PythonAPI/examples/manual_control.py

@@ -116,6 +116,7 @@ try:
     from pygame.locals import K_b
     from pygame.locals import K_c
     from pygame.locals import K_d
+    from pygame.locals import K_f
     from pygame.locals import K_g
     from pygame.locals import K_h
     from pygame.locals import K_i
@@ -275,6 +276,10 @@ class World(object):
                 sys.exit(1)
             spawn_points = self.map.get_spawn_points()
             spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
+            spawn_point = carla.Transform(
+                carla.Location(x=-250.0, y=245.0, z=2.0),
+                carla.Rotation()
+            )
             self.player = self.world.try_spawn_actor(blueprint, spawn_point)
             self.show_vehicle_telemetry = False
             self.modify_vehicle_physics(self.player)
@@ -370,8 +375,11 @@ class KeyboardControl(object):
     """Class that handles keyboard input."""
     def __init__(self, world, start_in_autopilot):
         self._autopilot_enabled = start_in_autopilot
+        self._ackermann_enabled = False
+        self._ackermann_reverse = 1
         if isinstance(world.player, carla.Vehicle):
             self._control = carla.VehicleControl()
+            self._ackermann_control = carla.VehicleAckermannControl()
             self._lights = carla.VehicleLightState.NONE
             world.player.set_autopilot(self._autopilot_enabled)
             world.player.set_light_state(self._lights)
@@ -500,8 +508,25 @@ class KeyboardControl(object):
                         world.recording_start += 1
                     world.hud.notification("Recording start time is %d" % (world.recording_start))
                 if isinstance(self._control, carla.VehicleControl):
+                    if event.key == K_f:
+                        world.player.apply_ackermann_controller_settings(
+                            carla.AckermannControllerSettings(
+                               speed_kp=0.15, speed_ki=0.0, speed_kd=0.25,
+                               accel_kp=0.01, accel_ki=0.0, accel_kd=0.01
+                            )
+                        )
+                        # Toggle ackermann controller
+                        self._ackermann_enabled = not self._ackermann_enabled
+                        world.hud.show_ackermann_info(self._ackermann_enabled)
+                        world.hud.notification("Ackermann Controller %s" %
+                                               ("Enabled" if self._ackermann_enabled else "Disabled"))
                     if event.key == K_q:
+                        if not self._ackermann_enabled:
                             self._control.gear = 1 if self._control.reverse else -1
+                        else:
+                            self._ackermann_reverse *= -1
+                            # Reset ackermann control
+                            self._ackermann_control = carla.VehicleAckermannControl()
                     elif event.key == K_m:
                         self._control.manual_gear_shift = not self._control.manual_gear_shift
                         self._control.gear = world.player.get_control().gear
@@ -563,19 +588,38 @@ class KeyboardControl(object):
                 if current_lights != self._lights: # Change the light state only if necessary
                     self._lights = current_lights
                     world.player.set_light_state(carla.VehicleLightState(self._lights))
+                # Apply control
+                if not self._ackermann_enabled:
+                    world.player.apply_control(self._control)
+                else:
+                    world.player.apply_ackermann_control(self._ackermann_control)
+                    # Update control to the last one applied by the ackermann controller.
+                    self._control = world.player.get_control()
+                    # Update hud with the newest ackermann control
+                    world.hud.update_ackermann_control(self._ackermann_control)
+
             elif isinstance(self._control, carla.WalkerControl):
                 self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world)
                 world.player.apply_control(self._control)
 
     def _parse_vehicle_keys(self, keys, milliseconds):
         if keys[K_UP] or keys[K_w]:
+            if not self._ackermann_enabled:
                 self._control.throttle = min(self._control.throttle + 0.01, 1.00)
             else:
+                self._ackermann_control.speed += round(milliseconds * 0.005, 2) * self._ackermann_reverse
+        else:
+            if not self._ackermann_enabled:
                 self._control.throttle = 0.0
 
         if keys[K_DOWN] or keys[K_s]:
+            if not self._ackermann_enabled:
                 self._control.brake = min(self._control.brake + 0.2, 1)
             else:
+                self._ackermann_control.speed -= min(abs(self._ackermann_control.speed), round(milliseconds * 0.005, 2)) * self._ackermann_reverse
+                self._ackermann_control.speed = max(0, abs(self._ackermann_control.speed)) * self._ackermann_reverse
+        else:
+            if not self._ackermann_enabled:
                 self._control.brake = 0
 
         steer_increment = 5e-4 * milliseconds
@@ -592,8 +636,11 @@ class KeyboardControl(object):
         else:
             self._steer_cache = 0.0
         self._steer_cache = min(0.7, max(-0.7, self._steer_cache))
+        if not self._ackermann_enabled:
             self._control.steer = round(self._steer_cache, 1)
             self._control.hand_brake = keys[K_SPACE]
+        else:
+            self._ackermann_control.steer = round(self._steer_cache, 1)
 
     def _parse_walker_keys(self, keys, milliseconds, world):
         self._control.speed = 0.0
@@ -640,6 +687,9 @@ class HUD(object):
         self._info_text = []
         self._server_clock = pygame.time.Clock()
 
+        self._show_ackermann_info = False
+        self._ackermann_control = carla.VehicleAckermannControl()
+
     def on_world_tick(self, timestamp):
         self._server_clock.tick()
         self.server_fps = self._server_clock.get_fps()
@@ -688,6 +738,15 @@ class HUD(object):
                 ('Hand brake:', c.hand_brake),
                 ('Manual:', c.manual_gear_shift),
                 'Gear:        %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)]
+            if self._show_ackermann_info:
+                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 += [
                 ('Speed:', c.speed, 0.0, 5.556),
@@ -708,6 +767,12 @@ class HUD(object):
                 vehicle_type = get_actor_display_name(vehicle, truncate=22)
                 self._info_text.append('% 4dm %s' % (d, vehicle_type))
 
+    def show_ackermann_info(self, enabled):
+        self._show_ackermann_info = enabled
+
+    def update_ackermann_control(self, ackermann_control):
+        self._ackermann_control = ackermann_control
+
     def toggle_info(self):
         self._show_info = not self._show_info
 

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp

@@ -102,7 +102,10 @@ void FVehicleData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* Carla
   {
     PhysicsControl = Vehicle->GetVehiclePhysicsControl();
   }
+  // TODO(joel): Record the state of the ackermann control
   Control = Vehicle->GetVehicleControl();
+  AckermannControl = Vehicle->GetVehicleAckermannControl();
+  bAckermannControlActive = Vehicle->IsAckermannControlActive();
   LightState = Vehicle->GetVehicleLightState();
   auto Controller = Cast<AWheeledVehicleAIController>(Vehicle->GetController());
   if (Controller)
@@ -121,7 +124,14 @@ void FVehicleData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* Carl
   {
     Vehicle->ApplyVehiclePhysicsControl(PhysicsControl);
   }
+  if (!bAckermannControlActive)
+  {
     Vehicle->ApplyVehicleControl(Control, EVehicleInputPriority::Client);
+  }
+  else 
+  {
+    Vehicle->ApplyVehicleAckermannControl(AckermannControl, EVehicleInputPriority::Client);
+  }
   Vehicle->SetVehicleLightState(LightState);
   auto Controller = Cast<AWheeledVehicleAIController>(Vehicle->GetController());
   if (Controller)

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h

@@ -7,6 +7,7 @@
 #pragma once
 
 #include "Math/DVector.h"
+#include "Carla/Vehicle/VehicleAckermannControl.h"
 #include "Carla/Vehicle/VehicleControl.h"
 #include "Carla/Vehicle/VehicleLightState.h"
 #include "Vehicle/VehicleInputPriority.h"
@@ -57,6 +58,10 @@ public:
 
   FVehicleControl Control;
 
+  FVehicleAckermannControl AckermannControl;
+
+  bool bAckermannControlActive = false;
+
   FVehicleLightState LightState;
 
   float SpeedLimit = 30;

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp

@@ -19,10 +19,12 @@
 #include "Carla/Game/CarlaStatics.h"
 
 #include <compiler/disable-ue4-macros.h>
+#include <carla/rpc/AckermannControllerSettings.h>
 #include "carla/rpc/LabelledPoint.h"
 #include <carla/rpc/LightState.h>
 #include <carla/rpc/MapInfo.h>
 #include <carla/rpc/MapLayer.h>
+#include <carla/rpc/VehicleAckermannControl.h>
 #include <carla/rpc/VehicleControl.h>
 #include <carla/rpc/VehiclePhysicsControl.h>
 #include <carla/rpc/VehicleLightState.h>
@@ -799,6 +801,7 @@ ECarlaServerResponse FVehicleActor::ApplyControlToVehicle(
   {
     FVehicleData* ActorData = GetActorData<FVehicleData>();
     ActorData->Control = Control;
+    ActorData->bAckermannControlActive = false;
   }
   else
   {
@@ -812,6 +815,27 @@ ECarlaServerResponse FVehicleActor::ApplyControlToVehicle(
   return ECarlaServerResponse::Success;
 }
 
+ECarlaServerResponse FVehicleActor::ApplyAckermannControlToVehicle(
+      const FVehicleAckermannControl& AckermannControl, const EVehicleInputPriority& Priority)
+{
+  if (IsDormant())
+  {
+    FVehicleData* ActorData = GetActorData<FVehicleData>();
+    ActorData->AckermannControl = AckermannControl;
+    ActorData->bAckermannControlActive = true;
+  }
+  else
+  {
+    auto Vehicle = Cast<ACarlaWheeledVehicle>(GetActor());
+    if (Vehicle == nullptr)
+    {
+      return ECarlaServerResponse::NotAVehicle;
+    }
+    Vehicle->ApplyVehicleAckermannControl(AckermannControl, Priority);
+  }
+  return ECarlaServerResponse::Success;
+}
+
 ECarlaServerResponse FVehicleActor::GetVehicleControl(FVehicleControl& VehicleControl)
 {
   if (IsDormant())
@@ -831,6 +855,66 @@ ECarlaServerResponse FVehicleActor::GetVehicleControl(FVehicleControl& VehicleCo
   return ECarlaServerResponse::Success;
 }
 
+ECarlaServerResponse FVehicleActor::GetVehicleAckermannControl(FVehicleAckermannControl& VehicleAckermannControl)
+{
+  if (IsDormant())
+  {
+    FVehicleData* ActorData = GetActorData<FVehicleData>();
+    VehicleAckermannControl = ActorData->AckermannControl;
+  }
+  else
+  {
+    auto Vehicle = Cast<ACarlaWheeledVehicle>(GetActor());
+    if (Vehicle == nullptr)
+    {
+      return ECarlaServerResponse::NotAVehicle;
+    }
+    VehicleAckermannControl = Vehicle->GetVehicleAckermannControl();
+  }
+  return ECarlaServerResponse::Success;
+}
+
+ECarlaServerResponse FVehicleActor::GetAckermannControllerSettings(
+  FAckermannControllerSettings& AckermannSettings)
+{
+  if (IsDormant())
+  {
+    //FVehicleData* ActorData = GetActorData<FVehicleData>();
+    //PhysicsControl = ActorData->PhysicsControl;
+  }
+  else
+  {
+    auto Vehicle = Cast<ACarlaWheeledVehicle>(GetActor());
+    if (Vehicle == nullptr)
+    {
+      return ECarlaServerResponse::NotAVehicle;
+    }
+    AckermannSettings = Vehicle->GetAckermannControllerSettings();
+  }
+  return ECarlaServerResponse::Success;
+}
+
+ECarlaServerResponse FVehicleActor::ApplyAckermannControllerSettings(
+      const FAckermannControllerSettings& AckermannSettings)
+{
+  if (IsDormant())
+  {
+    // FVehicleData* ActorData = GetActorData<FVehicleData>();
+    // ActorData->PhysicsControl = PhysicsControl;
+  }
+  else
+  {
+    auto Vehicle = Cast<ACarlaWheeledVehicle>(GetActor());
+    if (Vehicle == nullptr)
+    {
+      return ECarlaServerResponse::NotAVehicle;
+    }
+
+    Vehicle->ApplyAckermannControllerSettings(AckermannSettings);
+  }
+  return ECarlaServerResponse::Success;
+}
+
 ECarlaServerResponse FVehicleActor::SetActorAutopilot(bool bEnabled, bool bKeepState)
 {
   if (IsDormant())

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h

@@ -284,11 +284,32 @@ public:
     return ECarlaServerResponse::ActorTypeMismatch;
   }
 
+  virtual ECarlaServerResponse ApplyAckermannControlToVehicle(
+      const FVehicleAckermannControl&, const EVehicleInputPriority&)
+  {
+    return ECarlaServerResponse::ActorTypeMismatch;
+  }
+
   virtual ECarlaServerResponse GetVehicleControl(FVehicleControl&)
   {
     return ECarlaServerResponse::ActorTypeMismatch;
   }
 
+  virtual ECarlaServerResponse GetVehicleAckermannControl(FVehicleAckermannControl&)
+  {
+    return ECarlaServerResponse::ActorTypeMismatch;
+  }
+
+  virtual ECarlaServerResponse GetAckermannControllerSettings(FAckermannControllerSettings&)
+  {
+    return ECarlaServerResponse::ActorTypeMismatch;
+  }
+
+  virtual ECarlaServerResponse ApplyAckermannControllerSettings(const FAckermannControllerSettings&)
+  {
+    return ECarlaServerResponse::ActorTypeMismatch;
+  }
+
   virtual ECarlaServerResponse SetActorAutopilot(bool, bool bKeepState = false)
   {
     return ECarlaServerResponse::ActorTypeMismatch;
@@ -474,8 +495,17 @@ public:
   virtual ECarlaServerResponse ApplyControlToVehicle(
       const FVehicleControl&, const EVehicleInputPriority&) final;
 
+  virtual ECarlaServerResponse ApplyAckermannControlToVehicle(
+      const FVehicleAckermannControl&, const EVehicleInputPriority&) final;
+
   virtual ECarlaServerResponse GetVehicleControl(FVehicleControl&) final;
 
+  virtual ECarlaServerResponse GetVehicleAckermannControl(FVehicleAckermannControl&) final;
+
+  virtual ECarlaServerResponse GetAckermannControllerSettings(FAckermannControllerSettings&) final;
+
+  virtual ECarlaServerResponse ApplyAckermannControllerSettings(const FAckermannControllerSettings&) final;
+
   virtual ECarlaServerResponse SetActorAutopilot(bool bEnabled, bool bKeepState = false) final;
 
   virtual ECarlaServerResponse ShowVehicleDebugTelemetry(bool bEnabled) final;

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

@@ -29,6 +29,7 @@
 #include <compiler/disable-ue4-macros.h>
 #include <carla/Functional.h>
 #include <carla/Version.h>
+#include <carla/rpc/AckermannControllerSettings.h>
 #include <carla/rpc/Actor.h>
 #include <carla/rpc/ActorDefinition.h>
 #include <carla/rpc/ActorDescription.h>
@@ -50,6 +51,7 @@
 #include <carla/rpc/Vector2D.h>
 #include <carla/rpc/Vector3D.h>
 #include <carla/rpc/VehicleDoor.h>
+#include <carla/rpc/VehicleAckermannControl.h>
 #include <carla/rpc/VehicleControl.h>
 #include <carla/rpc/VehiclePhysicsControl.h>
 #include <carla/rpc/VehicleLightState.h>
@@ -1360,6 +1362,81 @@ void FCarlaServer::FPimpl::BindActions()
     return R<void>::Success();
   };
 
+  BIND_SYNC(apply_ackermann_control_to_vehicle) << [this](
+      cr::ActorId ActorId,
+      cr::VehicleAckermannControl Control) -> R<void>
+  {
+    REQUIRE_CARLA_EPISODE();
+    FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
+    if (!CarlaActor)
+    {
+      return RespondError(
+          "apply_ackermann_control_to_vehicle",
+          ECarlaServerResponse::ActorNotFound,
+          " Actor Id: " + FString::FromInt(ActorId));
+    }
+    ECarlaServerResponse Response =
+        CarlaActor->ApplyAckermannControlToVehicle(Control, EVehicleInputPriority::Client);
+    if (Response != ECarlaServerResponse::Success)
+    {
+      return RespondError(
+          "apply_ackermann_control_to_vehicle",
+          Response,
+          " Actor Id: " + FString::FromInt(ActorId));
+    }
+    return R<void>::Success();
+  };
+
+  BIND_SYNC(get_ackermann_controller_settings) << [this](
+      cr::ActorId ActorId) -> R<cr::AckermannControllerSettings>
+  {
+    REQUIRE_CARLA_EPISODE();
+    FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
+        if (!CarlaActor)
+    {
+      return RespondError(
+          "get_ackermann_controller_settings",
+          ECarlaServerResponse::ActorNotFound,
+          " Actor Id: " + FString::FromInt(ActorId));
+    }
+    FAckermannControllerSettings Settings;
+    ECarlaServerResponse Response =
+        CarlaActor->GetAckermannControllerSettings(Settings);
+    if (Response != ECarlaServerResponse::Success)
+    {
+      return RespondError(
+          "get_ackermann_controller_settings",
+          Response,
+          " Actor Id: " + FString::FromInt(ActorId));
+    }
+    return cr::AckermannControllerSettings(Settings);
+  };
+
+  BIND_SYNC(apply_ackermann_controller_settings) << [this](
+      cr::ActorId ActorId,
+      cr::AckermannControllerSettings AckermannSettings) -> R<void>
+  {
+    REQUIRE_CARLA_EPISODE();
+    FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
+    if (!CarlaActor)
+    {
+      return RespondError(
+          "apply_ackermann_controller_settings",
+          ECarlaServerResponse::ActorNotFound,
+          " Actor Id: " + FString::FromInt(ActorId));
+    }
+    ECarlaServerResponse Response =
+        CarlaActor->ApplyAckermannControllerSettings(FAckermannControllerSettings(AckermannSettings));
+    if (Response != ECarlaServerResponse::Success)
+    {
+      return RespondError(
+          "apply_ackermann_controller_settings",
+          Response,
+          " Actor Id: " + FString::FromInt(ActorId));
+    }
+    return R<void>::Success();
+  };
+
   BIND_SYNC(apply_control_to_walker) << [this](
       cr::ActorId ActorId,
       cr::WalkerControl Control) -> R<void>
@@ -2050,6 +2127,7 @@ void FCarlaServer::FPimpl::BindActions()
       },
       [=](auto, const C::DestroyActor &c) {         MAKE_RESULT(destroy_actor(c.actor)); },
       [=](auto, const C::ApplyVehicleControl &c) {  MAKE_RESULT(apply_control_to_vehicle(c.actor, c.control)); },
+      [=](auto, const C::ApplyVehicleAckermannControl &c) {  MAKE_RESULT(apply_ackermann_control_to_vehicle(c.actor, c.control)); },
       [=](auto, const C::ApplyWalkerControl &c) {   MAKE_RESULT(apply_control_to_walker(c.actor, c.control)); },
       [=](auto, const C::ApplyVehiclePhysicsControl &c) {  MAKE_RESULT(apply_physics_control(c.actor, c.physics_control)); },
       [=](auto, const C::ApplyTransform &c) {       MAKE_RESULT(set_actor_transform(c.actor, c.transform)); },

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

@@ -0,0 +1,305 @@
+// Copyright (c) 2021 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 "AckermannController.h"
+#include "CarlaWheeledVehicle.h"
+
+// =============================================================================
+// -- Constructor and destructor -----------------------------------------------
+// =============================================================================
+
+FAckermannController::~FAckermannController() {}
+
+// =============================================================================
+// -- FAckermannController --------------------------------------------------------------
+// =============================================================================
+
+FAckermannControllerSettings FAckermannController::GetSettings() const {
+  FAckermannControllerSettings Settings;
+
+  Settings.SpeedKp = SpeedController.Kp;
+  Settings.SpeedKi = SpeedController.Ki;
+  Settings.SpeedKd = SpeedController.Kd;
+
+  Settings.AccelKp = AccelerationController.Kp;
+  Settings.AccelKi = AccelerationController.Ki;
+  Settings.AccelKd = AccelerationController.Kd;
+
+  return Settings;
+}
+
+void FAckermannController::ApplySettings(const FAckermannControllerSettings& Settings) {
+  SpeedController.Kp = Settings.SpeedKp;
+  SpeedController.Ki = Settings.SpeedKi;
+  SpeedController.Kd = Settings.SpeedKd;
+  
+  AccelerationController.Kp = Settings.AccelKp;
+  AccelerationController.Ki = Settings.AccelKi;
+  AccelerationController.Kd = Settings.AccelKd;
+}
+
+void FAckermannController::SetTargetPoint(const FVehicleAckermannControl& AckermannControl) {
+  UserTargetPoint = AckermannControl;
+
+  TargetSteer = UserTargetPoint.Steer;
+  TargetSpeed = UserTargetPoint.Speed;
+  TargetAcceleration = UserTargetPoint.Acceleration;
+  TargetJerk = UserTargetPoint.Jerk;
+
+}
+
+void FAckermannController::Reset() {
+  // Reset controllers
+  SpeedController.Reset();
+  AccelerationController.Reset();
+
+  // Reset control parameters
+  Steer = 0.0f;
+
+  //SpeedControlActivation = 0;
+
+  SpeedControlAccelDelta = 0.0f;
+  SpeedControlAccelTarget = 0.0f;
+
+  AccelControlPedalDelta = 0.0f;
+  AccelControlPedalTarget = 0.0f;
+
+  //ThrottleLowerBorder = 0.0f;
+  //BrakeUpperBorder = 0.0f;
+
+  // Reset vehicle state
+  VehicleSpeed = 0.0f;
+  VehicleAcceleration = 0.0f;
+  //VehiclePitch = 0.0f;
+
+  LastVehicleSpeed = 0.0f;
+  LastVehicleAcceleration = 0.0f;
+  //LastVehiclePitch = 0.0f;
+}
+
+void FAckermannController::RunLoop(FVehicleControl& Control) {
+  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
+
+  // Lateral Control
+  RunControlSteering();
+
+  // Longitudinal Control
+  bool bStopped = RunControlFullStop();
+  if (!bStopped) {
+    RunControlReverse();
+    RunControlSpeed();
+    RunControlAcceleration();
+    UpdateVehicleControlCommand();
+  }
+
+  // Update control command
+  Control.Steer = Steer;
+  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() {
+  Steer = TargetSteer / VehicleMaxSteering;
+}
+
+bool FAckermannController::RunControlFullStop() {
+  // From this velocity on full brake is turned on
+  float FullStopEpsilon = 0.1; //[m/s]
+
+  if (FMath::Abs(VehicleSpeed) < FullStopEpsilon && FMath::Abs(UserTargetPoint.Speed) < FullStopEpsilon) {
+    Brake = 1.0;
+    Throttle = 0.0;
+    return true;
+  }
+  return false;  
+}
+
+void FAckermannController::RunControlReverse() {
+  // From this position on it is allowed to switch to reverse gear
+  float StandingStillEpsilon = 0.1;  // [m/s]
+
+  if (FMath::Abs(VehicleSpeed) < StandingStillEpsilon) {
+    // Standing still, change of driving direction allowed
+    if (UserTargetPoint.Speed < 0) {
+      // Change of driving direction to reverse.
+      bReverse = true;
+    } else if (UserTargetPoint.Speed >= 0) {
+      // Change of driving direction to forward.
+      bReverse = false;
+    }
+  }
+
+  if (FMath::Sign(VehicleSpeed) * FMath::Sign(UserTargetPoint.Speed) == -1) {
+    // Requested for change of driving direction.
+    // First we have to come to full stop before changing driving direction
+    UE_LOG(LogCarla, Log, TEXT("Requested change of driving direction. First going to full stop"));
+    TargetSpeed = 0.0;
+  }
+}
+
+
+void FAckermannController::RunControlSpeed() {
+
+  // TODO(joel): Minimum acceleration 1.0f
+  //if (FMath::Abs(TargetAcceleration) < 1.0f) {
+  //  SpeedControlActivation = (SpeedControlActivation < 5) ? SpeedControlActivation + 1 : SpeedControlActivation;
+  //}  else {
+  //  SpeedControlActivation = (SpeedControlActivation > 0) ? SpeedControlActivation - 1 : SpeedControlActivation; 
+  //}
+
+  //if (SpeedControlActivation >= 5) {
+      //SpeedController.SetTargetPoint(FMath::Abs(TargetSpeed));
+      //SpeedControlAccelDelta = SpeedController.Run(FMath::Abs(VehicleSpeed), DeltaTime);
+      SpeedController.SetTargetPoint(TargetSpeed);
+      SpeedControlAccelDelta = SpeedController.Run(VehicleSpeed, DeltaTime);
+
+      // Clipping borders
+      float ClippingLowerBorder = -FMath::Abs(TargetAcceleration);
+      float ClippingUpperBorder = FMath::Abs(TargetAcceleration);
+      if (FMath::Abs(TargetAcceleration) < 0.0001f) {
+        // Per definition of AckermannDrive: if zero, then use max value
+        ClippingLowerBorder = -MaxDecel;
+        ClippingUpperBorder = MaxAccel;
+      }
+      SpeedControlAccelTarget += SpeedControlAccelDelta;
+      SpeedControlAccelTarget = FMath::Clamp(SpeedControlAccelTarget,
+          ClippingLowerBorder, ClippingUpperBorder);
+
+  //} else {
+  //  SpeedControlAccelDelta = 0.0f;
+  //  SpeedControlAccelTarget = TargetAcceleration;
+  //}
+}
+
+void FAckermannController::RunControlAcceleration() {
+  AccelerationController.SetTargetPoint(SpeedControlAccelTarget);
+  AccelControlPedalDelta = AccelerationController.Run(VehicleAcceleration, DeltaTime);
+
+  // Clipping borders
+  AccelControlPedalTarget += AccelControlPedalDelta;
+  //AccelControlPedalTarget = FMath::Clamp(AccelControlPedalTarget, -MaxPedal, MaxPedal);
+  AccelControlPedalTarget = FMath::Clamp(AccelControlPedalTarget, -1.0f, 1.0f);
+
+}
+
+void FAckermannController::UpdateVehicleControlCommand() {
+
+    if (AccelControlPedalTarget < 0.0f) {
+      if (bReverse) {
+        Throttle = FMath::Abs(AccelControlPedalTarget);
+        Brake = 0.0f;
+      } else {
+        Throttle = 0.0f;
+        Brake = FMath::Abs(AccelControlPedalTarget);
+      }
+    } else {
+      if (bReverse) {
+        Throttle = 0.0f;
+        Brake = FMath::Abs(AccelControlPedalTarget);
+      } else {
+        Throttle = FMath::Abs(AccelControlPedalTarget);
+        Brake = 0.0f;
+      }
+    }
+
+    // ThrottleLowerBorder = GetVehicleDrivingImpedanceAcceleration();
+    // BrakeUpperBorder = ThrottleLowerBorder + GetVehicleLayOffEngineAcceleration();
+    
+    // if (AccelControlPedalTarget > ThrottleLowerBorder) {
+    //     // Accelerating
+    //     Throttle = (AccelControlPedalTarget - ThrottleLowerBorder) / MaxPedal;
+    //     Brake = 0.0f;
+
+    // } else if (AccelControlPedalTarget > BrakeUpperBorder) {
+    //     // Coasting
+    //     Throttle = 0.0f;
+    //     Brake = 0.0f;
+    // } else {
+    //     // Braking
+    //     Throttle = 0.0f;
+    //     Brake = (BrakeUpperBorder - AccelControlPedalTarget) / MaxPedal;
+    // }
+}
+
+// float FAckermannController::GetVehicleLayOffEngineAcceleration() {
+//     float EngineBrakeForce = 500.0f;  // [N]
+//     return -EngineBrakeForce / VehicleMass;
+// }
+
+// float FAckermannController::GetVehicleDrivingImpedanceAcceleration() {
+//     float RollingResistanceForce = 0.01 * 9.81f * VehicleMass;
+//     float AirDensity = 1.25f;
+//     float AerodynamicDragForce = 0.5f * AirDensity * VehicleDragCoefficient * VehicleDragArea * (VehicleSpeed * VehicleSpeed);
+//     //float SlopeForce = 9.81f * VehicleMass * FMath::Sin(VehiclePitch);
+//     float SlopeForce = 0.0f;
+
+//     return -(RollingResistanceForce + AerodynamicDragForce + SlopeForce) / VehicleMass;
+// }
+
+void FAckermannController::UpdateVehicleState(const ACarlaWheeledVehicle* Vehicle) {
+  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
+
+  LastVehicleSpeed = VehicleSpeed;
+  LastVehicleAcceleration = VehicleAcceleration;
+  //LastVehiclePitch = VehiclePitch;
+
+  // Update simulation state
+  DeltaTime = Vehicle->GetWorld()->GetDeltaSeconds();
+
+  // Update Vehicle state
+  //VehicleSpeed = FMath::RoundToFloat((Vehicle->GetVehicleForwardSpeed() / 100.0f) * 10.0f) / 10.0f;  // From cm/s to m/s
+  VehicleSpeed = Vehicle->GetVehicleForwardSpeed() / 100.0f;  // From cm/s to m/s
+  //VehicleSpeed = (4.0f*LastVehicleSpeed + CurrentSpeed) / 5.0f;
+  
+  float CurrentAcceleration = (VehicleSpeed - LastVehicleSpeed) / DeltaTime;
+  //CurrentAcceleration = FMath::RoundToFloat(CurrentAcceleration * 10.0f) / 10.0f;
+  VehicleAcceleration = (4.0f*LastVehicleAcceleration + CurrentAcceleration) / 5.0f;
+  //VehiclePitch = FMath::DegreesToRadians(
+  //  Vehicle->GetVehicleTransform().GetRotation().Rotator().Pitch
+  //);
+}
+
+void FAckermannController::UpdateVehiclePhysics(const ACarlaWheeledVehicle* Vehicle) {
+  UWheeledVehicleMovementComponent4W *Vehicle4W = Cast<UWheeledVehicleMovementComponent4W>(
+      Vehicle->GetVehicleMovement());
+  check(Vehicle4W != nullptr);
+
+  // Update vehicle physics
+  //VehicleMass = Vehicle4W->Mass;
+  //VehicleDragArea = Vehicle4W->DragArea / (100.0f * 100.0f);  // From cm2 to m2
+  //VehicleDragCoefficient = Vehicle4W->DragCoefficient;
+  VehicleMaxSteering = FMath::DegreesToRadians(Vehicle->GetMaximumSteerAngle());
+}

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

@@ -0,0 +1,164 @@
+// Copyright (c) 2021 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 "Vehicle/AckermannControllerSettings.h"
+#include "Vehicle/VehicleAckermannControl.h"
+#include "Vehicle/VehicleControl.h"
+
+// Forward declaration
+class ACarlaWheeledVehicle;
+
+class PID
+{
+  public:
+    PID() = default;
+    PID(float Kp, float Ki, float Kd): Kp(Kp), Ki(Ki), Kd(Kd) {}
+    ~PID() = default;
+
+    void SetTargetPoint(float Point)
+    {
+      SetPoint = Point;
+    }
+
+    float Run(float Input, float DeltaTime)
+    {
+      float Error = SetPoint - Input;
+
+      Proportional = Kp * Error;
+      Integral += Ki * Error * DeltaTime;
+      // Avoid integral windup
+      Integral = FMath::Clamp(Integral, MinOutput, MaxOutput);
+      Derivative = (-Kd * (Input - LastInput)) / DeltaTime;
+
+      float Out = Proportional + Integral + Derivative;
+      Out = FMath::Clamp(Out, MinOutput, MaxOutput);
+
+      // Keep track of the state
+      LastError = Out;
+      LastInput = Input;
+
+      return Out;
+    }
+
+  void Reset()
+  {
+    Proportional = 0.0f;
+    Integral = 0.0f;
+    Integral = FMath::Clamp(Integral, MinOutput, MaxOutput);
+    Derivative = 0.0f;
+    
+    LastError = 0.0f;
+    LastInput = 0.0f;
+  }
+
+  public:
+    float Kp = 0.0f;
+    float Ki = 0.0f;
+    float Kd = 0.0f;
+
+  private:
+    float SetPoint;
+
+    // Out Limits
+    float MinOutput = -1.0f;
+    float MaxOutput = 1.0f;
+
+    // Internal state.
+    float Proportional = 0.0f;
+    float Integral = 0.0f;
+    float Derivative = 0.0f;
+
+    float LastError = 0.0f;
+    float LastInput = 0.0f;
+};
+
+
+class FAckermannController
+{
+public:
+
+  FAckermannController() = default;
+  ~FAckermannController();
+
+public:
+
+  FAckermannControllerSettings GetSettings() const;
+  void ApplySettings(const FAckermannControllerSettings& Settings);
+
+  void UpdateVehicleState(const ACarlaWheeledVehicle* Vehicle);
+  void UpdateVehiclePhysics(const ACarlaWheeledVehicle* Vehicle);
+
+  void SetTargetPoint(const FVehicleAckermannControl& AckermannControl);
+  void Reset();
+
+  void RunLoop(FVehicleControl& Control);
+
+private:
+
+  // Lateral Control
+  void RunControlSteering();
+
+  // Longitudinal Control
+  bool RunControlFullStop();
+  void RunControlReverse();
+  void RunControlSpeed();
+  void RunControlAcceleration();
+  void UpdateVehicleControlCommand();
+
+  //float GetVehicleDrivingImpedanceAcceleration();
+  //float GetVehicleLayOffEngineAcceleration();
+
+private:
+
+  PID SpeedController = PID(0.15f, 0.0f, 0.25f);
+  PID AccelerationController = PID(0.01f, 0.0f, 0.01f);
+
+  FVehicleAckermannControl UserTargetPoint;
+
+  // Target point after aplying restrictions
+  float TargetSteer = 0.0;
+  float TargetSpeed = 0.0;
+  float TargetAcceleration = 0.0;
+  float TargetJerk = 0.0;
+
+  // Restricitions parameters
+  float MaxAccel = 3.0f;  // [m/s2]
+  float MaxDecel = 8.0f;  // [m/s2]
+
+  //float MaxPedal = 3.0f;  // min(MaxAccel, MaxDecel)
+
+  // Control parameters
+  float Steer = 0.0f;
+  float Throttle = 0.0f;
+  float Brake = 0.0f;
+  bool bReverse = false;
+
+  // Internal control parameters
+  float SpeedControlAccelDelta = 0.0f;
+  float SpeedControlAccelTarget = 0.0f;
+  
+  float AccelControlPedalDelta = 0.0f;
+  float AccelControlPedalTarget = 0.0f;
+
+  // Simulation state
+  float DeltaTime = 0.0f;                // [s]
+
+  // Vehicle state
+  //float VehicleMass = 0.0f;              // [Kg]
+  float VehicleMaxSteering = 0.0f;       // [rad]
+  //float VehicleDragArea = 0.0f;          // [m2]
+  //float VehicleDragCoefficient = 0.0f;    // [?]
+
+  float VehicleSpeed = 0.0f;             // [m/s]
+  float VehicleAcceleration = 0.0f;      // [m/s2]
+  //float VehiclePitch = 0.0f;             // [rad]
+
+  float LastVehicleSpeed = 0.0f;         // [m/s]
+  float LastVehicleAcceleration = 0.0f;  // [m/s2]
+  //float LastVehiclePitch = 0.0f;         // [rad]
+};

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

@@ -0,0 +1,33 @@
+// Copyright (c) 2021 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 "AckermannControllerSettings.generated.h"
+
+USTRUCT(BlueprintType)
+struct CARLA_API FAckermannControllerSettings
+{
+  GENERATED_BODY()
+
+  UPROPERTY(Category = "Ackermann Controller Settings", EditAnywhere, BlueprintReadWrite)
+  float SpeedKp = 0.0f;
+
+  UPROPERTY(Category = "Ackermann Controller Settings", EditAnywhere, BlueprintReadWrite)
+  float SpeedKi = 0.0f;
+
+  UPROPERTY(Category = "Ackermann Controller Settings", EditAnywhere, BlueprintReadWrite)
+  float SpeedKd = 0.0f;
+
+  UPROPERTY(Category = "Ackermann Controller Settings", EditAnywhere, BlueprintReadWrite)
+  float AccelKp = 0.0f;
+
+  UPROPERTY(Category = "Ackermann Controller Settings", EditAnywhere, BlueprintReadWrite)
+  float AccelKi = 0.0f;
+
+  UPROPERTY(Category = "Ackermann Controller Settings", EditAnywhere, BlueprintReadWrite)
+  float AccelKd = 0.0f;
+};

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

@@ -98,6 +98,8 @@ void ACarlaWheeledVehicle::BeginPlay()
 {
   Super::BeginPlay();
 
+  UE_LOG(LogCarla, Log, TEXT("[AckermannLog];Frame;DeltaSeconds;TargetSteer;TargetSpeed;TargetAcceleration;TargetJerk;CurrentSpeed;CurrentAcceleration;CurrentPitch;Steer;Throttle;Brake;Gear;SpeedP;SpeedI;SpeedD;SpeedControlAccelDelta;SpeedControlAccelTarget;AccelP;AccelI;AccelD;AccelControlPedalDelta;AccelControlPedalTarget;BrakeUpperBorder;ThrottleLowerBorder"));
+
   UDefaultMovementComponent::CreateDefaultMovementComponent(this);
 
   // Get constraint components and their initial transforms
@@ -261,6 +263,11 @@ float ACarlaWheeledVehicle::GetMaximumSteerAngle() const
 
 void ACarlaWheeledVehicle::FlushVehicleControl()
 {
+  if (bAckermannControlActive) {
+    AckermannController.UpdateVehicleState(this);
+    AckermannController.RunLoop(InputControl.Control);
+  }
+
   BaseMovementComponent->ProcessControl(InputControl.Control);
   InputControl.Control.bReverse = InputControl.Control.Gear < 0;
   LastAppliedControl = InputControl.Control;
@@ -721,6 +728,9 @@ void ACarlaWheeledVehicle::ApplyVehiclePhysicsControl(const FVehiclePhysicsContr
   {
     Recorder->AddPhysicsControl(*this);
   }
+  
+  // Update physics in the Ackermann Controller
+  AckermannController.UpdateVehiclePhysics(this);
 }
 
 void ACarlaWheeledVehicle::ActivateVelocityControl(const FVector &Velocity)

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

@@ -8,7 +8,10 @@
 
 #include "WheeledVehicle.h"
 
+#include "Vehicle/AckermannController.h"
+#include "Vehicle/AckermannControllerSettings.h"
 #include "Vehicle/CarlaWheeledVehicleState.h"
+#include "Vehicle/VehicleAckermannControl.h"
 #include "Vehicle/VehicleControl.h"
 #include "Vehicle/VehicleLightState.h"
 #include "Vehicle/VehicleInputPriority.h"
@@ -87,6 +90,13 @@ public:
     return LastAppliedControl;
   }
 
+  /// Vehicle Ackermann control currently applied to this vehicle.
+  UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
+  const FVehicleAckermannControl &GetVehicleAckermannControl() const
+  {
+    return LastAppliedAckermannControl;
+  }
+
   /// Transform of the vehicle. Location is shifted so it matches center of the
   /// vehicle bounds rather than the actor's location.
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
@@ -148,6 +158,11 @@ public:
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   FVehiclePhysicsControl GetVehiclePhysicsControl() const;
 
+  UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
+  FAckermannControllerSettings GetAckermannControllerSettings() const {
+    return AckermannController.GetSettings();
+  }
+
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   void RestoreVehiclePhysicsControl();
 
@@ -156,6 +171,10 @@ public:
 
   void ApplyVehiclePhysicsControl(const FVehiclePhysicsControl &PhysicsControl);
 
+  void ApplyAckermannControllerSettings(const FAckermannControllerSettings &AckermannControllerSettings) {
+    return AckermannController.ApplySettings(AckermannControllerSettings);
+  }
+
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   void SetSimulatePhysics(bool enabled);
 
@@ -181,6 +200,11 @@ public:
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   void ApplyVehicleControl(const FVehicleControl &Control, EVehicleInputPriority Priority)
   {
+    if (bAckermannControlActive) {
+      AckermannController.Reset();
+    }
+    bAckermannControlActive = false;
+
     if (InputControl.Priority <= Priority)
     {
       InputControl.Control = Control;
@@ -188,6 +212,19 @@ public:
     }
   }
 
+  UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
+  void ApplyVehicleAckermannControl(const FVehicleAckermannControl &AckermannControl, EVehicleInputPriority Priority)
+  {
+    bAckermannControlActive = true;
+    LastAppliedAckermannControl = AckermannControl;
+    AckermannController.SetTargetPoint(AckermannControl);
+  }
+
+  bool IsAckermannControlActive() const
+  {
+    return bAckermannControlActive;
+  }
+
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   void ActivateVelocityControl(const FVector &Velocity);
 
@@ -299,8 +336,12 @@ private:
   InputControl;
 
   FVehicleControl LastAppliedControl;
+  FVehicleAckermannControl LastAppliedAckermannControl;
   FVehiclePhysicsControl LastPhysicsControl;
 
+  bool bAckermannControlActive = false;
+  FAckermannController AckermannController;
+
 public:
 
   /// Set the rotation of the car wheels indicated by the user

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

@@ -0,0 +1,31 @@
+// Copyright (c) 2021 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 "VehicleAckermannControl.generated.h"
+
+USTRUCT(BlueprintType)
+struct CARLA_API FVehicleAckermannControl
+{
+  GENERATED_BODY()
+
+  UPROPERTY(Category = "Vehicle Ackermann Control", EditAnywhere, BlueprintReadWrite)
+  float Steer = 0.0f;
+
+  UPROPERTY(Category = "Vehicle Ackermann Control", EditAnywhere, BlueprintReadWrite)
+  float SteerSpeed = 0.0f;
+
+  UPROPERTY(Category = "Vehicle Ackermann Control", EditAnywhere, BlueprintReadWrite)
+  float Speed = 0.0f;
+
+  UPROPERTY(Category = "Vehicle Ackermann Control", EditAnywhere, BlueprintReadWrite)
+  float Acceleration = 0.0f;
+
+  UPROPERTY(Category = "Vehicle Ackermann Control", EditAnywhere, BlueprintReadWrite)
+  float Jerk = 0.0f;
+};
+