蓝桥ROS云课一键配置f1tenth和PID绕圈（竞赛更新版）

zhangrelay

发布于 2024-05-26 15:01:19

1210

发布于 2024-05-26 15:01:19

文章被收录于专栏：机器人课程与技术

准备资料

竞赛说明：

ROS机器人虚拟仿真挑战赛学习笔记

之前版本：

蓝桥ROS云课→一键配置←f1tenth和PID绕圈

修改定制

提高调试效率，非常关键^_^

配置文件：

echo "Upgrade Mission Begins."
 
echo "-----BEGIN PGP PUBLIC KEY BLOCK-----
Version: GnuPG v1
 
mQINBFzvJpYBEADY8l1YvO7iYW5gUESyzsTGnMvVUmlV3XarBaJz9bGRmgPXh7jc
VFrQhE0L/HV7LOfoLI9H2GWYyHBqN5ERBlcA8XxG3ZvX7t9nAZPQT2Xxe3GT3tro
u5oCR+SyHN9xPnUwDuqUSvJ2eqMYb9B/Hph3OmtjG30jSNq9kOF5bBTk1hOTGPH4
K/AY0jzT6OpHfXU6ytlFsI47ZKsnTUhipGsKucQ1CXlyirndZ3V3k70YaooZ55rG
aIoAWlx2H0J7sAHmqS29N9jV9mo135d+d+TdLBXI0PXtiHzE9IPaX+ctdSUrPnp+
TwR99lxglpIG6hLuvOMAaxiqFBB/Jf3XJ8OBakfS6nHrWH2WqQxRbiITl0irkQoz
pwNEF2Bv0+Jvs1UFEdVGz5a8xexQHst/RmKrtHLct3iOCvBNqoAQRbvWvBhPjO/p
V5cYeUljZ5wpHyFkaEViClaVWqa6PIsyLqmyjsruPCWlURLsQoQxABcL8bwxX7UT
hM6CtH6tGlYZ85RIzRifIm2oudzV5l+8oRgFr9yVcwyOFT6JCioqkwldW52P1pk/
/SnuexC6LYqqDuHUs5NnokzzpfS6QaWfTY5P5tz4KHJfsjDIktly3mKVfY0fSPVV
okdGpcUzvz2hq1fqjxB6MlB/1vtk0bImfcsoxBmF7H+4E9ZN1sX/tSb0KQARAQAB
tCZPcGVuIFJvYm90aWNzIDxpbmZvQG9zcmZvdW5kYXRpb24ub3JnPokCVAQTAQgA
PgIbAwULCQgHAgYVCgkICwIEFgIDAQIeAQIXgBYhBMHPbjHmut6IaLFytPQu1vur
F8ZUBQJgsdhRBQkLTMW7AAoJEPQu1vurF8ZUTMwP/3f7EkOPIFjUdRmpNJ2db4iB
RQu5b2SJRG+KIdbvQBzKUBMV6/RUhEDPjhXZI3zDevzBewvAMKkqs2Q1cWo9WV7Z
PyTkvSyey/Tjn+PozcdvzkvrEjDMftIk8E1WzLGq7vnPLZ1q/b6Vq4H373Z+EDWa
DaDwW72CbCBLWAVtqff80CwlI2x8fYHKr3VBUnwcXNHR4+nRABfAWnaU4k+oTshC
Qucsd8vitNfsSXrKuKyz91IRHRPnJjx8UvGU4tRGfrHkw1505EZvgP02vXeRyWBR
fKiL1vGy4tCSRDdZO3ms2J2m08VPv65HsHaWYMnO+rNJmMZj9d9JdL/9GRf5F6U0
quoIFL39BhUEvBynuqlrqistnyOhw8W/IQy/ymNzBMcMz6rcMjMwhkgm/LNXoSD1
1OrJu4ktQwRhwvGVarnB8ihwjsTxZFylaLmFSfaA+OAlOqCLS1OkIVMzjW+Ul6A6
qjiCEUOsnlf4CGlhzNMZOx3low6ixzEqKOcfECpeIj80a2fBDmWkcAAjlHu6VBhA
TUDG9e2xKLzV2Z/DLYsb3+n9QW7KO0yZKfiuUo6AYboAioQKn5jh3iRvjGh2Ujpo
22G+oae3PcCc7G+z12j6xIY709FQuA49dA2YpzMda0/OX4LP56STEveDRrO+CnV6
WE+F5FaIKwb72PL4rLi4
=i0tj
-----END PGP PUBLIC KEY BLOCK-----" >> ~/ros.asc
 
