p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

First iteration ackermann controller

This commit is contained in:
Joel Moriana 2022-02-07 12:39:38 +01:00 committed by joel-mb
parent 8e7a859c36
commit 8cc6f132fc
23 changed files with 1181 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
LibCarla/source/carla/client/Vehicle.cpp
View File

@ -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);
}

9
LibCarla/source/carla/client/Vehicle.h
View File

@ -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);

15
LibCarla/source/carla/client/detail/Client.cpp
View File

@ -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);
}

12
LibCarla/source/carla/client/detail/Client.h
View File

@ -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);

12
LibCarla/source/carla/client/detail/Simulator.h
View File

@ -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);
}

92
LibCarla/source/carla/rpc/AckermannControllerSettings.h Normal file
View File

@ -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

12
LibCarla/source/carla/rpc/Command.h
View File

@ -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,

84
LibCarla/source/carla/rpc/VehicleAckermannControl.h Normal file
View File

@ -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

3
PythonAPI/carla/source/libcarla/Actor.cpp
View File

@ -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)

8
PythonAPI/carla/source/libcarla/Commands.cpp
View File

@ -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>();

57
PythonAPI/carla/source/libcarla/Control.cpp
View File

@ -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},

81
PythonAPI/examples/manual_control.py
View File

@ -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

12
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp
View File

@ -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);
}
Vehicle->ApplyVehicleControl(Control, EVehicleInputPriority::Client);
if (!bAckermannControlActive)
{
Vehicle->ApplyVehicleControl(Control, EVehicleInputPriority::Client);
}
else
{
Vehicle->ApplyVehicleAckermannControl(AckermannControl, EVehicleInputPriority::Client);
}
Vehicle->SetVehicleLightState(LightState);
auto Controller = Cast<AWheeledVehicleAIController>(Vehicle->GetController());
if (Controller)

5
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h
View File

@ -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;

84
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp
View File

@ -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())

30
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h
View File

@ -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;

78
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp
View File

@ -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)); },

305
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AcekrmannController.cpp Normal file
View File

@ -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());
}

164
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannController.h Normal file
View File

@ -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]
};

33
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/AckermannControllerSettings.h Normal file
View File

@ -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;
};

10
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp
View File

@ -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)

41
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h
View File

@ -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

31
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/VehicleAckermannControl.h Normal file
View File

@ -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;
};