From 8cc6f132fc0b27d3bb4b786b42de2d48a1e77317 Mon Sep 17 00:00:00 2001 From: Joel Moriana Date: Mon, 7 Feb 2022 12:39:38 +0100 Subject: [PATCH] First iteration ackermann controller --- LibCarla/source/carla/client/Vehicle.cpp | 12 + LibCarla/source/carla/client/Vehicle.h | 9 + .../source/carla/client/detail/Client.cpp | 15 + LibCarla/source/carla/client/detail/Client.h | 12 + .../source/carla/client/detail/Simulator.h | 12 + .../carla/rpc/AckermannControllerSettings.h | 92 ++++++ LibCarla/source/carla/rpc/Command.h | 12 + .../carla/rpc/VehicleAckermannControl.h | 84 +++++ PythonAPI/carla/source/libcarla/Actor.cpp | 3 + PythonAPI/carla/source/libcarla/Commands.cpp | 8 + PythonAPI/carla/source/libcarla/Control.cpp | 57 ++++ PythonAPI/examples/manual_control.py | 81 ++++- .../Carla/Source/Carla/Actor/ActorData.cpp | 12 +- .../Carla/Source/Carla/Actor/ActorData.h | 5 + .../Carla/Source/Carla/Actor/CarlaActor.cpp | 84 +++++ .../Carla/Source/Carla/Actor/CarlaActor.h | 30 ++ .../Carla/Source/Carla/Server/CarlaServer.cpp | 78 +++++ .../Carla/Vehicle/AcekrmannController.cpp | 305 ++++++++++++++++++ .../Carla/Vehicle/AckermannController.h | 164 ++++++++++ .../Vehicle/AckermannControllerSettings.h | 33 ++ .../Carla/Vehicle/CarlaWheeledVehicle.cpp | 10 + .../Carla/Vehicle/CarlaWheeledVehicle.h | 41 +++ .../Carla/Vehicle/VehicleAckermannControl.h | 31 ++ 23 files changed, 1181 insertions(+), 9 deletions(-) create mode 100644 LibCarla/source/carla/rpc/AckermannControllerSettings.h create mode 100644 LibCarla/source/carla/rpc/VehicleAckermannControl.h create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AcekrmannController.cpp create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannController.h create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannControllerSettings.h create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleAckermannControl.h diff --git a/LibCarla/source/carla/client/Vehicle.cpp b/LibCarla/source/carla/client/Vehicle.cpp index b944c3176..70950fd2c 100644 --- a/LibCarla/source/carla/client/Vehicle.cpp +++ b/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); } diff --git a/LibCarla/source/carla/client/Vehicle.h b/LibCarla/source/carla/client/Vehicle.h index 973facb83..fedb42c4e 100644 --- a/LibCarla/source/carla/client/Vehicle.h +++ b/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); diff --git a/LibCarla/source/carla/client/detail/Client.cpp b/LibCarla/source/carla/client/detail/Client.cpp index 91b97bbc8..690f6a69a 100644 --- a/LibCarla/source/carla/client/detail/Client.cpp +++ b/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("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); } diff --git a/LibCarla/source/carla/client/detail/Client.h b/LibCarla/source/carla/client/detail/Client.h index 4ea2c7633..ccc454fa4 100644 --- a/LibCarla/source/carla/client/detail/Client.h +++ b/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); diff --git a/LibCarla/source/carla/client/detail/Simulator.h b/LibCarla/source/carla/client/detail/Simulator.h index 33a06aa9a..d292f36fc 100644 --- a/LibCarla/source/carla/client/detail/Simulator.h +++ b/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); } diff --git a/LibCarla/source/carla/rpc/AckermannControllerSettings.h b/LibCarla/source/carla/rpc/AckermannControllerSettings.h new file mode 100644 index 000000000..f9776d978 --- /dev/null +++ b/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 . + +#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 diff --git a/LibCarla/source/carla/rpc/Command.h b/LibCarla/source/carla/rpc/Command.h index 53154f38f..b698a1124 100644 --- a/LibCarla/source/carla/rpc/Command.h +++ b/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() = default; + ApplyVehicleAckermannControl(ActorId id, const VehicleAckermannControl &value) + : actor(id), + control(value) {} + ActorId actor; + VehicleAckermannControl control; + MSGPACK_DEFINE_ARRAY(actor, control); + }; + struct ApplyWalkerControl : CommandBase { ApplyWalkerControl() = default; ApplyWalkerControl(ActorId id, const WalkerControl &value) @@ -236,6 +247,7 @@ namespace rpc { SpawnActor, DestroyActor, ApplyVehicleControl, + ApplyVehicleAckermannControl, ApplyWalkerControl, ApplyVehiclePhysicsControl, ApplyTransform, diff --git a/LibCarla/source/carla/rpc/VehicleAckermannControl.h b/LibCarla/source/carla/rpc/VehicleAckermannControl.h new file mode 100644 index 000000000..9bc4ce167 --- /dev/null +++ b/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 . + +#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 \ No newline at end of file diff --git a/PythonAPI/carla/source/libcarla/Actor.cpp b/PythonAPI/carla/source/libcarla/Actor.cpp index 9a4443d66..155dbc748 100644 --- a/PythonAPI/carla/source/libcarla/Actor.cpp +++ b/PythonAPI/carla/source/libcarla/Actor.cpp @@ -165,6 +165,7 @@ void export_actor() { class_, boost::noncopyable, boost::shared_ptr>("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) diff --git a/PythonAPI/carla/source/libcarla/Commands.cpp b/PythonAPI/carla/source/libcarla/Commands.cpp index cb09d6f0c..7fd5166da 100644 --- a/PythonAPI/carla/source/libcarla/Commands.cpp +++ b/PythonAPI/carla/source/libcarla/Commands.cpp @@ -101,6 +101,13 @@ void export_commands() { .def_readwrite("control", &cr::Command::ApplyVehicleControl::control) ; + class_("ApplyVehicleAckermannControl") + .def("__init__", &command_impl::CustomInit, (arg("actor"), arg("control"))) + .def(init((arg("actor_id"), arg("control")))) + .def_readwrite("actor_id", &cr::Command::ApplyVehicleAckermannControl::actor) + .def_readwrite("control", &cr::Command::ApplyVehicleAckermannControl::control) + ; + class_("ApplyWalkerControl") .def("__init__", &command_impl::CustomInit, (arg("actor"), arg("control"))) .def(init((arg("actor_id"), arg("control")))) @@ -211,6 +218,7 @@ void export_commands() { implicitly_convertible(); implicitly_convertible(); implicitly_convertible(); + implicitly_convertible(); implicitly_convertible(); implicitly_convertible(); implicitly_convertible(); diff --git a/PythonAPI/carla/source/libcarla/Control.cpp b/PythonAPI/carla/source/libcarla/Control.cpp index 38fc51dd2..98a0e9bfb 100644 --- a/PythonAPI/carla/source/libcarla/Control.cpp +++ b/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 . +#include #include #include #include @@ -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_("VehicleAckermannControl") + .def(init( + (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_("AckermannControllerSettings") + .def(init( + (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_("WalkerControl") .def(init( (arg("direction") = cg::Vector3D{1.0f, 0.0f, 0.0f}, diff --git a/PythonAPI/examples/manual_control.py b/PythonAPI/examples/manual_control.py index 19bc8aaab..299783820 100755 --- a/PythonAPI/examples/manual_control.py +++ b/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: - self._control.gear = 1 if self._control.reverse else -1 + 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,20 +588,39 @@ 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) + world.player.apply_control(self._control) def _parse_vehicle_keys(self, keys, milliseconds): if keys[K_UP] or keys[K_w]: - self._control.throttle = min(self._control.throttle + 0.01, 1.00) + 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: - self._control.throttle = 0.0 + if not self._ackermann_enabled: + self._control.throttle = 0.0 if keys[K_DOWN] or keys[K_s]: - self._control.brake = min(self._control.brake + 0.2, 1) + 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: - self._control.brake = 0 + if not self._ackermann_enabled: + self._control.brake = 0 steer_increment = 5e-4 * milliseconds if keys[K_LEFT] or keys[K_a]: @@ -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)) - self._control.steer = round(self._steer_cache, 1) - self._control.hand_brake = keys[K_SPACE] + 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 diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp index c09716587..7f17115b1 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp +++ b/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(Vehicle->GetController()); if (Controller) @@ -121,7 +124,14 @@ void FVehicleData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* Carl { Vehicle->ApplyVehiclePhysicsControl(PhysicsControl); } - Vehicle->ApplyVehicleControl(Control, EVehicleInputPriority::Client); + if (!bAckermannControlActive) + { + Vehicle->ApplyVehicleControl(Control, EVehicleInputPriority::Client); + } + else + { + Vehicle->ApplyVehicleAckermannControl(AckermannControl, EVehicleInputPriority::Client); + } Vehicle->SetVehicleLightState(LightState); auto Controller = Cast(Vehicle->GetController()); if (Controller) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h index 01a335562..55abe53b7 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h +++ b/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; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp index 4e92e22b9..0f54b0fe2 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp @@ -19,10 +19,12 @@ #include "Carla/Game/CarlaStatics.h" #include +#include #include "carla/rpc/LabelledPoint.h" #include #include #include +#include #include #include #include @@ -799,6 +801,7 @@ ECarlaServerResponse FVehicleActor::ApplyControlToVehicle( { FVehicleData* ActorData = GetActorData(); 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(); + ActorData->AckermannControl = AckermannControl; + ActorData->bAckermannControlActive = true; + } + else + { + auto Vehicle = Cast(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(); + VehicleAckermannControl = ActorData->AckermannControl; + } + else + { + auto Vehicle = Cast(GetActor()); + if (Vehicle == nullptr) + { + return ECarlaServerResponse::NotAVehicle; + } + VehicleAckermannControl = Vehicle->GetVehicleAckermannControl(); + } + return ECarlaServerResponse::Success; +} + +ECarlaServerResponse FVehicleActor::GetAckermannControllerSettings( + FAckermannControllerSettings& AckermannSettings) +{ + if (IsDormant()) + { + //FVehicleData* ActorData = GetActorData(); + //PhysicsControl = ActorData->PhysicsControl; + } + else + { + auto Vehicle = Cast(GetActor()); + if (Vehicle == nullptr) + { + return ECarlaServerResponse::NotAVehicle; + } + AckermannSettings = Vehicle->GetAckermannControllerSettings(); + } + return ECarlaServerResponse::Success; +} + +ECarlaServerResponse FVehicleActor::ApplyAckermannControllerSettings( + const FAckermannControllerSettings& AckermannSettings) +{ + if (IsDormant()) + { + // FVehicleData* ActorData = GetActorData(); + // ActorData->PhysicsControl = PhysicsControl; + } + else + { + auto Vehicle = Cast(GetActor()); + if (Vehicle == nullptr) + { + return ECarlaServerResponse::NotAVehicle; + } + + Vehicle->ApplyAckermannControllerSettings(AckermannSettings); + } + return ECarlaServerResponse::Success; +} + ECarlaServerResponse FVehicleActor::SetActorAutopilot(bool bEnabled, bool bKeepState) { if (IsDormant()) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h index e9ffa6213..5f4c6d94c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h +++ b/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; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp index f303b2f34..c3e324e99 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -1360,6 +1362,81 @@ void FCarlaServer::FPimpl::BindActions() return R::Success(); }; + BIND_SYNC(apply_ackermann_control_to_vehicle) << [this]( + cr::ActorId ActorId, + cr::VehicleAckermannControl Control) -> R + { + 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::Success(); + }; + + BIND_SYNC(get_ackermann_controller_settings) << [this]( + cr::ActorId ActorId) -> R + { + 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 + { + 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::Success(); + }; + BIND_SYNC(apply_control_to_walker) << [this]( cr::ActorId ActorId, cr::WalkerControl Control) -> R @@ -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)); }, diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AcekrmannController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AcekrmannController.cpp new file mode 100644 index 000000000..8b2fee944 --- /dev/null +++ b/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 . + +#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( + 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()); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannController.h new file mode 100644 index 000000000..e87b546a9 --- /dev/null +++ b/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 . + +#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] +}; \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannControllerSettings.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannControllerSettings.h new file mode 100644 index 000000000..55fa5c701 --- /dev/null +++ b/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 . + +#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; +}; \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp index 56a4ce2ee..26d4cda6e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp +++ b/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) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h index effb08c0d..89b1fb316 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h +++ b/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 diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleAckermannControl.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleAckermannControl.h new file mode 100644 index 000000000..f3118440c --- /dev/null +++ b/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 . + +#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; +}; +