sudo apt-key add ros.asc
 
git clone https://gitcode.net/ZhangRelay/f1tenth.git
 
mkdir -p ~/f1tenth/src
 
cd ~/f1tenth/src
 
unzip ~/f1tenth/f1tenth_simulator.zip
 
sudo rm /etc/apt/sources.list.d/ros2-latest.list
 
sudo apt update
 
sudo apt install ros-kinetic-tf2-geometry-msgs ros-kinetic-ackermann-msgs ros-kinetic-joy ros-kinetic-map-server ros-kinetic-message-runtime -y
 
sudo apt upgrade -y
 
cd ~/f1tenth
 
catkin_make

echo "source ~/f1tenth/devel/setup.zsh" >> ~/.zshrc
source ~/.zshrc
 
roslaunch f1tenth_simulator simulator.launch
 
echo "Upgrade Mission Completed."

跑起来

最简单的一个控制器案例（PID）：

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

智能助手分析

这段脚本主要涉及到ROS（Robot Operating System，机器人操作系统）环境的配置和F1/10th自动驾驶小车的模拟器的安装。以下是对这段脚本的详细分析：

添加ROS密钥:

bash复制代码

	sudo apt-key add ros.asc

这行代码的目的是添加ROS的公钥到APT的密钥管理中，以便后续能够安全地从ROS的官方仓库下载软件包。

克隆F1/10th仓库:

bash复制代码

	git clone https://gitcode.net/ZhangRelay/f1tenth.git

这行代码从gitcode.net这个网站克隆了一个F1/10th的Git仓库到本地。

bash复制代码

	mkdir -p ~/f1tenth/src

使用mkdir -p命令来创建一个名为f1tenth的目录在~（即用户主目录）下，并在其中创建一个src子目录。

解压模拟器文件:

bash复制代码

	cd ~/f1tenth/src
	unzip ~/f1tenth/f1tenth_simulator.zip

首先切换到~/f1tenth/src目录，然后解压存放在~/f1tenth/目录下的f1tenth_simulator.zip文件。

移除ROS 2的源列表:

bash复制代码

	sudo rm /etc/apt/sources.list.d/ros2-latest.list

这行命令移除了系统中关于ROS 2的软件包源列表，可能是为了避免与ROS 1（这里使用的是Kinetic版本）发生冲突。

更新软件包列表并安装依赖:

bash复制代码

	sudo apt update
	sudo apt install ros-kinetic-tf2-geometry-msgs ros-kinetic-ackermann-msgs ros-kinetic-joy ros-kinetic-map-server ros-kinetic-message-runtime -y
	sudo apt upgrade -y

这三行命令分别用来更新APT软件包列表，安装指定的一些ROS软件包（与机器人控制、通讯等相关的包），并升级系统中所有可升级的软件包。

编译F1/10th项目:

bash复制代码

	cd ~/f1tenth
	catkin_make

切换到~/f1tenth目录，并使用catkin_make命令来编译整个ROS项目。

配置环境变量:

bash复制代码

	echo "source ~/f1tenth/devel/setup.zsh" >> ~/.zshrc
	source ~/.zshrc

这两行命令将source ~/f1tenth/devel/setup.zsh这行代码追加到~/.zshrc文件中，然后重新加载.zshrc文件以更新环境变量。这步操作确保了当前会话以及后续的会话都能识别到F1/10th项目中的ROS包。

