ros中的pid的C++程序

以下是一个ROS中的PID控制器的C++程序示例：

#include <ros/ros.h>
#include <std_msgs/Float64.h>

class PIDController {
public:
    PIDController(double kp, double ki, double kd, double max_out, double min_out) :
        kp_(kp), ki_(ki), kd_(kd), max_out_(max_out), min_out_(min_out), error_sum_(0), last_error_(0) {}

    double compute(double setpoint, double input) {
        double error = setpoint - input;
        error_sum_ += error;
        double error_diff = error - last_error_;
        last_error_ = error;

        double p_term = kp_ * error;
        double i_term = ki_ * error_sum_;
        double d_term = kd_ * error_diff;

        double output = p_term + i_term + d_term;

        if (output > max_out_) {
            output = max_out_;
        }
        else if (output < min_out_) {
            output = min_out_;
        }

        return output;
    }

private:
    double kp_, ki_, kd_;
    double max_out_, min_out_;
    double error_sum_, last_error_;
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "pid_controller");
    ros::NodeHandle nh;

    double kp, ki, kd, max_out, min_out, setpoint, input;
    nh.getParam("kp", kp);
    nh.getParam("ki", ki);
    nh.getParam("kd", kd);
    nh.getParam("max_out", max_out);
    nh.getParam("min_out", min_out);

    PIDController pid_controller(kp, ki, kd, max_out, min_out);

    ros::Publisher output_pub = nh.advertise<std_msgs::Float64>("pid_output", 10);
    ros::Subscriber setpoint_sub = nh.subscribe("setpoint", 10, &PIDController::setSetpoint, &pid_controller);
    ros::Subscriber input_sub = nh.subscribe("input", 10, &PIDController::setInput, &pid_controller);

    ros::Rate loop_rate(10);

    while (ros::ok()) {
        double output = pid_controller.compute(setpoint, input);

        std_msgs::Float64 output_msg;
        output_msg.data = output;
        output_pub.publish(output_msg);

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

这个PID控制器类中包含了计算PID输出的函数`compute`，以及用于更新设定值和输入值的回调函数`setSetpoint`和`setInput`。在主函数中，创建了一个PIDController对象，并将其回调函数注册为ROS节点的订阅者。然后，在主循环中调用`compute`函数计算输出，并发布到ROS话题中。

需要注意的是，此程序中的`setpoint`和`input`变量需要从ROS话题中获取，否则它们将始终保持初始值。