请用在ROS系统中用写出四轮差速机器人的纯跟踪算法

在ROS系统中实现四轮差速机器人的纯跟踪算法，需要进行以下步骤：

1. 获取机器人当前位置和目标位置。
可以使用ROS中的odom话题获取机器人当前位置信息，可以使用自定义话题或服务获取目标位置信息。

2. 计算机器人需要达到的线速度和角速度。
线速度可以根据机器人当前位置和目标位置之间的距离来计算，角速度可以根据目标位置与机器人当前位置之间的角度偏差来计算，采用PID控制器进行计算。

3. 控制机器人执行运动。
根据计算得到的线速度和角速度，通过ROS中的cmd_vel话题发送速度指令，控制机器人运动。

4. 循环执行以上步骤，直到机器人到达目标位置或停止运行。

下面是一个简单的纯跟踪算法的ROS节点示例代码，其中机器人和目标位置信息通过ROS中的odom和自定义的goal话题获取，速度指令通过cmd_vel话题发送。

#!/usr/bin/env python

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist, Point, Quaternion
from math import atan2, sqrt, pow

class PurePursuit():
    def __init__(self):
        rospy.init_node('pure_pursuit')
        rospy.Subscriber('/odom', Odometry, self.odom_callback)
        rospy.Subscriber('/goal', Point, self.goal_callback)
        self.pub_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.robot_pos = Point()
        self.robot_ori = Quaternion()
        self.goal_pos = Point()
        self.goal_thresh = rospy.get_param('goal_thresh', 0.1)
        self.max_speed = rospy.get_param('max_speed', 0.5)
        self.kp = rospy.get_param('kp', 1.0)
        self.kd = rospy.get_param('kd', 0.0)
        self.ki = rospy.get_param('ki', 0.0)
        self.dt = rospy.get_param('dt', 0.1)
        self.prev_error = 0
        self.sum_error = 0
        rospy.spin()

    def odom_callback(self, msg):
        self.robot_pos = msg.pose.pose.position
        self.robot_ori = msg.pose.pose.orientation

    def goal_callback(self, msg):
        self.goal_pos = msg

    def control_loop(self):
        rate = rospy.Rate(1.0 / self.dt)
        while not rospy.is_shutdown():
            distance_to_goal = sqrt(
                pow(self.goal_pos.x - self.robot_pos.x, 2) + pow(self.goal_pos.y - self.robot_pos.y, 2)
            )
            if distance_to_goal < self.goal_thresh:
                break
            angle_to_goal = atan2(
                self.goal_pos.y - self.robot_pos.y,
                self.goal_pos.x - self.robot_pos.x
            )
            angle_error = angle_to_goal - self.robot_ori.z
            pid_output = self.kp * angle_error + self.kd * (angle_error-self.prev_error)/self.dt + self.ki*self.sum_error
            speed = self.max_speed
            if abs(pid_output) > speed:
                pid_output = pid_output/abs(pid_output)*speed
            vel_msg = Twist()
            vel_msg.linear.x = speed
            vel_msg.angular.z = pid_output
            self.pub_vel.publish(vel_msg)
            self.prev_error = angle_error
            self.sum_error += angle_error
            rate.sleep()

if __name__ == '__main__':
    try:
        pp = PurePursuit()
        pp.control_loop()
    except rospy.ROSInterruptException:
        rospy.logerr("ROS Interrupt Exception!")
        pass

上述代码中实现了一个简单的纯跟踪算法节点，其中控制机器人运动的主要算法部分位于control_loop方法中，通过pose信息和目标位置信息计算需要达到的线速度和角速度，并通过PID控制器进行计算，最后将速度指令发布到cmd_vel话题中。在程序运行时，可以通过ROS参数服务器设置各个参数，如goal_thresh，max_speed，kp，kd，ki和dt等。