启动模拟器:

bash复制代码

	roslaunch f1tenth_simulator simulator.launch

使用roslaunch命令来启动F1/10th模拟器。

输出完成信息:

bash复制代码

	echo "Upgrade Mission Completed."

最后，输出一条信息表示升级任务已完成。

整个脚本主要完成了ROS环境的配置、F1/10th项目的安装和编译，以及模拟器的启动。但是，有几个需要注意的地方：

该脚本是为Zsh shell编写的，如果使用的是Bash shell，则~/.zshrc应改为~/.bashrc。
在执行catkin_make之前，确保已经安装了所有必要的编译工具和依赖库。
该脚本未进行错误检查，如网络问题、软件包安装失败等情况可能会导致脚本运行中断或运行不完全。
需要保证在执行该脚本前已经安装了ROS Kinetic版本，并且已经初始化了ROS环境。
脚本中提到的f1tenth_simulator.zip文件需要事先准备好，否则unzip命令会失败。
该脚本可能需要特定的用户权限（如sudo）来执行某些命令，确保执行脚本的用户有足够的权限。

这段Python代码是用于实现一个ROS（Robot Operating System）节点，该节点旨在实现机器人的墙壁跟随功能。下面是对代码各个部分的详细分析：

导入模块和库

代码首先导入了一些Python标准库，如sys和math，以及numpy库。此外，还导入了ROS相关的库，如rospy和sensor_msgs、ackermann_msgs等，这些库在ROS开发中用于处理消息和发布/订阅话题。

定义PID控制参数和墙壁跟随参数

代码中定义了一些PID控制参数（kp、kd、ki）以及墙壁跟随参数（如角度范围、期望的左右距离、速度等）。PID控制是一种广泛使用的控制算法，用于根据误差来调整系统输出。

定义WallFollow类

WallFollow类用于实现墙壁跟随功能。类中包含了一些初始化方法（__init__）和其他方法。

在__init__方法中，首先定义了几个全局变量（如prev_time），然后初始化了激光雷达数据的订阅者和Ackermann驱动命令的发布者。

getRange方法

getRange方法用于从激光雷达数据中获取特定角度范围内的距离。它接受两个参数：激光雷达数据和角度。根据输入的角度，方法计算出在激光雷达扫描数据中的索引，并返回该索引对应的距离值。需要注意的是，这个方法目前只是一个占位符，实际的实现细节需要进一步完善。

pid_control方法

pid_control方法用于实现PID控制算法。它接受误差和速度作为输入，并计算出应该应用的角度。然而，这个方法的实现也是不完整的，只提供了PID控制算法的基本框架，具体的实现细节（如如何计算积分项、如何应用PID参数等）还需要进一步补充。

其他

代码的最后部分似乎被截断了，没有显示pid_control方法的完整实现，也没有显示其他可能的方法或功能。

总结

这段Python代码是一个ROS节点的初步实现，旨在实现墙壁跟随功能。然而，代码目前是不完整的，很多方法和功能只是提供了基本的框架，具体的实现细节还需要进一步补充和完善。例如，getRange和pid_control方法的实现需要详细编写，以确保它们能够正确地根据激光雷达数据计算出应该应用的角度。此外，代码还可能需要添加一些错误处理和边界检查机制，以确保程序的健壮性和可靠性。

修改代码以使其更加优质和高效是一个涉及多方面的任务，包括改善代码的可读性、可维护性、性能以及安全性等。以下是一些建议，并通过一个具体的案例来展示如何应用这些建议。

1. 代码可读性

使用有意义的变量名：避免使用如a、b、temp等无意义的变量名。
添加注释：解释复杂逻辑或代码段的目的。
格式化代码：保持一致的缩进和空格使用。

2. 代码可维护性

模块化：将功能分解为独立的函数或类。
避免全局变量：尽量使用局部变量或传递参数。
使用异常处理：处理可能出现的错误情况。

3. 性能优化

