在此案例前需完成：
☞ 蓝桥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"
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）