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

Smoothed TM's PID

This commit is contained in:
Guillermo 2021-07-26 16:30:50 +02:00 committed by bernat
parent 16c8709abe
commit c611c1b464
5 changed files with 31 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
LibCarla/source/carla/trafficmanager/Constants.h
View File

@ -50,7 +50,7 @@ static const float HORIZON_RATE = 1.45f;
} // namespace PathBufferUpdate
namespace WaypointSelection {
static const float TARGET_WAYPOINT_TIME_HORIZON = 0.4f;
static const float TARGET_WAYPOINT_TIME_HORIZON = 0.5f;
static const float TARGET_WAYPOINT_HORIZON_LENGTH = 1.0f;
static const float JUNCTION_LOOK_AHEAD = 5.0f;
static const float SAFE_DISTANCE_AFTER_JUNCTION = 4.0f;
@ -130,14 +130,12 @@ static const float MAX_THROTTLE = 0.85f;
static const float MAX_BRAKE = 1.0f;
static const float MAX_STEERING = 0.8f;
static const float MAX_STEERING_DIFF = 0.15f;
static const float VELOCITY_INTEGRAL_MAX = 5.0f;
static const float VELOCITY_INTEGRAL_MIN = -5.0f;
static const float DT = 0.05f;
static const float INV_DT = 1.0f / DT;
static const std::vector<float> LONGITUDIAL_PARAM = {3.0f, 0.01f, 0.02f};
static const std::vector<float> LONGITUDIAL_HIGHWAY_PARAM = {3.6f, 0.01f, 0.05f};
static const std::vector<float> LATERAL_PARAM = {5.0f, 0.02f, 1.0f};
static const std::vector<float> LATERAL_HIGHWAY_PARAM = {3.0f, 0.02f, 0.5f};
static const std::vector<float> LONGITUDIAL_PARAM = {12.0f, 0.05f, 0.02f};
static const std::vector<float> LONGITUDIAL_HIGHWAY_PARAM = {20.0f, 0.05f, 0.01f};
static const std::vector<float> LATERAL_PARAM = {5.0f, 0.02f, 0.8f};
static const std::vector<float> LATERAL_HIGHWAY_PARAM = {3.0f, 0.02f, 0.4f};
} // namespace PID
namespace TrackTraffic {

6
LibCarla/source/carla/trafficmanager/DataStructures.h
View File

@ -61,10 +61,8 @@ struct ActuationSignal {
/// Structure to hold the controller state.
struct StateEntry {
cc::Timestamp time_instance;
float deviation;
float velocity;
float deviation_integral;
float velocity_integral;
float angular_deviation;
float velocity_deviation;
float steer;
};

13
LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp
View File

@ -94,7 +94,6 @@ void MotionPlanStage::Update(const unsigned long index) {
if (simulation_state.IsDormant(actor_id) && parameters.GetRespawnDormantVehicles() && is_hero_alive) {
// Flushing controller state for vehicle.
current_state = {current_timestamp,
0.0f, 0.0f,
0.0f, 0.0f,
0.0f};
@ -167,10 +166,11 @@ void MotionPlanStage::Update(const unsigned long index) {
if (cross_product < 0.0f) {
dot_product *= -1.0f;
}
const float current_deviation = dot_product;
const float angular_deviation = dot_product;
const float velocity_deviation = (dynamic_target_velocity - vehicle_speed) / dynamic_target_velocity;
// If previous state for vehicle not found, initialize state entry.
if (pid_state_map.find(actor_id) == pid_state_map.end()) {
const auto initial_state = StateEntry{current_timestamp, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
const auto initial_state = StateEntry{current_timestamp, 0.0f, 0.0f, 0.0f};
pid_state_map.insert({actor_id, initial_state});
}
@ -191,17 +191,13 @@ void MotionPlanStage::Update(const unsigned long index) {
// If physics is enabled for the vehicle, use PID controller.
// State update for vehicle.
current_state = PID::StateUpdate(previous_state, vehicle_speed, dynamic_target_velocity,
current_deviation, current_timestamp);
current_state = {current_timestamp, angular_deviation, velocity_deviation, 0.0f};
// Controller actuation.
actuation_signal = PID::RunStep(current_state, previous_state,
longitudinal_parameters, lateral_parameters);
if (emergency_stop) {
current_state.deviation_integral = 0.0f;
current_state.velocity_integral = 0.0f;
actuation_signal.throttle = 0.0f;
actuation_signal.brake = 1.0f;
}
@ -225,7 +221,6 @@ void MotionPlanStage::Update(const unsigned long index) {
else {
// Flushing controller state for vehicle.
current_state = {current_timestamp,
0.0f, 0.0f,
0.0f, 0.0f,
0.0f};

38
LibCarla/source/carla/trafficmanager/PIDController.h
View File

@ -22,32 +22,6 @@ using TimeInstance = chr::time_point<chr::system_clock, chr::nanoseconds>;
namespace PID {
/// This function calculates the present state of the vehicle including
/// the accumulated integral component of the PID system.
inline StateEntry StateUpdate(StateEntry previous_state,
float current_velocity,
float target_velocity,
float angular_deviation,
cc::Timestamp current_time) {
StateEntry current_state = {
current_time,
angular_deviation,
target_velocity - current_velocity,
0.0f,
0.0f,
0.0f};
// Calculating integrals.
current_state.deviation_integral = angular_deviation * DT + previous_state.deviation_integral;
current_state.velocity_integral = current_state.velocity * DT + previous_state.velocity_integral;
// Clamp velocity integral to avoid accumulating over-compensation
// with time for vehicles that take a long time to reach target velocity.
current_state.velocity_integral = std::min(VELOCITY_INTEGRAL_MAX, std::max(current_state.velocity_integral, VELOCITY_INTEGRAL_MIN));
return current_state;
}
/// This function calculates the actuation signals based on the resent state
/// change of the vehicle to minimize PID error.
inline ActuationSignal RunStep(StateEntry present_state,
@ -57,9 +31,9 @@ inline ActuationSignal RunStep(StateEntry present_state,
// Longitudinal PID calculation.
const float expr_v =
longitudinal_parameters[0] * present_state.velocity +
longitudinal_parameters[1] * present_state.velocity_integral +
longitudinal_parameters[2] * (present_state.velocity - previous_state.velocity) * INV_DT;
longitudinal_parameters[0] * present_state.velocity_deviation +
longitudinal_parameters[1] * (present_state.velocity_deviation + previous_state.velocity_deviation) * DT +
longitudinal_parameters[2] * (present_state.velocity_deviation - previous_state.velocity_deviation) * INV_DT;
float throttle;
float brake;
@ -74,9 +48,9 @@ inline ActuationSignal RunStep(StateEntry present_state,
// Lateral PID calculation.
float steer =
lateral_parameters[0] * present_state.deviation +
lateral_parameters[1] * present_state.deviation_integral +
lateral_parameters[2] * (present_state.deviation - previous_state.deviation) * INV_DT;
lateral_parameters[0] * present_state.angular_deviation +
lateral_parameters[1] * (present_state.angular_deviation + previous_state.angular_deviation) * DT +
lateral_parameters[2] * (present_state.angular_deviation - previous_state.angular_deviation) * INV_DT;
steer = std::max(previous_state.steer - MAX_STEERING_DIFF, std::min(steer, previous_state.steer + MAX_STEERING_DIFF));
steer = std::max(-MAX_STEERING, std::min(steer, MAX_STEERING));

16
PythonAPI/examples/manual_control.py
View File

@ -164,6 +164,7 @@ def get_actor_display_name(actor, truncate=250):
class World(object):
def __init__(self, carla_world, hud, args):
self.world = carla_world
self.sync = args.sync
self.actor_role_name = args.rolename
try:
self.map = self.world.get_map()
@ -256,6 +257,11 @@ class World(object):
actor_type = get_actor_display_name(self.player)
self.hud.notification(actor_type)
if self.sync:
self.world.tick()
else:
self.world.wait_for_tick()
def next_weather(self, reverse=False):
self._weather_index += -1 if reverse else 1
self._weather_index %= len(self._weather_presets)
@ -330,8 +336,9 @@ class World(object):
class KeyboardControl(object):
"""Class that handles keyboard input."""
def __init__(self, world, start_in_autopilot):
def __init__(self, world, start_in_autopilot, sync_mode):
self._autopilot_enabled = start_in_autopilot
self._sync_mode = sync_mode
if isinstance(world.player, carla.Vehicle):
self._control = carla.VehicleControl()
self._lights = carla.VehicleLightState.NONE
@ -462,6 +469,8 @@ class KeyboardControl(object):
elif self._control.manual_gear_shift and event.key == K_PERIOD:
self._control.gear = self._control.gear + 1
elif event.key == K_p and not pygame.key.get_mods() & KMOD_CTRL:
if not self._autopilot_enabled and not self._sync_mode:
print("WARNING: It is recommended to use the autopilot in synchronous mode")
self._autopilot_enabled = not self._autopilot_enabled
world.player.set_autopilot(self._autopilot_enabled)
world.hud.notification(
@ -1156,7 +1165,7 @@ def game_loop(args):
hud = HUD(args.width, args.height)
world = World(sim_world, hud, args)
controller = KeyboardControl(world, args.autopilot)
controller = KeyboardControl(world, args.autopilot, args.sync)
if args.sync:
sim_world.tick()
@ -1256,6 +1265,9 @@ def main():
print(__doc__)
if args.autopilot and not args.sync:
print("WARNING: it is recommended to use the autopilot in synchronous mode")
try:
game_loop(args)