ROS从入门到放弃 —— 玩一玩F1TENTH

• 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)$是我们转角的度数
• $e(t)$是设定值和当前值的差值，我们这个例子中是我们希望的和墙壁的夹角和实际夹角之间的差距。
  

1.2 跟墙走！

1.2.1 如何测量和墙壁之间的夹角:

• 记$D_t$为t时刻车辆中心与墙壁之间的距离。
  

由图，我们可以计算出夹角$\alpha$
$$\alpha =arctan(\frac{a\ast cos(\theta )-b}{a\ast sin(\theta )})$$
因此，我们可以得到当前汽车和右墙的距离：
$$D_t=b\ast cos(\alpha )$$
因此，我们可以根据我们实时测出来的数据$D_{mes}$计算出误差
$$e(t)=D_{mes}-D_t$$
但是因为我们汽车速度过快，为了防止我们的汽车撞墙，我们可以使用一个提前量$L$，
$$D_{t+1}=D_t+L\ast sin(\alpha )$$

具体操作

创建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)

结果：

车车开起来！