From c611c1b464002ef4b4a20624fa7d520b1df79740 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Mon, 26 Jul 2021 16:30:50 +0200 Subject: [PATCH] Smoothed TM's PID --- .../source/carla/trafficmanager/Constants.h | 12 +++--- .../carla/trafficmanager/DataStructures.h | 6 +-- .../carla/trafficmanager/MotionPlanStage.cpp | 13 ++----- .../carla/trafficmanager/PIDController.h | 38 +++---------------- PythonAPI/examples/manual_control.py | 16 +++++++- 5 files changed, 31 insertions(+), 54 deletions(-) diff --git a/LibCarla/source/carla/trafficmanager/Constants.h b/LibCarla/source/carla/trafficmanager/Constants.h index e175e4aa5..0fcf9040b 100644 --- a/LibCarla/source/carla/trafficmanager/Constants.h +++ b/LibCarla/source/carla/trafficmanager/Constants.h @@ -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 LONGITUDIAL_PARAM = {3.0f, 0.01f, 0.02f}; -static const std::vector LONGITUDIAL_HIGHWAY_PARAM = {3.6f, 0.01f, 0.05f}; -static const std::vector LATERAL_PARAM = {5.0f, 0.02f, 1.0f}; -static const std::vector LATERAL_HIGHWAY_PARAM = {3.0f, 0.02f, 0.5f}; +static const std::vector LONGITUDIAL_PARAM = {12.0f, 0.05f, 0.02f}; +static const std::vector LONGITUDIAL_HIGHWAY_PARAM = {20.0f, 0.05f, 0.01f}; +static const std::vector LATERAL_PARAM = {5.0f, 0.02f, 0.8f}; +static const std::vector LATERAL_HIGHWAY_PARAM = {3.0f, 0.02f, 0.4f}; } // namespace PID namespace TrackTraffic { diff --git a/LibCarla/source/carla/trafficmanager/DataStructures.h b/LibCarla/source/carla/trafficmanager/DataStructures.h index e9ed7778a..60ec1198a 100644 --- a/LibCarla/source/carla/trafficmanager/DataStructures.h +++ b/LibCarla/source/carla/trafficmanager/DataStructures.h @@ -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; }; diff --git a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp index c1d22fee5..6fbc2ed1a 100644 --- a/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp +++ b/LibCarla/source/carla/trafficmanager/MotionPlanStage.cpp @@ -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}; diff --git a/LibCarla/source/carla/trafficmanager/PIDController.h b/LibCarla/source/carla/trafficmanager/PIDController.h index 6ccacbaae..f855a6b27 100644 --- a/LibCarla/source/carla/trafficmanager/PIDController.h +++ b/LibCarla/source/carla/trafficmanager/PIDController.h @@ -22,32 +22,6 @@ using TimeInstance = chr::time_point; 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)); diff --git a/PythonAPI/examples/manual_control.py b/PythonAPI/examples/manual_control.py index 727359aee..b65dc5927 100755 --- a/PythonAPI/examples/manual_control.py +++ b/PythonAPI/examples/manual_control.py @@ -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)