蓝桥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 metersclass WallFollow:""" Implement Wall Following on the car"""def __init__(self):#Topics & Subs, Pubslidarscan_topic = '/scan'drive_topic = '/nav'self.lidar_sub = #TODO: Subscribe to LIDARself.drive_pub = #TODO: Publish to drivedef 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: implementreturn 0.0def pid_control(self, error, velocity):global integralglobal prev_errorglobal kpglobal kiglobal kdangle = 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 = angledrive_msg.drive.speed = velocityself.drive_pub.publish(drive_msg)def followLeft(self, data, leftDist):#Follow left wall as per the algorithm #TODO:implementreturn 0.0 def lidar_callback(self, data):""" """error = 0.0 #TODO: replace with error returned by followLeft#send error to pid_controlself.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 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 rospy
import math
from sensor_msgs.msg import LaserScan
from race.msg import pid_inputangle_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.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 rospy
from race.msg import pid_input
from ackermann_msgs.msg import AckermannDriveStampedkp = 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 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 < min_angle:angle = min_angleelif 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 = velo   msg.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 rospy
import math
from sensor_msgs.msg import LaserScan
from race.msg import pid_inputangle_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.0pub = 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_factorfor 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):dist_in_front = getRange(data, 180)theta = 30left_dist = getRange(data, 270)right_dist = getRange(data,90)    # Ray perpendicular to right side of cara_right = getRange(data,(90 + theta)) # Ray at degree theta from right_dista_left = getRange(data,(270 - theta))theta = math.radians(theta)# Calculating the deviation of steering(alpha) from rightalpha_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 Positionfut_pos_r = curr_pos_r + car_length * math.sin(alpha_r) # projection in Future Position# Calculating the deviation of steering(alpha) from leftalpha_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 Positionfut_pos_l = curr_pos_l + car_length * math.sin(alpha_l) # projection in Future Positionerror = - (fut_pos_r - fut_pos_l) # total error# print('error: ', error) #Testing# Sending PID error to Controlmsg = pid_input()msg.pid_error = errormsg.pid_vel = dist_in_front # pid_vel used for distance in frontpub.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 AckermannDriveStampedkp = 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 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 <= 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 angleangle = pid_error#print("raw velo:", velo) # Testing#Speed limitif velo > 50 :velo = 50# Filtering steering angle for Out-of-Range valuesif angle < min_angle:angle = min_angleelif 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 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 metersclass WallFollow:""" Implement Wall Following on the car"""def __init__(self):global prev_time#Topics & Subs, Pubslidarscan_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: implementif angle >= -45 and angle <= 225:iterator = len(data) * (angle + 90) / 360if not np.isnan(data[int(iterator)]) and not np.isinf(data[int(iterator)]):return data[int(iterator)]def pid_control(self, error, velocity):global integralglobal prev_errorglobal kpglobal kiglobal kdglobal prev_timeangle = 0.0current_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_timeangle = kp * error + ki * integral + kd * (error - prev_error) / del_timeprev_error = errorprev_time = current_timedrive_msg = AckermannDriveStamped()drive_msg.header.stamp = rospy.Time.now()drive_msg.header.frame_id = "laser"drive_msg.drive.steering_angle = -angleif abs(angle) > math.radians(0) and abs(angle) <= math.radians(10):drive_msg.drive.speed = velocityelif abs(angle) > math.radians(10) and abs (angle) <= math.radians(20):drive_msg.drive.speed = 1.0else:drive_msg.drive.speed = 0.5self.drive_pub.publish(drive_msg)def followLeft(self, data, leftDist):#Follow left wall as per the algorithm #TODO:implementfront_scan_angle = 125back_scan_angle = 180teta = 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_distdef lidar_callback(self, data):""" """error = self.followLeft(data.ranges, DESIRED_DISTANCE_LEFT) #TODO: replace with error returned by followLeft#send error to pid_controlself.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 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 <= 2*np.pi):if start_idx == 0:start_idx = idxangle -= np.pi/2filter_angle = np.append(filter_angle, angle)if end_idx == 0 and 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 errordef 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 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"

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

蓝桥ROS之f1tenth案例学习与调试（成功）
失败案例: 蓝桥ROS之f1tenth案例学习与调试(失败) 其实这个版本是适用于kinetic/melodic/noetic 为何失败呢? 需要将kinetic功能包全部更新到20210503版本! ...
蓝桥ROS之f1tenth案例学习与调试（失败）
过程和pc端完全一致: ROS域名解析问题记录(蓝桥云课ros.asc) f1tenth案例学习与调试ros1版本pc端在蓝桥云课ROS中测试一下是否可行(kinetic/melodic ...
蓝桥ROS云课→一键配置←f1tenth和PID绕圈
虽然是一键配置,但还是需要若干步骤的. 参考: ROS1云课→18一键配置蓝桥ROS之f1tenth案例学习与调试(成功) 蓝桥ROS之f1tenth简单PID沿墙跑起来(Python) 一键升级脚 ...
蓝桥ROS→f1tenth和PID绕圈←外传
#勤写标兵挑战赛# 本节实现后,效果如下: 主要有3个点: 车体模型的简单配置赛道环境的修改算法参数的简要解释 ↓开启本节之前,需要完成↓: 一键配置←f1tenth和PID绕圈 (*Φ皿Φ*) ...
蓝桥ROS机器人之STDR沿墙跑
效果如何呢,请看下图: 跑一下看看: 启动环境如下: <launch><include file="$(find stdr_robot)/launch/robot_mana ...
蓝桥ROS之半自动贪吃龟turtlesim版
为啥不是全自动?全自动是作业,半自动是提示. 一步一步完成吧,非常简单. 第一步:打开蓝桥ROS www.lanqiao.cn/courses/854 第二步:双击xfce终端,分别在不同窗口开启ro ...
ros如何编译python文件_Python为ROS编写一个简单的发布者和订阅者
Python为ROS编写一个简单的发布者和订阅者 1.创建工作空间 1.1建立文件夹hello_rospy,再在该目录下建立子目录src,并创建工作空间 mkdir -p ~/hello_rospy/ ...
Unity简单实现图片墙功能
Unity简单实现图片墙功能前言在做之前公司的项目中,我做过很多实现照片墙效果的功能.其中我觉得我做的效果比较好而且比较有难度的就是雀巢项目中的那个仿照apple watch拖拽效果实现的那个照片 ...
html版本的简单的表白墙
实现一个简单的表白墙(html) 实现一个这样的表白墙 <!DOCTYPE html> <html lang="en"> <head><m ...

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

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

最新文章

热门文章