ROS机器人轨迹跟踪PID控制器代码示例？

以下是一个简单的ROS机器人轨迹跟踪PID控制器代码示例：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

double Kp = 1.;
double Ki = .;
double Kd = .;
double error_sum = .;
double last_error = .;
double max_speed = .5;

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
double x = msg->pose.pose.position.x;
double y = msg->pose.pose.position.y;
double theta = atan2(2 * msg->pose.pose.orientation.w * msg->pose.pose.orientation.z,
1 - 2 * msg->pose.pose.orientation.z * msg->pose.pose.orientation.z);

double target_x = 1.;
double target_y = 1.;
double target_theta = atan2(target_y - y, target_x - x);

double error = target_theta - theta;
error_sum += error;
double error_diff = error - last_error;
last_error = error;

double cmd_vel = Kp * error + Ki * error_sum + Kd * error_diff;
cmd_vel = std::min(std::max(cmd_vel, -max_speed), max_speed);

geometry_msgs::Twist twist;
twist.linear.x = cmd_vel;
twist.angular.z = .;
pub.publish(twist);
}

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

ros::Subscriber sub = nh.subscribe("odom", 1, odomCallback);
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);

ros::spin();
return ;
}