蓝桥ROS之f1tenth简单PID沿墙跑起来（Python）

在此案例前需完成：

☞ ​​​​​​蓝桥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\"