藍橋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"

Python

版權聲明：本文內容由網絡用戶投稿，版權歸原作者所有，本站不擁有其著作權，亦不承擔相應法律責任。如果您發現本站中有涉嫌抄襲或描述失實的內容，請聯系我們jiasou666@gmail.com 處理，核實后本網站將在24小時內刪除侵權內容。