ROS从入门到放弃 —— 玩一玩F1TENTH
ROS从入门到放弃 —— 玩一玩F1TENTH
- 0. 准备工作
-
- 0.1 开车车
- 0.2 学习如何控制车车！
- 1. 让车车跟墙墙走
-
- 1.1 PID
- 1.2 跟墙走！
-
- 1.2.1 如何测量和墙壁之间的夹角:
- 具体操作
-
- 具体代码：
- 结果：
- F1TENTH官网
- F1TENTH安装
0. 准备工作
- 这里我们先安装一下F1TENTH。
- 如果使用ROS kinetic，可以用这样的方式安装
sudo apt-get install ros-kinetic-tf2-geometry-msgs ros-kinetic-ackermann-msgs ros-kinetic-joy ros-kinetic-map-server
cd ~/catkin_ws/src
git clone https://github.com/f1tenth/f1tenth_simulator.git
cd ~/catkin_ws
catkin_make
source devel/setup.bash
- 期间可能会出在catkin_make出现的问题：
解决办法：
观察具体安装的版本
然后
cd /opt/ros/kinetic/lib/
sudo ln -s liborocos-kdl.so.1.3.0 liborocos-kdl.so.1.3.2
结果：
0.1 开车车
使用下面的代码打开Simulation，并对着终端按下k。之后我们就可以用wasd来控制汽车移动了，并用空格让汽车停下。
roslaunch f1tenth_simulator simulator.launch
并且可以用rviz里面的2d Pose Estimate来重置车车的位置，下面是系统运行时候的rqt_graph。
0.2 学习如何控制车车！
- 话题：
/key - 消息类型:
std_msgs/String - 具体定义：
string data
我们使用rostopic info /key可以看到这个topic使用的消息类型：std_msgs/String。
而这个消息具体的定义：rosmsg show std_msgs/String：string data
其他的一些参数的定义可见：
Advanced Information
1. 让车车跟墙墙走
参考教程
具体内容
PID
让车跟墙走
1.1 PID
- 用于闭环控制
- u ( t ) u(t) u(t)是我们转角的度数
- e ( t ) e(t) e(t)是设定值和当前值的差值，我们这个例子中是我们希望的和墙壁的夹角和实际夹角之间的差距。
1.2 跟墙走！
1.2.1 如何测量和墙壁之间的夹角:
- 记 D t D_t Dt为t时刻车辆中心与墙壁之间的距离。
由图，我们可以计算出夹角 α \alpha α
α = a r c t a n ( a ∗ c o s ( θ ) − b a ∗ s i n ( θ ) ) \alpha = arctan(\frac{a*cos(\theta)-b}{a*sin(\theta)}) α=arctan(a∗sin(θ)a∗cos(θ)−b)
因此，我们可以得到当前汽车和右墙的距离：
D t = b ∗ c o s ( α ) D_t = b*cos(\alpha) Dt=b∗cos(α)
因此，我们可以根据我们实时测出来的数据 D m e s D_{mes} Dmes计算出误差
e ( t ) = D m e s − D t e(t)=D_{mes}-D_t e(t)=Dmes−Dt
但是因为我们汽车速度过快，为了防止我们的汽车撞墙，我们可以使用一个提前量 L L L，
D t + 1 = D t + L ∗ s i n ( α ) D_{t+1}=D_t+L*sin(\alpha) Dt+1=Dt+L∗sin(α)
具体操作
创建pkg，创建scripts，创建wall_follow.py。
catkin_create_pkg wallfollow std_msgs rospy roscpp
roscd wallfollow
create scripts
cd scripts
touch wall_follow.py
chmod +x wall_follow.py
cd ~/catkin_ws
catkin_make
看消息
header:
seq: 3256804
stamp:
secs: 1608304084
nsecs: 280176187
frame_id: "laser"
angle_min: -3.14159274101
angle_max: 3.14159274101
angle_increment: 0.00582315586507
time_increment: 0.0
scan_time: 0.0
range_min: 0.0
range_max: 100.0
ranges: [...]
intensities: [...]
他是从-pi到pi，每次增加0.00582315586507。
具体代码：
#!/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
#TODO kp,kd,ki
kp = -0.6
kd = -0.3
ki = 0
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 = '/drive' # 注意这里的topic是drive!
self.lidar_sub = rospy.Subscriber(lidarscan_topic, LaserScan, self.lidar_callback) #TODO: Subscribe to LIDAR
self.drive_pub = rospy.Publisher(drive_topic, AckermannDriveStamped, queue_size=10) #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
# To calculate range for the -45 degrees and 225 degrees
# -pi -> 0
# -pi/2 = 0 degrees -> pi/2/0.005823 = 270
# -45 degrees -> pi/4/0.005823 = 135
# 225 degrees -> 7*pi/4/0.005823 = 945
index = (angle+45)*3+135
return data.ranges[index]
def pid_control(self, error, velocity):
global integral
global prev_error
global kp
global ki
global kd
angle = 0.0 # in radian
#TODO: Use kp, ki & kd to implement a PID controller for
integral += prev_error
angle = kp*error+ ki*integral+ kd*(error-prev_error)
prev_error = error # Update the pre_error
# calculate the velocity
ratio = math.pi/180
if 0<=angle and angle <10*ratio:
velocity = 1.5 # meter
elif 10*ratio<=angle and angle <20*ratio:
velocity = 1 # meter
else:
velocity = 0.5
velocity *= 2
#print("Error ",error,"angle ",angle,"angle in degree", angle/ratio," velocity ",velocity)
print("Error ",error)
#velocity=0
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
# Calculate all the parameters
theta = 45
a,b = self.getRange(data,0),self.getRange(data,theta)
alpha = math.atan((a*math.cos(theta)-b)/(a*math.sin(theta)))
Dt = b*math.cos(alpha)
Dt1 = Dt + 0.5*math.sin(alpha) # D_t+1, L=0.5
# Return the error, which is the difference between 'current' value and target value
return Dt1-leftDist
def lidar_callback(self, data):
"""
"""
#TODO: replace with error returned by followLeft
error = self.followLeft(data,DESIRED_DISTANCE_LEFT)
#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)
结果：