减少循环次数：使用更高效的算法或数据结构。
避免不必要的计算：存储计算结果以便重复使用。
利用缓存：对于重复计算或查询，使用缓存机制。

4. 安全性

验证输入：确保用户输入符合预期的格式和范围。
避免硬编码：不要在代码中直接写入敏感信息，如密码或密钥。

案例：一个简单的计算器程序

原始代码（可能存在一些问题）：

python复制代码

	def add(x, y):
	return x + y

	def subtract(x, y):
	return x - y

	def multiply(x, y):
	return x * y

	def divide(x, y):
	if y == 0:
	return "Error: Division by zero"
	else:
	return x / y

	使用示例
	num1 = int(input("Enter first number: "))
	num2 = int(input("Enter second number: "))

	operation = input("Enter operation (+, -, *, /): ")

	if operation == '+':
	result = add(num1, num2)
	elif operation == '-':
	result = subtract(num1, num2)
	elif operation == '*':
	result = multiply(num1, num2)
	elif operation == '/':
	result = divide(num1, num2)
	else:
	print("Invalid operation")

	print("Result:", result)

修改后的代码：

python复制代码

	def calculate(x, y, operation):
	"""执行给定的数学运算并返回结果。"""
	if operation == '+':
	return x + y
	elif operation == '-':
	return x - y
	elif operation == '*':
	return x * y
	elif operation == '/':
	if y == 0:
	raise ValueError("Error: Division by zero")
	return x / y
	else:
	raise ValueError("Invalid operation")

	使用示例
	try:
	num1 = int(input("请输入第一个数字: "))
	num2 = int(input("请输入第二个数字: "))
	operation = input("请输入运算符号 (+, -, *, /): ")

	result = calculate(num1, num2, operation)
	print("结果:", result)
	except ValueError as e:
	print("错误:", e)

修改点解释：

模块化：将计算逻辑封装在calculate函数中，提高了代码的可维护性。
异常处理：使用try-except块处理可能的错误情况，如除以零或无效的运算符号，提高了代码的健壮性。
代码可读性：添加了函数文档字符串，解释了函数的作用和参数。同时，将输入和输出逻辑与计算逻辑分开，使代码结构更清晰。
安全性：通过异常处理避免了因用户输入错误而导致的程序崩溃或不可预期的行为。

为了对文档中的算法进行优化，我们首先需要了解当前的算法逻辑、性能瓶颈以及优化目标。然而，您提供的文档片段主要是一些函数的定义和导入语句，并没有展示完整的算法逻辑。因此，我会基于您提供的代码片段，给出一个通用的优化建议，并尝试提供一个示例性的优化代码。

通用优化建议

减少不必要的计算：避免在循环或函数中重复计算相同的值。将这些值存储在变量中，并在需要时引用它们。
使用更有效的数据结构：根据数据的性质和使用方式，选择合适的数据结构（如列表、字典、集合等）。
利用并行计算：如果可能，使用并行处理来加速计算密集型任务。
优化循环：避免在循环中执行昂贵的操作，尽量将循环内部的计算简化。
代码可读性：虽然性能很重要，但代码的可读性和可维护性同样重要。确保代码逻辑清晰，注释充分。

示例性优化代码

假设您的算法中有一个循环，它正在计算一系列点的距离，并基于这些距离做出决策。我们可以尝试优化这个循环。

原始代码（假设）：

python复制代码

	def calculate_distances(points, origin):
	distances = []
	for point in points:
	distance = math.sqrt((point0 - origin0) 2 + (point1 - origin1) 2)
	distances.append(distance)
	return distances

优化后的代码：

python复制代码

	import numpy as np

	def calculate_distances(points, origin):
	将点和原点转换为NumPy数组，以便利用矢量化操作
	points = np.array(points)
	origin = np.array(origin)

	使用NumPy的广播功能来计算所有点的距离
	distances = np.sqrt((points:, 0 - origin0) 2 + (points:, 1 - origin1) 2)

	return distances.tolist() # 如果需要，将NumPy数组转换回列表