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 actuation signals based on the resent state
26 /// change of the vehicle to minimize PID error.
27 inline ActuationSignal RunStep(StateEntry present_state,
28  StateEntry previous_state,
29  const std::vector<float> &longitudinal_parameters,
30  const std::vector<float> &lateral_parameters) {
31 
32  // Longitudinal PID calculation.
33  const float expr_v =
34  longitudinal_parameters[0] * present_state.velocity_deviation +
35  longitudinal_parameters[1] * (present_state.velocity_deviation + previous_state.velocity_deviation) * DT +
36  longitudinal_parameters[2] * (present_state.velocity_deviation - previous_state.velocity_deviation) * INV_DT;
37 
38  float throttle;
39  float brake;
40 
41  if (expr_v > 0.0f) {
42  throttle = std::min(expr_v, MAX_THROTTLE);
43  brake = 0.0f;
44  } else {
45  throttle = 0.0f;
46  brake = std::min(std::abs(expr_v), MAX_BRAKE);
47  }
48 
49  // Lateral PID calculation.
50  float steer =
51  lateral_parameters[0] * present_state.angular_deviation +
52  lateral_parameters[1] * (present_state.angular_deviation + previous_state.angular_deviation) * DT +
53  lateral_parameters[2] * (present_state.angular_deviation - previous_state.angular_deviation) * INV_DT;
54 
55  steer = std::max(previous_state.steer - MAX_STEERING_DIFF, std::min(steer, previous_state.steer + MAX_STEERING_DIFF));
56  steer = std::max(-MAX_STEERING, std::min(steer, MAX_STEERING));
57 
58  return ActuationSignal{throttle, brake, steer};
59 }
60 
61 } // namespace PID
62 } // namespace traffic_manager
63 } // 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:133
static const float MAX_STEERING_DIFF
Definition: Constants.h:150
Structure to hold the actuation signals.
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:27