蓝桥ROS之f1tenth简单PID沿墙跑起来(Python)
【摘要】
在此案例前需完成:
☞ 蓝桥ROS之f1tenth案例学习与调试(成功)
遥控肯定不过瘾,那么如何用一个PID程序使小车自动沿墙跑呢???
公式如上……
跑一跑看看?
阅读pdf文档:
python程序模板:
#!/usr...
在此案例前需完成:
☞ 蓝桥ROS之f1tenth案例学习与调试(成功)
遥控肯定不过瘾,那么如何用一个PID程序使小车自动沿墙跑呢???
公式如上……
跑一跑看看?
阅读pdf文档:
python程序模板:
-
#!/usr/bin/env python
-
from __future__ import print_function
-
import sys
-
import math
-
import numpy as np
-
-
#ROS Imports
-
import rospy
-
from sensor_msgs.msg import Image, LaserScan
-
from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive
-
-
#PID CONTROL PARAMS
-
kp = #TODO
-
kd = #TODO
-
ki = #TODO
-
servo_offset = 0.0
-
prev_error = 0.0
-
error = 0.0
-
integral = 0.0
-
-
#WALL FOLLOW PARAMS
-
ANGLE_RANGE = 270 # Hokuyo 10LX has 270 degrees scan
-
DESIRED_DISTANCE_RIGHT = 0.9 # meters
-
DESIRED_DISTANCE_LEFT = 0.55
-
VELOCITY = 2.00 # meters per second
-
CAR_LENGTH = 0.50 # Traxxas Rally is 20 inches or 0.5 meters
-
-
class WallFollow:
-
""" Implement Wall Following on the car
-
"""
-
def __init__(self):
-
#Topics & Subs, Pubs
-
lidarscan_topic = '/scan'
-
drive_topic = '/nav'
-
-
self.lidar_sub = #TODO: Subscribe to LIDAR
-
self.drive_pub = #TODO: Publish to drive
-
-
def getRange(self, data, angle):
-
# data: single message from topic /scan
-
# angle: between -45 to 225 degrees, where 0 degrees is directly to the right
-
# Outputs length in meters to object with angle in lidar scan field of view
-
#make sure to take care of nans etc.
-
#TODO: implement
-
return 0.0
-
-
def pid_control(self, error, velocity):
-
global integral
-
global prev_error
-
global kp
-
global ki
-
global kd
-
angle = 0.0
-
#TODO: Use kp, ki & kd to implement a PID controller for
-
drive_msg = AckermannDriveStamped()
-
drive_msg.header.stamp = rospy.Time.now()
-
drive_msg.header.frame_id = "laser"
-
drive_msg.drive.steering_angle = angle
-
drive_msg.drive.speed = velocity
-
self.drive_pub.publish(drive_msg)
-
-
def followLeft(self, data, leftDist):
-
#Follow left wall as per the algorithm
-
#TODO:implement
-
return 0.0
-
-
def lidar_callback(self, data):
-
"""
-
"""
-
error = 0.0 #TODO: replace with error returned by followLeft
-
#send error to pid_control
-
self.pid_control(error, VELOCITY)
-
-
def main(args):
-
rospy.init_node("WallFollow_node", anonymous=True)
-
wf = WallFollow()
-
rospy.sleep(0.1)
-
rospy.spin()
-
-
if __name__=='__main__':
-
main(sys.argv)
参考程序:
scan.py
-
import rospy
-
from sensor_msgs.msg import LaserScan
-
-
def callback(data):
-
print(data.ranges[270])
-
-
rospy.init_node("scan_test", anonymous=False)
-
sub = rospy.Subscriber("/scan", LaserScan, callback)
-
rospy.spin()
dist.py
-
import rospy
-
import math
-
from sensor_msgs.msg import LaserScan
-
from race.msg import pid_input
-
-
-
angle_range = 360 # sensor angle range of the lidar
-
car_length = 1.5 # projection distance we project car forward.
-
vel = 1.5 # used for pid_vel (not much use).
-
error = 0.0
-
dist_from_wall = 0.8
-
-
pub = rospy.Publisher('error', pid_input, queue_size=10)
-
-
def getRange(data,theta):
-
-
angle_range = data.angle_max - data.angle_min
-
angle_increment = data.angle_increment
-
scan_range = []
-
rad2deg_factor = 57.296
-
angle_range *= rad2deg_factor
-
angle_increment *= rad2deg_factor
-
-
for element in data.ranges:
-
if math.isnan(element) or math.isinf(element):
-
element = 100
-
scan_range.append(element)
-
-
index = round(theta / angle_increment)
-
-
return scan_range[index]
-
-
def callback(data):
-
theta = 55
-
left_dist = float(getRange(data, 270))
-
a = getRange(data,(90 + theta)) # Ray at degree theta from right_dist
-
right_dist = getRange(data,90) # Ray perpendicular to right side of car
-
theta_r = math.radians(theta)
-
# dist_from_wall = (left_dist + right_dist)/2 # keep in middle
-
-
# Calculating the deviation of steering(alpha)
-
alpha = math.atan( (a * math.cos(theta_r) - right_dist)/ a * math.sin(theta_r) )
-
AB = right_dist * math.cos(alpha) # Present Position
-
CD = AB + car_length * math.sin(alpha) # projection in Future Position
-
-
error = dist_from_wall - CD # total error
-
# print('error: ', error) #Testing
-
-
# Sending PID error to Control
-
msg = pid_input()
-
msg.pid_error = error
-
msg.pid_vel = vel
-
pub.publish(msg)
-
-
-
if __name__ == '__main__':
-
print("Laser node started")
-
rospy.init_node('dist_finder',anonymous = True)
-
rospy.Subscriber("scan",LaserScan,callback)
-
rospy.spin()
control.py
-
import rospy
-
from race.msg import pid_input
-
from ackermann_msgs.msg import AckermannDriveStamped
-
-
kp = 0.7 #45
-
kd = 0.00125#0.001 #0.09
-
ki = 0#0.5
-
kp_vel = 6 #9 is using keep in middle
-
kd_vel = 0.0001
-
ki_error = 0
-
prev_error = 0.0
-
vel_input = 2.5 # base velocity
-
angle = 0.0 # initial steering angle
-
-
pub = rospy.Publisher("/drive", AckermannDriveStamped, queue_size=5)
-
-
-
def control(data):
-
global prev_error
-
global vel_input
-
global kp
-
global kp_vel
-
global kd
-
global kd_vel
-
global ki
-
global angle
-
-
e = data.pid_error
-
# Calculating deviation for lateral deviation from path
-
kp_error = kp * e
-
kd_error = kd * (e - prev_error)
-
-
# Calculating error for velocity
-
kp_vel_er = kp_vel * e
-
kd_vel_er = kd * (e - prev_error)
-
# ki_error = ki_error + (ki * e)
-
-
vel_error = kp_vel_er + kd_vel_er
-
pid_error = kp_error + kd_error
-
-
-
min_angle=-20
-
max_angle= 20
-
-
# Heigher error results in lower velocity
-
# while lower error results in heigher velocity
-
velo = vel_input + 1/(abs(vel_error))
-
-
#corrected steering angle
-
angle = pid_error
-
-
#print("raw velo:", velo) # Testing
-
-
#Speed limit
-
if velo > 15 :
-
velo = 10
-
-
# Filtering steering angle for Out-of-Range values
-
if angle < min_angle:
-
angle = min_angle
-
elif angle > max_angle:
-
angle = max_angle
-
-
# print("filtered angle :" , angle) # Testing
-
-
-
# Sending Drive information to Car
-
msg = AckermannDriveStamped()
-
msg.header.stamp = rospy.Time.now()
-
msg.header.frame_id = ''
-
msg.drive.speed = velo
-
msg.drive.steering_angle = angle
-
pub.publish(msg)
-
-
if __name__ == '__main__':
-
print("Starting control...")
-
rospy.init_node('pid_controller', anonymous=True)
-
rospy.Subscriber("error", pid_input, control)
-
rospy.spin()
centre_wall_follow.py
-
import rospy
-
import math
-
from sensor_msgs.msg import LaserScan
-
from race.msg import pid_input
-
-
-
angle_range = 360 # sensor angle range of the lidar
-
car_length = 1.5 # projection distance we project car forward.
-
vel = 1.0 # used for pid_vel (not much use).
-
error = 0.0
-
-
pub = rospy.Publisher('error', pid_input, queue_size=10)
-
-
def getRange(data,theta):
-
-
angle_range = data.angle_max - data.angle_min
-
angle_increment = data.angle_increment
-
scan_range = []
-
rad2deg_factor = 57.296
-
angle_range *= rad2deg_factor
-
angle_increment *= rad2deg_factor
-
-
for element in data.ranges:
-
if math.isnan(element) or math.isinf(element):
-
element = 100
-
scan_range.append(element)
-
-
index = round(theta / angle_increment)
-
-
return scan_range[index]
-
-
def callback(data):
-
-
dist_in_front = getRange(data, 180)
-
theta = 30
-
left_dist = getRange(data, 270)
-
right_dist = getRange(data,90) # Ray perpendicular to right side of car
-
a_right = getRange(data,(90 + theta)) # Ray at degree theta from right_dist
-
a_left = getRange(data,(270 - theta))
-
theta = math.radians(theta)
-
-
# Calculating the deviation of steering(alpha) from right
-
alpha_r = math.atan( (a_right * math.cos(theta) - right_dist)/ a_right * math.sin(theta) )
-
curr_pos_r = right_dist * math.cos(alpha_r) # Present Position
-
fut_pos_r = curr_pos_r + car_length * math.sin(alpha_r) # projection in Future Position
-
-
# Calculating the deviation of steering(alpha) from left
-
alpha_l = math.atan( (a_left * math.cos(theta) - left_dist)/ a_left * math.sin(theta) )
-
curr_pos_l = left_dist * math.cos(alpha_l) # Present Position
-
fut_pos_l = curr_pos_l + car_length * math.sin(alpha_l) # projection in Future Position
-
-
error = - (fut_pos_r - fut_pos_l) # total error
-
# print('error: ', error) #Testing
-
-
# Sending PID error to Control
-
msg = pid_input()
-
msg.pid_error = error
-
msg.pid_vel = dist_in_front # pid_vel used for distance in front
-
pub.publish(msg)
-
-
-
if __name__ == '__main__':
-
print("Laser node started")
-
rospy.init_node('dist_finder',anonymous = True)
-
rospy.Subscriber("scan",LaserScan,callback)
-
rospy.spin()
centre_wall_control.py
-
import rospy
-
import math
-
from race.msg import pid_input
-
from ackermann_msgs.msg import AckermannDriveStamped
-
-
kp = 0.27# 0.27 for porto
-
kd = 0.0125#0.001 #0.09
-
ki = 0 #0.5
-
kp_vel = 0.9 #9 is using keep in middle
-
kd_vel = 0.0001
-
ki_error = 0
-
prev_error = 0.0
-
vel_input = 2.5 # base velocity
-
angle = 0.0 # initial steering angle
-
-
pub = rospy.Publisher("/drive", AckermannDriveStamped, queue_size=5)
-
-
-
def control(data):
-
global prev_error
-
global vel_input
-
global kp
-
global kp_vel
-
global kd
-
global kd_vel
-
global ki
-
global angle
-
-
dist_in_front = data.pid_vel
-
front_obs = 1
-
e = data.pid_error
-
# Calculating deviation for lateral deviation from path
-
kp_error = kp * e
-
kd_error = kd * (e - prev_error)
-
-
# Calculating error for velocity
-
kp_vel_er = kp_vel * e
-
kd_vel_er = kd * (e - prev_error)
-
# ki_error = ki_error + (ki * e)
-
-
vel_error = kp_vel_er + kd_vel_er
-
pid_error = kp_error + kd_error
-
-
-
min_angle=-20
-
max_angle= 20
-
-
# Heigher error results in lower velocity
-
# while lower error results in heigher velocity
-
if dist_in_front <= 5:
-
front_obs = 3#abs(- ( (math.log10(dist_in_front/5)/math.log10(2)) +1 ))
-
velo = vel_input + 1/ ( (abs(vel_error)*front_obs ))
-
print("velo :", velo)
-
#corrected steering angle
-
angle = pid_error
-
-
#print("raw velo:", velo) # Testing
-
-
#Speed limit
-
if velo > 50 :
-
velo = 50
-
-
# Filtering steering angle for Out-of-Range values
-
if angle < min_angle:
-
angle = min_angle
-
elif angle > max_angle:
-
angle = max_angle
-
-
# print("filtered angle :" , angle) # Testing
-
-
-
# Sending Drive information to Car
-
msg = AckermannDriveStamped()
-
msg.header.stamp = rospy.Time.now()
-
msg.header.frame_id = ''
-
msg.drive.speed = velo
-
msg.drive.steering_angle = angle
-
pub.publish(msg)
-
-
if __name__ == '__main__':
-
print("Starting control...")
-
rospy.init_node('pid_controller', anonymous=True)
-
rospy.Subscriber("error", pid_input, control)
-
rospy.spin()
(⊙﹏⊙)
如果按模板写不行吗???
参考1:
-
#!/usr/bin/env python
-
from __future__ import print_function
-
import sys
-
import math
-
import numpy as np
-
-
#ROS Imports
-
import rospy
-
from sensor_msgs.msg import Image, LaserScan
-
from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive
-
-
#PID CONTROL PARAMS
-
kp = 1.0
-
kd = 0.001
-
ki = 0.005
-
servo_offset = 0.0
-
prev_error = 0.0
-
error = 0.0
-
integral = 0.0
-
prev_time = 0.0
-
-
#WALL FOLLOW PARAMS
-
ANGLE_RANGE = 270 # Hokuyo 10LX has 270 degrees scan
-
DESIRED_DISTANCE_RIGHT = 0.9 # meters
-
DESIRED_DISTANCE_LEFT = 0.85
-
VELOCITY = 1.5 # meters per second
-
CAR_LENGTH = 1.0 # Traxxas Rally is 20 inches or 0.5 meters
-
-
class WallFollow:
-
""" Implement Wall Following on the car
-
"""
-
def __init__(self):
-
global prev_time
-
#Topics & Subs, Pubs
-
lidarscan_topic = '/scan'
-
drive_topic = '/nav'
-
prev_time = rospy.get_time()
-
-
self.lidar_sub = rospy.Subscriber(lidarscan_topic, LaserScan, self.lidar_callback)
-
self.drive_pub = rospy.Publisher(drive_topic, AckermannDriveStamped, queue_size = 10)
-
-
def getRange(self, data, angle):
-
# data: single message from topic /scan
-
# angle: between -45 to 225 degrees, where 0 degrees is directly to the right
-
# Outputs length in meters to object with angle in lidar scan field of view
-
#make sure to take care of nans etc.
-
#TODO: implement
-
if angle >= -45 and angle <= 225:
-
iterator = len(data) * (angle + 90) / 360
-
if not np.isnan(data[int(iterator)]) and not np.isinf(data[int(iterator)]):
-
return data[int(iterator)]
-
-
def pid_control(self, error, velocity):
-
global integral
-
global prev_error
-
global kp
-
global ki
-
global kd
-
global prev_time
-
angle = 0.0
-
current_time = rospy.get_time()
-
del_time = current_time - prev_time
-
#TODO: Use kp, ki & kd to implement a PID controller for
-
integral += prev_error * del_time
-
angle = kp * error + ki * integral + kd * (error - prev_error) / del_time
-
prev_error = error
-
prev_time = current_time
-
drive_msg = AckermannDriveStamped()
-
drive_msg.header.stamp = rospy.Time.now()
-
drive_msg.header.frame_id = "laser"
-
drive_msg.drive.steering_angle = -angle
-
if abs(angle) > math.radians(0) and abs(angle) <= math.radians(10):
-
drive_msg.drive.speed = velocity
-
elif abs(angle) > math.radians(10) and abs (angle) <= math.radians(20):
-
drive_msg.drive.speed = 1.0
-
else:
-
drive_msg.drive.speed = 0.5
-
self.drive_pub.publish(drive_msg)
-
-
def followLeft(self, data, leftDist):
-
#Follow left wall as per the algorithm
-
#TODO:implement
-
front_scan_angle = 125
-
back_scan_angle = 180
-
teta = math.radians(abs(front_scan_angle - back_scan_angle))
-
front_scan_dist = self.getRange(data, front_scan_angle)
-
back_scan_dist = self.getRange(data, back_scan_angle)
-
alpha = math.atan2(front_scan_dist * math.cos(teta) - back_scan_dist, front_scan_dist * math.sin(teta))
-
wall_dist = back_scan_dist * math.cos(alpha)
-
ahead_wall_dist = wall_dist + CAR_LENGTH * math.sin(alpha)
-
return leftDist - ahead_wall_dist
-
-
def lidar_callback(self, data):
-
"""
-
"""
-
error = self.followLeft(data.ranges, DESIRED_DISTANCE_LEFT) #TODO: replace with error returned by followLeft
-
#send error to pid_control
-
self.pid_control(error, VELOCITY)
-
-
def main(args):
-
rospy.init_node("WallFollow_node", anonymous=True)
-
wf = WallFollow()
-
rospy.sleep(0.1)
-
rospy.spin()
-
-
if __name__=='__main__':
-
main(sys.argv)
参考2:
-
#!/usr/bin/env python3
-
import sys
-
import math
-
import numpy as np
-
-
#ROS Imports
-
import rospy
-
from sensor_msgs.msg import LaserScan
-
from ackermann_msgs.msg import AckermannDriveStamped
-
-
#PID CONTROL PARAMS
-
kp = 10
-
kd = 70
-
ki = 0.00001
-
prev_error = 0
-
integral = 0
-
-
#WALL FOLLOW PARAMS
-
ANGLE_RANGE = 270 # Hokuyo 10LX has 270 degrees scan
-
-
class WallFollow:
-
""" Implement Wall Following on the car
-
"""
-
def __init__(self):
-
self.rate = rospy.Rate(10)
-
self.lidar_sub = rospy.Subscriber('/scan' , LaserScan, self.lidar_callback, queue_size = 1)
-
self.drive_pub = rospy.Publisher('/nav', AckermannDriveStamped, queue_size = 1)
-
-
def getRange(self, data, angle, distance):
-
self.Dt_max = False
-
angle_btwnScan = 70
-
future_dist = 0.55
-
theta = angle[int((180-angle_btwnScan+45)/ANGLE_RANGE * len(angle))]
-
a = distance[int((180-angle_btwnScan+45)/ANGLE_RANGE * len(angle))]
-
b = distance[int((180+45)/ANGLE_RANGE * len(angle))]
-
alpha = math.atan((a*np.cos(np.pi-theta) - b)/a*np.sin(np.pi-theta))
-
Dt = b*np.cos(alpha)
-
Dt1 = Dt + future_dist*np.sin(alpha)
-
# check condition for bottom part of map
-
a2 = a = distance[int((180-15+45)/ANGLE_RANGE * len(angle))]
-
theta2 = angle[int((180-15+45)/ANGLE_RANGE * len(angle))]
-
alpha2 = math.atan((a2*np.cos(np.pi-theta2) - b)/a2*np.sin(np.pi-theta2))
-
bot_error = a2*np.cos(np.pi-theta2+alpha2)
-
Dt2 = b*np.cos(alpha2)
-
self.bot_state = math.isclose(bot_error, Dt2, rel_tol = 0.1)
-
if Dt > 1.1:
-
self.Dt_max = True
-
return Dt1
-
-
def filter_scan(self, scan_msg):
-
angle_range = enumerate(scan_msg.ranges)
-
filter_data = [[count*scan_msg.angle_increment, val] for count, val in angle_range if not np.isinf(val) and not np.isnan(val)]
-
filter_data = np.array(filter_data)
-
angles = filter_data[:,0]
-
distance = filter_data[:,1]
-
filter_angle = np.array([])
-
idx = 0
-
start_idx = 0
-
end_idx = 0
-
for angle in angles:
-
if (not 0 <= angle < np.pi/4) and (not 7*np.pi/4 < angle <= 2*np.pi):
-
if start_idx == 0:
-
start_idx = idx
-
angle -= np.pi/2
-
filter_angle = np.append(filter_angle, angle)
-
if end_idx == 0 and angle > 7*np.pi/4:
-
end_idx = idx - 1
-
idx += 1
-
distance = distance[start_idx: end_idx+1]
-
return filter_angle, distance
-
-
def pid_control(self, error):
-
global integral
-
global prev_error
-
global kp
-
global ki
-
global kd
-
integral += error
-
angle = kp*error + kd*(error - prev_error) + ki*integral
-
prev_error = error
-
if self.bot_state == True and self.Dt_max == True:
-
angle = -0.01*np.pi/180
-
if -np.pi/18 < angle < np.pi/18:
-
velocity = 1.5
-
elif -np.pi/9 < angle <= -np.pi/18 or np.pi/18 <= angle < np.pi/9:
-
velocity = 1
-
else:
-
velocity = 0.5
-
drive_msg = AckermannDriveStamped()
-
drive_msg.header.stamp = rospy.Time.now()
-
drive_msg.header.frame_id = "laser"
-
drive_msg.drive.steering_angle = angle
-
drive_msg.drive.speed = velocity
-
self.drive_pub.publish(drive_msg)
-
-
def followLeft(self, leftDist):
-
distToWall = 0.55
-
error = -(distToWall - leftDist)
-
return error
-
-
def lidar_callback(self, data):
-
try:
-
angle, distance = self.filter_scan(data)
-
Dt1 = self.getRange(data, angle, distance)
-
error = self.followLeft(Dt1)
-
self.pid_control(error)
-
except rospy.ROSException as e:
-
# rospy.logerr(e)
-
pass
-
-
def main(args):
-
rospy.init_node("WallFollow_node", anonymous=True)
-
wf = WallFollow()
-
rospy.spin()
-
-
if __name__=='__main__':
-
main(sys.argv)
试一试看:
-
# Keyboard characters for toggling mux
-
joy_key_char: "j"
-
keyboard_key_char: "k"
-
brake_key_char: "b"
-
random_walk_key_char: "r"
-
nav_key_char: "n"
-
# **Add button for new planning method here**
-
new_key_char: "z"
文章来源: zhangrelay.blog.csdn.net,作者:zhangrelay,版权归原作者所有,如需转载,请联系作者。
原文链接:zhangrelay.blog.csdn.net/article/details/124547642
【版权声明】本文为华为云社区用户转载文章,如果您发现本社区中有涉嫌抄袭的内容,欢迎发送邮件进行举报,并提供相关证据,一经查实,本社区将立刻删除涉嫌侵权内容,举报邮箱:
cloudbbs@huaweicloud.com
- 点赞
- 收藏
- 关注作者
评论(0)