pid.hpp

Go to the documentation of this file.

#pragma once
 
#include "base_longitudinal_controller.hpp"
#include "common_lib/structures/position.hpp"
#include "utils/utils.hpp"
#include "gtest/gtest.h"
#include <rclcpp/rclcpp.hpp>
 
class PID : public LongitudinalController {
private:
  std::vector<custom_interfaces::msg::PathPoint> last_path_msg_;
  custom_interfaces::msg::Velocities last_velocity_msg_;
  custom_interfaces::msg::Pose last_pose_msg_;
  double absolute_velocity_ = 0.0;
 
  bool received_first_path_ = false;
  bool received_first_state_ = false;
  bool received_first_pose_ = false;
 
  double proportional_{0.0f};   
  double integrator_{0.0f};     
  double differentiator_{0.0f}; 
  double prev_error_{0.0f};       
  double prev_measurement_{0.0f}; 
  double out_{0.0f}; 
  double calculate_error(double setpoint, double measurement) const;
 
  void calculate_proportional_term(double error);
 
  void calculate_integral_term(double error);
 
  void anti_wind_up();
 
  void calculate_derivative_term(double measurement);
 
  void compute_output();
 
  double update(double setpoint, double measurement);
 
public:
  PID(const ControlParameters &params);
 
  void path_callback(const custom_interfaces::msg::PathPointArray& msg) override;
  void vehicle_state_callback(const custom_interfaces::msg::Velocities& msg) override;
  void vehicle_pose_callback(const custom_interfaces::msg::Pose& msg) override;
  common_lib::structures::ControlCommand get_throttle_command() override;
  void publish_solver_data(std::shared_ptr<rclcpp::Node> node, std::map<std::string, std::shared_ptr<rclcpp::PublisherBase>>& publisher_map) override;
 
  FRIEND_TEST(PidTests, TestAntiWindUp1);
  FRIEND_TEST(PidTests, TestAntiWindUp2);
  FRIEND_TEST(PidTests, TestAntiWindUp3);
  FRIEND_TEST(PidTests, ProportionalTerm);
  FRIEND_TEST(PidTests, IntegralTerm1);
  FRIEND_TEST(PidTests, IntegralTerm2);
  FRIEND_TEST(PidTests, DerivativeTerm1);
  FRIEND_TEST(PidTests, DerivativeTerm2);
  FRIEND_TEST(PidTests, Output1);
  FRIEND_TEST(PidTests, Output2);
  FRIEND_TEST(PidTests, Output3);
  FRIEND_TEST(PidTests, Update1);
};