蓝桥ROS之f1tenth简单PID沿墙跑起来（Python）
在此案例前需完成：
☞ 蓝桥ROS之f1tenth案例学习与调试（成功）
遥控肯定不过瘾，那么如何用一个PID程序使小车自动沿墙跑呢？？？
公式如上……
跑一跑看看？
阅读pdf文档：
python程序模板：
#!/usr/bin/env pythonfrom __future__ import print_functionimport sysimport mathimport numpy as np#ROS Importsimport rospyfrom sensor_msgs.msg import Image, LaserScanfrom ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive#PID CONTROL PARAMSkp = #TODOkd = #TODOki = #TODOservo_offset = 0.0prev_error = 0.0 error = 0.0integral = 0.0#WALL FOLLOW PARAMSANGLE_RANGE = 270 # Hokuyo 10LX has 270 degrees scanDESIRED_DISTANCE_RIGHT = 0.9 # metersDESIRED_DISTANCE_LEFT = 0.55VELOCITY = 2.00 # meters per secondCAR_LENGTH = 0.50 # Traxxas Rally is 20 inches or 0.5 metersclass 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 rospyfrom sensor_msgs.msg import LaserScandef callback(data): print(data.ranges[270])rospy.init_node("scan_test", anonymous=False)sub = rospy.Subscriber("/scan", LaserScan, callback)rospy.spin()dist.py
import rospyimport mathfrom sensor_msgs.msg import LaserScanfrom race.msg import pid_inputangle_range = 360# sensor angle range of the lidarcar_length = 1.5# projection distance we project car forward. vel = 1.5 # used for pid_vel (not much use).error = 0.0dist_from_wall = 0.8pub = rospy.Publisher('error', pid_input, queue_size=10)def getRange(data,theta):angle_range = data.angle_max - data.angle_minangle_increment = data.angle_incrementscan_range = []rad2deg_factor = 57.296angle_range *= rad2deg_factorangle_increment *= rad2deg_factor for element in data.ranges:if math.isnan(element) or math.isinf(element):element = 100scan_range.append(element)index = round(theta / angle_increment)return scan_range[index]def callback(data):theta = 55left_dist = float(getRange(data, 270))a = getRange(data,(90 + theta)) # Ray at degree theta from right_distright_dist = getRange(data,90)# Ray perpendicular to right side of cartheta_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 PositionCD = AB + car_length * math.sin(alpha) # projection in Future Positionerror = dist_from_wall - CD # total error# print('error: ', error) #Testing# Sending PID error to Controlmsg = pid_input()msg.pid_error = errormsg.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 rospyfrom race.msg import pid_inputfrom ackermann_msgs.msg import AckermannDriveStampedkp = 0.7 #45kd = 0.00125#0.001 #0.09ki = 0#0.5kp_vel = 6 #9 is using keep in middlekd_vel = 0.0001ki_error = 0prev_error = 0.0 vel_input = 2.5# base velocityangle = 0.0# initial steering anglepub = rospy.Publisher("/drive", AckermannDriveStamped, queue_size=5)def control(data):global prev_errorglobal vel_inputglobal kpglobal kp_velglobal kdglobal kd_velglobal kiglobal anglee = data.pid_error# Calculating deviation for lateral deviation from pathkp_error = kp * ekd_error = kd * (e - prev_error)# Calculating error for velocitykp_vel_er = kp_vel * ekd_vel_er = kd * (e - prev_error)# ki_error = ki_error + (ki * e)vel_error = kp_vel_er + kd_vel_erpid_error = kp_error + kd_errormin_angle=-20max_angle= 20# Heigher error results in lower velocity # while lower error results in heigher velocityvelo = vel_input + 1/(abs(vel_error))#corrected steering angleangle = pid_error#print("raw velo:", velo) # Testing#Speed limitif velo > 15 :velo = 10 # Filtering steering angle for Out-of-Range valuesif angle max_angle:angle = max_angle# print("filtered angle :" , angle) # Testing# Sending Drive information to Carmsg = AckermannDriveStamped()msg.header.stamp = rospy.Time.now()msg.header.frame_id = ''msg.drive.speed = velomsg.drive.steering_angle = anglepub.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 rospyimport mathfrom sensor_msgs.msg import LaserScanfrom race.msg import pid_inputangle_range = 360# sensor angle range of the lidarcar_length = 1.5# projection distance we project car forward. vel = 1.0 # used for pid_vel (not much use).error = 0.0pub = 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 rospyimport mathfrom race.msg import pid_inputfrom ackermann_msgs.msg import AckermannDriveStampedkp = 0.27# 0.27 for portokd = 0.0125#0.001 #0.09ki = 0 #0.5kp_vel = 0.9 #9 is using keep in middlekd_vel = 0.0001ki_error = 0prev_error = 0.0 vel_input = 2.5# base velocityangle = 0.0# initial steering anglepub = rospy.Publisher("/drive", AckermannDriveStamped, queue_size=5)def control(data):global prev_errorglobal vel_inputglobal kpglobal kp_velglobal kdglobal kd_velglobal kiglobal angledist_in_front = data.pid_velfront_obs = 1e = data.pid_error# Calculating deviation for lateral deviation from pathkp_error = kp * ekd_error = kd * (e - prev_error)# Calculating error for velocitykp_vel_er = kp_vel * ekd_vel_er = kd * (e - prev_error)# ki_error = ki_error + (ki * e)vel_error = kp_vel_er + kd_vel_erpid_error = kp_error + kd_errormin_angle=-20max_angle= 20# Heigher error results in lower velocity# while lower error results in heigher velocityif dist_in_front 50 :velo = 50# Filtering steering angle for Out-of-Range valuesif angle max_angle:angle = max_angle# print("filtered angle :" , angle) # Testing# Sending Drive information to Carmsg = AckermannDriveStamped()msg.header.stamp = rospy.Time.now()msg.header.frame_id = ''msg.drive.speed = velomsg.drive.steering_angle = anglepub.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 pythonfrom __future__ import print_functionimport sysimport mathimport numpy as np#ROS Importsimport rospyfrom sensor_msgs.msg import Image, LaserScanfrom ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive#PID CONTROL PARAMSkp = 1.0kd = 0.001ki = 0.005servo_offset = 0.0prev_error = 0.0 error = 0.0integral = 0.0prev_time = 0.0#WALL FOLLOW PARAMSANGLE_RANGE = 270 # Hokuyo 10LX has 270 degrees scanDESIRED_DISTANCE_RIGHT = 0.9 # metersDESIRED_DISTANCE_LEFT = 0.85VELOCITY = 1.5 # meters per secondCAR_LENGTH = 1.0 # Traxxas Rally is 20 inches or 0.5 metersclass 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 math.radians(0) and 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 python3import sysimport mathimport numpy as np#ROS Importsimport rospyfrom sensor_msgs.msg import LaserScanfrom ackermann_msgs.msg import AckermannDriveStamped#PID CONTROL PARAMSkp = 10kd = 70ki = 0.00001prev_error = 0 integral = 0#WALL FOLLOW PARAMSANGLE_RANGE = 270 # Hokuyo 10LX has 270 degrees scanclass 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 = Falseangle_btwnScan = 70future_dist = 0.55theta = 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 mapa2 = 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 = Truereturn Dt1def 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 = 0start_idx = 0end_idx = 0for angle in angles:if (not 0 <= angle < np.pi/4) and (not 7*np.pi/4 < angle 7*np.pi/4:end_idx = idx - 1idx += 1distance = distance[start_idx: end_idx+1]return filter_angle, distancedef pid_control(self, error):global integralglobal prev_errorglobal kpglobal kiglobal kdintegral += errorangle = kp*error + kd*(error - prev_error) + ki*integralprev_error = errorif self.bot_state == True and self.Dt_max == True:angle = -0.01*np.pi/180if -np.pi/18 < angle < np.pi/18:velocity = 1.5elif -np.pi/9 < angle <= -np.pi/18 or np.pi/18 <= angle < np.pi/9:velocity = 1else:velocity = 0.5drive_msg = AckermannDriveStamped()drive_msg.header.stamp = rospy.Time.now()drive_msg.header.frame_id = "laser"drive_msg.drive.steering_angle = angledrive_msg.drive.speed = velocityself.drive_pub.publish(drive_msg)def followLeft(self, leftDist):distToWall = 0.55error = -(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)passdef main(args):rospy.init_node("WallFollow_node", anonymous=True)wf = WallFollow()rospy.spin()if __name__=='__main__':main(sys.argv)试一试看：
# Keyboard characters for toggling muxjoy_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"
创作打卡挑战赛 
赢取流量/现金/CSDN周边激励大奖