CARLA
PIDController.h
Go to the documentation of this file.
1 // Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #pragma once
8 
9 #include <algorithm>
10 
13 
14 namespace carla {
15 namespace traffic_manager {
16 
17 namespace chr = std::chrono;
18 
19 using namespace constants::PID;
20 
21 using TimeInstance = chr::time_point<chr::system_clock, chr::nanoseconds>;
22 
23 namespace PID {
24 
25 /// This function calculates the present state of the vehicle including
26 /// the accumulated integral component of the PID system.
28  float current_velocity,
29  float target_velocity,
30  float angular_deviation,
31  cc::Timestamp current_time) {
32  StateEntry current_state = {
33  current_time,
34  angular_deviation,
35  (current_velocity - target_velocity) / target_velocity,
36  0.0f,
37  0.0f};
38 
39  // Calculating integrals.
40  current_state.deviation_integral = angular_deviation * DT + previous_state.deviation_integral;
41  current_state.velocity_integral = DT * current_state.velocity + previous_state.velocity_integral;
42 
43  // Clamp velocity integral to avoid accumulating over-compensation
44  // with time for vehicles that take a long time to reach target velocity.
45  current_state.velocity_integral = std::min(VELOCITY_INTEGRAL_MAX, std::max(current_state.velocity_integral, VELOCITY_INTEGRAL_MIN));
46 
47  return current_state;
48 }
49 
50 /// This function calculates the actuation signals based on the resent state
51 /// change of the vehicle to minimize PID error.
53  StateEntry previous_state,
54  const std::vector<float> &longitudinal_parameters,
55  const std::vector<float> &lateral_parameters) {
56 
57  // Longitudinal PID calculation.
58  const float expr_v =
59  longitudinal_parameters[0] * present_state.velocity +
60  longitudinal_parameters[1] * present_state.velocity_integral +
61  longitudinal_parameters[2] * (present_state.velocity - previous_state.velocity) * INV_DT;
62 
63  float throttle;
64  float brake;
65 
66  if (expr_v < 0.0f) {
67  throttle = std::min(std::abs(expr_v), MAX_THROTTLE);
68  brake = 0.0f;
69  } else {
70  throttle = 0.0f;
71  brake = std::min(expr_v, MAX_BRAKE);
72  }
73 
74  // Lateral PID calculation.
75  float steer =
76  lateral_parameters[0] * present_state.deviation +
77  lateral_parameters[1] * present_state.deviation_integral +
78  lateral_parameters[2] * (present_state.deviation - previous_state.deviation) * INV_DT;
79 
80  steer = std::max(-0.8f, std::min(steer, 0.8f));
81 
82  return ActuationSignal{throttle, brake, steer};
83 }
84 
85 } // namespace PID
86 } // namespace traffic_manager
87 } // namespace carla
chr::time_point< chr::system_clock, chr::nanoseconds > TimeInstance
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
StateEntry StateUpdate(StateEntry previous_state, float current_velocity, float target_velocity, float angular_deviation, cc::Timestamp current_time)
This function calculates the present state of the vehicle including the accumulated integral componen...
Definition: PIDController.h:27
Structure to hold the actuation signals.
static const float VELOCITY_INTEGRAL_MAX
Definition: Constants.h:121
static const float VELOCITY_INTEGRAL_MIN
Definition: Constants.h:122
Structure to hold the controller state.
ActuationSignal RunStep(StateEntry present_state, StateEntry previous_state, const std::vector< float > &longitudinal_parameters, const std::vector< float > &lateral_parameters)
This function calculates the actuation signals based on the resent state change of the vehicle to min...
Definition: PIDController.h:52