ros写一个追踪算法，可以用简单的pid，也可以用纯追踪，在机器人gazebo环境下编程实现一个y= 1.0+ 0.5*sin(x)曲线路径的纯追踪，在控制窗口里实时输出机器人位置与预定轨迹的跟踪误差。

以下是一个基于PID的追踪算法的ROS实现，它可以追踪y=1.0+0.5*sin(x)曲线路径。

首先，需要创建一个ROS节点并订阅机器人的位姿信息和控制指令。代码如下：

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import PoseStamped, Twist

# callback function to get the robot pose
def pose_callback(msg):
    # TODO: implement the pose callback function
    pass

# callback function to get the control command
def cmd_callback(msg):
    # TODO: implement the cmd callback function
    pass

if __name__ == '__main__':
    rospy.init_node('path_tracker')

    # subscribe to the robot pose and control command topics
    rospy.Subscriber('/robot_pose', PoseStamped, pose_callback)
    rospy.Subscriber('/control_cmd', Twist, cmd_callback)

    # TODO: initialize the PID controller and other variables

    rospy.spin()

然后，需要实现PID控制器并计算控制指令。代码如下：

import math

class PIDController:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.integral = 0.0
        self.derivative = 0.0
        self.prev_error = 0.0

    def compute(self, error):
        self.integral += error
        self.derivative = error - self.prev_error
        self.prev_error = error
        return self.Kp * error + self.Ki * self.integral + self.Kd * self.derivative

# path tracking function
def track_path(x, y, vx, vy):
    # calculate the desired position and velocity
    x_des = x
    y_des = 1.0 + 0.5 * math.sin(x)
    vx_des = vx
    vy_des = 0.5 * math.cos(x) * vx

    # calculate the tracking error
    error_x = x_des - x
    error_y = y_des - y

    # calculate the control command using PID
    cmd_vx = pid_x.compute(error_x)
    cmd_vy = pid_y.compute(error_y)

    # publish the control command
    cmd_msg = Twist()
    cmd_msg.linear.x = cmd_vx
    cmd_msg.linear.y = cmd_vy
    cmd_msg.angular.z = 0.0
    cmd_pub.publish(cmd_msg)

    # publish the tracking error
    error_msg = PoseStamped()
    error_msg.pose.position.x = error_x
    error_msg.pose.position.y = error_y
    error_msg.pose.orientation.w = 1.0
    error_pub.publish(error_msg)

最后，需要在回调函数中调用path_tracking函数并发布控制指令和跟踪误差。代码如下：

pid_x = PIDController(Kp=0.5, Ki=0.0, Kd=0.1)
pid_y = PIDController(Kp=0.5, Ki=0.0, Kd=0.1)
cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
error_pub = rospy.Publisher('/error', PoseStamped, queue_size=10)

def pose_callback(msg):
    x = msg.pose.position.x
    y = msg.pose.position.y
    vx = msg.twist.linear.x
    vy = msg.twist.linear.y
    track_path(x, y, vx, vy)

def cmd_callback(msg):
    # ignore the control command from external sources
    pass

完整的代码如下：

#!/usr/bin/env python

import math
import rospy
from geometry_msgs.msg import PoseStamped, Twist

class PIDController:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.integral = 0.0
        self.derivative = 0.0
        self.prev_error = 0.0

    def compute(self, error):
        self.integral += error
        self.derivative = error - self.prev_error
        self.prev_error = error
        return self.Kp * error + self.Ki * self.integral + self.Kd * self.derivative

# path tracking function
def track_path(x, y, vx, vy):
    # calculate the desired position and velocity
    x_des = x
    y_des = 1.0 + 0.5 * math.sin(x)
    vx_des = vx
    vy_des = 0.5 * math.cos(x) * vx

    # calculate the tracking error
    error_x = x_des - x
    error_y = y_des - y

    # calculate the control command using PID
    cmd_vx = pid_x.compute(error_x)
    cmd_vy = pid_y.compute(error_y)

    # publish the control command
    cmd_msg = Twist()
    cmd_msg.linear.x = cmd_vx
    cmd_msg.linear.y = cmd_vy
    cmd_msg.angular.z = 0.0
    cmd_pub.publish(cmd_msg)

    # publish the tracking error
    error_msg = PoseStamped()
    error_msg.pose.position.x = error_x
    error_msg.pose.position.y = error_y
    error_msg.pose.orientation.w = 1.0
    error_pub.publish(error_msg)

# callback function to get the robot pose
def pose_callback(msg):
    x = msg.pose.position.x
    y = msg.pose.position.y
    vx = msg.twist.linear.x
    vy = msg.twist.linear.y
    track_path(x, y, vx, vy)

# callback function to get the control command
def cmd_callback(msg):
    # ignore the control command from external sources
    pass

if __name__ == '__main__':
    rospy.init_node('path_tracker')

    # subscribe to the robot pose and control command topics
    rospy.Subscriber('/robot_pose', PoseStamped, pose_callback)
    rospy.Subscriber('/control_cmd', Twist, cmd_callback)

    # initialize the PID controller and other variables
    pid_x = PIDController(Kp=0.5, Ki=0.0, Kd=0.1)
    pid_y = PIDController(Kp=0.5, Ki=0.0, Kd=0.1)
    cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    error_pub = rospy.Publisher('/error', PoseStamped, queue_size=10)

    rospy.spin()

这个算法在机器人运动过程中会实时输出机器人位置与预定轨迹的跟踪误差，并调整控制指令以使机器人尽可能接近预定轨迹。