#勤写标兵挑战赛#

本节实现后，效果如下：

主要有3个点：

• 车体模型的简单配置
• 赛道环境的修改
• 算法参数的简要解释

↓开启本节之前，需要完成↓：

一键配置←f1tenth和PID绕圈

(*Φ皿Φ*) 

车体模型：

原有的车体模型如下：

蓝色车体和黑色轮子。

修改如下代码：

例如，黄色车身（yellow）和银灰色轮子（silvergray）。先添加对应颜色：

颜色名称:银灰色,颜色RGB代码:A9A6AB 

银灰色的Rgb值都是统称为银色的,它的标准颜色16进制的RGB数值是:#c0c0c0,标准颜色十进制RGB数值是:红(192) 绿(192) 蓝(192)。

  <material name="black">
    <color rgba="0.2 0.2 0.2 1."/>
  </material>
 
  <material name="blue">
    <color rgba="0.3 0.57 1. 1."/>
  </material>
 
  <material name="silvergray">
    <color rgba="0.6 0.6 0.6 1."/>
  </material>
 
  <material name="yellow">
    <color rgba="0.8 0.8 0.0 1."/>
  </material>

然后，修改相关参数：

车体

  <link name="base_link">
    <visual>
      <origin xyz="${wheelbase/2} 0 ${ground_offset+height/2}"/>
      <geometry>
        <box size="${wheelbase} ${width} ${height}"/>
      </geometry>
      <material name="yellow"/>
    </visual>
  </link>

 轮子-此处修改前轮也就是转向轮

  <link name="front_left_wheel">
    <visual>
      <geometry>
        <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
      </geometry>
      <material name="silvergray"/>
      <origin rpy="${pi/2} 0 0"/>
    </visual>
  </link>
 
  <joint name="base_to_front_right_hinge" type="fixed">
    <parent link="base_link"/>
    <child link="front_right_hinge"/>
    <origin xyz="${wheelbase} ${-(wheel_length+width)/2} ${wheel_radius}"/>
  </joint>
 
  <link name="front_right_hinge"/>
 
  <joint name="front_right_hinge_to_wheel" type="continuous">
    <parent link="front_right_hinge"/>
    <child link="front_right_wheel"/>
  </joint>
 
  <link name="front_right_wheel">
    <visual>
      <geometry>
        <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
      </geometry>
      <material name="silvergray"/>
      <origin rpy="${pi/2} 0 0"/>
    </visual>
  </link>

然后再启动仿真看效果。

AI讲解：

要配置F1Tenth车体模型，需要进行以下步骤：

1. 下载F1Tenth车体模型文件，可以从GitHub等网站上下载。

2. 安装ROS（机器人操作系统）和Gazebo（仿真软件），确保系统已经安装了所需的依赖项。

3. 将车体模型文件放置在ROS工作空间的src目录下。

4. 编译ROS工作空间，以确保模型文件被正确编译。

5. 启动Gazebo仿真器，使用ROS命令行工具启动仿真器。

6. 在仿真器中加载车体模型文件，可以使用ROS的Launch文件或直接在仿真器中加载。

7. 配置车体模型的参数，如车轮半径、质量等。

8. 测试车体模型的运动，可以使用ROS的控制节点来控制车体的运动，也可以手动控制。

需要注意的是，F1Tenth车体模型的配置可能因具体的应用场景而有所不同，上述步骤仅供参考。

赛道环境

内置赛道有如下：

有没有可以搞算法锦标赛的感觉？O(∩_∩)O

修改如下文件

  <!-- Launch a map from the maps folder-->
  <arg name="map" default="$(find f1tenth_simulator)/maps/levine.yaml"/>
  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map)"/>

将levine修改为需要的地图。

  <!-- Launch a map from the maps folder-->
  <arg name="map" default="$(find f1tenth_simulator)/maps/berlin.yaml"/>
  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map)"/>

由于云端系统，无需外接游戏手柄控制，删除如下内容：

  <!-- Listen to messages from joysicks -->
  <node pkg="joy" name="joy_node" type="joy_node"/> 

完成后启动：

AI讲解：

F1tenth赛道环境可以通过修改赛道的形状、颜色、纹理等方式进行修改。具体的修改方法如下：

1. 修改赛道的形状：可以通过修改赛道的坐标点位置来改变赛道的形状。可以使用CAD软件或者其他绘图工具来绘制赛道的形状，然后将绘制的形状转换成坐标点的形式，再将坐标点导入到F1tenth环境中。
2. 修改赛道的颜色：可以通过修改赛道的材质属性来改变赛道的颜色。可以在F1tenth环境中选择赛道对象，然后修改对象的材质属性，如颜色、透明度等。
3. 修改赛道的纹理：可以通过添加纹理图像来给赛道增加纹理效果。可以在F1tenth环境中选择赛道对象，然后添加纹理图像，调整纹理图像的大小和位置，以实现想要的效果。

总之，F1tenth赛道环境可以通过多种方式进行修改，使得赛道更加逼真、多样化。

算法参数

使用算法如下：

#!/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)

控制周期修改小一点：

rospy.sleep(0.05)

20Hz对应0.05，10Hz对应0.1。

        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)

两个控制量：

1. 前轮转向角steering_angle
2. 速度控制speed

快速跑起来。需要调节合适参数，先不要求修改控制器，也就是PID足矣，学有余力可以试一试MPC控制。

AI讲解：

F1Tenth巡线PID算法是一种基于比例-积分-微分控制的算法，用于控制自动驾驶车辆在指定路径上行驶。该算法通过对车辆的速度和转向角度进行控制，使车辆能够沿着指定路径行驶，并保持在路径中心线上。

该算法的主要思路是根据车辆当前位置和目标位置之间的误差来计算控制指令，然后将指令输入到车辆控制系统中，以实现车辆的运动控制。具体实现步骤如下：

1 获取当前车辆位置和目标位置

算法首先需要获取车辆当前的位置和目标位置。当前位置可以通过传感器获取，例如GPS、激光雷达等，而目标位置则由路径规划算法提供。

2 计算误差

根据当前位置和目标位置，可以计算出车辆的误差，即偏离路径中心线的距离。偏离路径中心线的距离越大，误差也越大。

3 计算PID控制指令

根据误差，可以计算出PID控制器的三个参数：比例系数、积分系数和微分系数。比例系数用于控制车辆的转向角度，积分系数用于消除系统的稳态误差，微分系数用于消除系统的瞬态误差。

4 输入控制指令

将计算出的PID控制指令输入到车辆控制系统中，控制车辆的速度和转向角度。如果车辆偏离了路径中心线，PID控制器会自动调整控制指令，使车辆重新回到路径中心线上。

总结来说，F1Tenth巡线PID算法是一种基于误差计算和PID控制的算法，通过对车辆的速度和转向角度进行控制，使车辆能够沿着指定路径行驶，并保持在路径中心线上。该算法具有简单、高效、稳定等优点，在自动驾驶领域得到了广泛应用。