#! /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 =
f1tenth仿真沿墙pid跑圈(两个周期在起点停车)
猜你喜欢
转载自blog.csdn.net/weixin_72050316/article/details/132152583
今日推荐
周排行