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

vsropy

已于 2023-08-07 19:29:51 修改

阅读量63

点赞数

分类专栏： f1tenth仿真文章标签：机器学习自动驾驶

于 2023-08-07 18:58:28 首次发布

本文链接：https://blog.csdn.net/weixin_72050316/article/details/132152583

版权

f1tenth仿真专栏收录该内容

9 篇文章 1 订阅 ¥9.90 ¥99.00

订阅专栏

超级会员免费看

#! /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 =

了解本专栏