f1tenth仿真沿墙pid跑圈（两个周期在起点停车）

#! /usr/bin/env python
import rospy
import numpy as np
from sensor_msgs.msg import LaserScan
from ackermann_msgs.msg import AckermannDriveStamped

KP = 1.000
KD = 0.005
prev_error = 0
FILTER_VALUE = 10.0
DESIRED_DISTANCE_RIGHT = 1.0

start_time = None  # 记录起始时间

def get_range(data, angle, deg=True):
    if deg:
        angle = np.deg2rad(angle)
    dis = data.ranges[int((angle - data.angle_min) / data.angle_increment)]
    if dis < data.range_min or dis > data.range_max:
        dis = FILTER_VALUE
    return dis

def wall_following_callback(data):
    global start_time, prev_error

    if start_time is None:
        start_time = rospy.Time.now()

    # 获取经过的时间（秒）
    elapsed_time = (rospy.Time.now() - start_time).to_sec()

    if elapsed_time >= 36.348:  # 经过37秒后停止程序
        rospy.signal_shutdown("Program terminated after 37 seconds.")

    # 1、计算公式：根据雷达信息，得到error
    THETA =