智能车学习笔记/备忘录乱写 | 地平线篇
前言
持续更新中！
完稿后结语：这篇博客实际上是备赛思路的进化史而非简单知识点的罗列，从里面可以看出来我们备赛从学习ROS2，到学习各任务需要的零散知识，到巡线，到巡线+odom导航，最终采用路径规划方案的思路。食用时需要哪部分可以点开右下角的目录直接快速跳转。
校内赛打完分到地平线组了，好多新东西，写篇笔记记录。
比赛使用OriginBot车模，车模搭载Ubuntu系统。以后估计主要在Ubuntu虚拟机上工作。现在需要先学习ROS2和FoxGlove。
一些很基础或者很好记的东西就不写了。
常用网址
Linux没见过的指令
将终端输出保存进文件
在有输出的指令后加>><file_name>可以将输出存到文件里。
指令连Wifi
nmcli device wifi rescan # 扫描wifi网络
nmcli device wifi list # 列出找到的wifi网络
wifi_connect "SSID" "PASSWD" # 连接某指定的wifi网络
后面的所有操作都是默认上位机和小车连接到同一局域网下进行的。
SSH
ssh <target_username>@<target_ip_address>可以直接连接到target的一个终端。
用rsync <local_file_address> <target_username>@<target_ip_address>:<target_address>将local_file_address发送到target的指定位置。在本机写完代码可以用这个同步到车上。下面是一个使用示例：
rsync -r "/home/darthvictor/test/src/python_test" root@192.168.100.228:/root/test/src
ROS2
基本原理
存在若干分工不同的结点同时运行，结点之间可以通过话题/服务等方式相互沟通数据。
工作空间
ROS2的工作空间由src, build, install, build, log四个文件夹构成，其中源码在src里，结构是src-若干功能包-若干结点代码。
写完代码之后可通过rosdepc install -i --from-path src --rosdistro foxy -y自动安装依赖。
之后通过colcon build编译工作空间。
编译成功后运行source install/local_setup.sh将自己写好的包添加到环境变量里，就能运行包里的节点了。
当然，也可以运行一次echo "source ~/<workspace_name>/install/local_setup.sh">>~/.bashrc，这样此工作空间就被添加到.bashrc里了，以后再编译完就能直接运行这个工作空间里的包和结点。
功能包
ros2 pkg create --build-type ament_cmake <package_name>创建使用C艹的功能包。
ros2 pkg create --build-type ament_python <package_name>创建使用python的功能包。
以下描述针对Python功能包：
在功能包内的同名文件夹内编写若干结点代码，之后需要在setup.py里添加结点代码的接口。
setup.py内部如下所示：
from setuptools import setup
package_name = 'python_test'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='darthvictor',
maintainer_email='darthvictor@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'test=python_test.test:main',
'pub_test=python_test.pub_test:main',
'sub_test=python_test.sub_test:main',
'client_test=python_test.client_test:main',
'server_test=python_test.server_test:main',
],
},
)
添加接口的形式如'console_scripts'中所示。
结点
下面是一个简单、典型的结点代码，它会不断在日志里输出Hello，World！：
import rclpy #rclpy是一个提供了ROS2相关操作的库
from rclpy.node import Node
import time
def main(args=None):
rclpy.init(args=args) #固定起手
node=Node('test') #创建结点
while rclpy.ok():
node.get_logger().info('Hello,World!') #循环在日志中输出信息
time.sleep(1)
node.destroy_node() #运行结束后销毁结点
rclpy.shutdown() #关闭ROS2
在编译之后用ros2 run <package_name> <node_name>运行节点。
通过ros2 node list可以查看现在正在运行的结点。
结点间的通讯
话题（topic）
结点之间的数据沟通方式之一。由若干个Publisher单向发送数据，然后由若干个Subscriber单向接收数据，即支持多对多。
下面是一个典型的Publisher代码，它在chatter话题下循环发布信息：
import rclpy
from rclpy.node import Node
from std_msgs.msg import String #std_msgs.msg中包含了ROS2传递消息时一些常用的数据类型
class PublisherNode(Node): #创建结点类
def __init__(self,name):
super().__init__(name) #调用父类Node的初始化方法
self.pub=self.create_publisher(String,'chatter',10) #创建了一个Publisher，它会在chatter主题下发布string类型的消息，队列长度为10（队列长度意为消息发不出去时最多储存的消息数）
self.timer=self.create_timer(0.5,self.timer_callback) #创建一个定时器，每0.5s调用一次下面的timer_callback
def timer_callback(self):
msg=String() #创建一条信息
msg.data='Hello,World!' #信息的内容
self.pub.publish(msg) #发布信息
self.get_logger().info('Message Published Successfully!')
def main(args=None):
rclpy.init(args=args)
node=PublisherNode('pub_test')
rclpy.spin(node) #spin用于循环启动结点运行
node.destroy_node()
rclpy.shutdown()
下面是一个典型的Subscriber代码，它会不断接收chatter话题下的新信息：
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
class SubscriberNode(Node):
def __init__(self,name):
super().__init__(name)
self.sub=self.create_subscription(String,'chatter',self.listener,10) #创建一个Subscriber，它不断从chatter获取字符串类型的信息，队列长度为10，当有新消息到达时，会传入listener处理
def listener(self,msg):
self.get_logger().info('I heard: %s'%msg.data) #listener在日志里输出收到的信息
def main(args=None):
rclpy.init(args=args)
node=SubscriberNode('sub_test')
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
值得注意的是，话题传递信息是使用的是封装好的数据类型，例如上面例程中的from std_msgs.msg import String就是从标准数据类型库里引入了String这种封装好的字符串类型。
当然，这种封装是可以自己写的。方法是创建一个C艹功能包，在功能包下创建一个.msg后缀的文件即可。例如假如封装的内容是表示位置的两个整型，那么我在position.msg里写的内容如下：
int64 x
int64 y
之后需要在CMakeLists.txt里添加这个封装相应的接口。CMakeLists.txt内容如下：
cmake_minimum_required(VERSION 3.5)
project(test_data_struct)
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/position.msg"
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
我们在rosidl_generate_interfaces部分按照相对路径添加接口即可。
服务（Service）
话题收发信息是单向的，即只支持Publisher发出信息，Subscriber接受信息，而服务是双向的，Client向Server发送信息，Server对信息进行特定的处理后再传送回Client。
下面是一个典型的Server结点代码，它在"Adder"这个服务器接收到两个加数后返回它们的和到Client：
import rclpy
from rclpy.node import Node
from test_data_struct.srv import Adder #Adder是自定义的封装好的数据接口类型，后面会解释
class Server(Node):
def __init__(self,name):
super().__init__(name)
self.srv=self.create_service(Adder,'Adder',self.call_back) #创建Server Adder，它会在接收到信息时调用call_back
def call_back(self,request,response):
response.sum=request.a+request.b
self.get_logger().info("Server Received %d and %d. Answer had been sent."%(request.a,request.b))
return response
def main(args=None):
rclpy.init(args=args)
node=Server('server_test')
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
下面是一个典型的Client结点代码，它会发送两个加数到"Adder"这个服务器接：
import rclpy
from rclpy.node import Node
from test_data_struct.srv import Adder
class Client(Node):
def __init__(self,name):
super().__init__(name)
self.client=self.create_client(Adder,"Adder") #创建一个Client，向Adder收发信息
while not self.client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...') #在服务器上线前等待并反复尝试连接
self.request=Adder.Request() #创建提交用的Request
def send_request(self,a,b):
self.request.a=a
self.request.b=b
self.future=self.client.call_async(self.request) #发送Request
def main(args=None):
rclpy.init(args=args)
node=Client('client_test')
while rclpy.ok():
tmp=input()
a=int(tmp.split(' ')[0])
b=int(tmp.split(' ')[1])
node.send_request(a,b)
while not node.future.done():
rclpy.spin_once(node)
print("Received Answer:%d" %node.future.result().sum)
node.destroy_node()
rclpy.shutdown()
类似于话题通讯时使用的是封装好的数据类型，服务通讯时使用的也是封装好的“数据接口”。它的创建方法同上面所述的msg数据类型的创建方法，只不过现在现在创建的数据接口文件是.srv类型。例如上面例程中提到的Adder类型，它所在的文件为Adder.srv，其内容如下：
int64 a
int64 b
---
int64 sum
分割线上方表示Request的形式，下方是Response的形式。
动作（Action）
动作和服务类似，它能在两个结点间完整地传递一个动作的信息。具体地说，Client可以向Server发送动作的开始指令，之后由Server执行动作并向Client反馈动作现在的状态以及是否结束。
下面是一个典型的ActionServer结点代码，它能够接收Client发来的开始指令，执行一个虚拟的转圈动作（用for循环表示）并在执行时反馈目前旋转了多少度，最后反馈一个动作完成的指令。
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from test_data_struct.action import Angle #Angle是一种自定义的动作传输类型，下面会解释
import time
class ActionServerTest(Node):
def __init__(self,name):
super().__init__(name)
self.server=ActionServer(self,Angle,"action_test",self.call_back) #创建ActionServer，在接收到请求后会执行callback
def call_back(self,state):
self.get_logger().info("Received Instruction!")
if not state.request.enable: #如果收到的请求指示非使能
self.get_logger().info("Instructed not to act!")
state.succeed()
result=Angle.Result()
result.finish=False
return result
else: #如果收到的请求是动作开始使能
self.get_logger().info("Action Started!")
feedback_msg=Angle.Feedback() #创建反馈信息类型
for i in range(0,360,30): #虚拟执行转圈动作
feedback_msg.angle=i
self.get_logger().info("Rotated %d degrees!"%feedback_msg.angle)
state.publish_feedback(feedback_msg) #反馈运行状况
time.sleep(0.2)
state.succeed() #结束执行
result=Angle.Result() #创建运行结果类型
result.finish=True
return result #反馈运行结果
def main(args=None):
rclpy.init(args=args)
node=ActionServerTest("action_server_test")
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
下面是一个典型的ActionClient结点代码，它能够接收向Server发送动作开始指令并接收动作运行状态：
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from test_data_struct.action import Angle
import time
class ActionClientTest(Node):
def __init__(self,name):
super().__init__(name)
self.client=ActionClient(self,Angle,"action_test") #创建一个ActionClient
def send(self,enable):
msg=Angle.Goal() #创建信息发送类型
msg.enable=enable
while not self.client.wait_for_server(timeout_sec=1.0): #等待Server上线
self.get_logger().info('Service not available, waiting again...')
self.send_state=self.client.send_goal_async(msg,self.feedback_call_back) #发送信息，向feedback_call_back反馈运行情况
self.send_state.add_done_callback(self.accepted_call_back) #发送完成时向accepted_call_back反馈请求是否被接受
def accepted_call_back(self,future):
state=future.result()
if not state.accepted:
self.get_logger().info("Action Rejected!")
return
self.action_state=state.get_result_async() #若请求被接受了则等待完成信息
self.action_state.add_done_callback(self.done_call_back) #若收到了动作已完成的信息则调用done_call_back
def feedback_call_back(self,feedback_msg):
self.get_logger().info("Rotated %d degrees!"%feedback_msg.feedback.angle) #输出反馈回来的运行情况
def done_call_back(self,future):
self.get_logger().info("Action Finished:%d"%future.result().result.finish) #输出动作的结束情况
def main(args=None):
rclpy.init(args=args)
node=ActionClientTest("action_client_test")
while True:
tmp=input()
if tmp=='Start':
break
op=int(input())
if op==0:
node.send(False)
else:
node.send(True)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
上面提到的Angle类型与话题、服务的msg srv类似，也是一种自定义的数据传输类型，它存在于.action结尾的文件内，内容如下：
bool enable
---
bool finish
---
int64 angle
自上而下的三项依次为请求信号Goal，结束信号Result和反馈数据Feedback。
参数（Param）
全局变量。所有结点都能修改/查询，也算是比较简单的一种通讯方法。
常用方法：
ros2 param list查看所有参数。
ros2 param describe <node_name> <param_name>查看某参数是干什么的。
ros2 param get <node_name> <param_name>查看某参数现在的具体值。
ros2 param set <node_name> <param_name> <value>设定某参数现在的具体值。
ros2 param dump <node_name>会把某结点全部参数都打印出来，可以配合>>存到文件里。
ros2 param load <node_name> <file_name>会按照文件更新参数。
关于参数在程序中的使用，下面是一个例程：
import rclpy
from rclpy.node import Node
class ParamTest(Node):
def __init__(self,name):
super().__init__(name)
self.declare_parameter('A_Certain_Param',int(100)) #创建参数并赋初始值
self.timer=self.create_timer(1.0,self.call_back)
def call_back(self):
now_param=self.get_parameter("A_Certain_Param") #查询参数现在的值
print("Param Value:%s"%str(now_param.value))
set_param=rclpy.parameter.Parameter("A_Certain_Param",rclpy.Parameter.Type.INTEGER,100)
self.set_parameters([set_param]) #设置参数的值
def main(args=None):
rclpy.init(args=args)
node=ParamTest("param_test")
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
它会生成一个名为A_Certain_Param的参数，不断显示它并不断修正为初始值。
需要注意的是，设置变量时使用的set_parameters接收参数的格式是列表，因此先用形如rclpy.parameter.Parameter("A_Certain_Param",rclpy.Parameter.Type.INTEGER,100)生成其需要的列表内容会方便很多。
Launch
脚本，可以用于一次开始运行多个节点。
下面是一个典型的脚本文件，它会同时开始执行python_test功能包里的pub_test和sub_test这两个结点：
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='python_test',
executable='pub_test',
name='publisher',
),
Node(
package='python_test',
executable='sub_test',
name='subscriber',
),
])
其中Node()封装中的name参数表示该结点将被重命名后以新的名字在终端中运行，可以防止同时反复运行一个结点时的同名冲突。
Node()中还可以封装其他参数，例如arguments表示在命令行中执行功能包时还要加上什么参数（-h之类的），parameters可以为结点中的参量赋值，等等。
当然，launch文件写完后也要在setup.py里添加路径。具体位置是在data_files部分，形式如下：
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share',package_name,'launch'),glob(os.path.join('launch','*.launch.py'))),
],
OriginBot的使用
代码编写
OriginBot机器人端的功能包都放置在/userdata/dev_ws工作空间，编写的时候参照上面的ROS2部分即可。
手操常用功能
这些方法都是使用指令控制的时候用的，实际上它们都已经集成到了FoxGlove里。
启动与运动控制
ros2 launch originbot_bringup originbot.launch.py use_camera:=true use_lidar:=true use_imu:=true启动底盘，后面两个参数决定是否同时启动摄像头和雷达。之后在第二个终端里使用ros2 run teleop_twist_keyboard teleop_twist_keyboard启动键盘控制结点即可控制小车。
摄像头
ros2 launch originbot_bringup camera_websoket_display.launch.py启动摄像头。之后在上位机打开http://<小车的IP地址>:8000即可查看摄像头。
FoxGlove
常用网址里给的FoxGlove教程网站写的足够详细干练了，直接看。
注意连接到机器人后要选择一下自己需要哪些话题的信息，然后选择性接收，否则接收所有话题的话传输会卡的一批。
比赛
具体开始准备比赛后的笔记将按照大致的任务来分。
常用指令合集
启动摄像头
ros2 launch hobot_usb_cam hobot_usb_cam.launch.py usb_video_device:=/dev/video8
启动底盘
ros2 launch origincar_base origincar_bringup.launch.py
启动ROSBridge
ros2 launch rosbridge_server rosbridge_websocket_launch.xml
启动巡线+避障转/cmd_vel控制
ros2 launch racing_control racing_control.launch.py avoid_angular_ratio:=1.4 avoid_linear_speed:=0.4 follow_angular_ratio:=-1.4 follow_linear_speed:=0.4
将摄像头发布的图像话题decode
ros2 launch hobot_codec hobot_codec_decode.launch.py codec_sub_topic:=/image codec_pub_topic:=/hbmem_img
延迟问题
刚上手OriginBot的时候信息传输（尤其是图像传输）存在巨大延迟。解决方法如下：
-
不要用手机/电脑热点充当通讯局域网，搞个好点的路由器；
-
给小车加装天线；
-
用于传输的图像压缩之后再发布。
虚拟环境与实际协同
FoxGlove自带该功能，不多赘述。
巡线
cv2简单处理法
（暂时）使用了从三轮小车（先前从工创中心借的一套OriginBot）里面扒下来的现成的巡线程序，代码如下：
import rclpy, cv2, cv_bridge, numpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CompressedImage
from geometry_msgs.msg import PointStamped
class Locater(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info("Start Center Locating!")
self.bridge = cv_bridge.CvBridge()
self.image_sub = self.create_subscription(Image, '/image', self.image_callback, 10)
self.center_point_pub = self.create_publisher(PointStamped, 'racing_track_center_detection', 10)
self.pub = self.create_publisher(Image, '/camera/process_image', 10)
self.pub_bin = self.create_publisher(Image, '/camera/process_image_bin', 10)
self.cnt=0
def image_callback(self, msg):
image = self.bridge.compressed_imgmsg_to_cv2(msg,"bgr8")
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_yellow = numpy.array([ 10, 70, 30])
upper_yellow = numpy.array([255, 255, 250])
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
h, w, d = image.shape
search_top = int(h*0.6)
search_bot = int(h*0.6 + 20)
mask[0:h, 0:int(w*0.2)] = 0
mask[0:h, int(w*0.8):w] = 0
mask[search_bot:h, int(w*0.2):int(w*0.8)] = 0
mask[0:search_top, int(w*0.2):int(w*0.8)] = 0
M = cv2.moments(mask)
if M['m00'] > 0:
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
cv2.circle(image, (cx, cy), 20, (0,0,255), -1)
point_msg=PointStamped()
point_msg.point.x=cx
point_msg.point.y=cy
self.center_point_pub.publish(point_msg)
self.cnt += 1
if self.cnt >= 3:
img_new = cv2.resize(image,(120,90))
img_new = cv2.cvtColor(img_new, cv2.COLOR_BGR2GRAY)
self.pub.publish(self.bridge.cv2_to_imgmsg(img_new))
self.pub_bin.publish(self.bridge.cv2_to_imgmsg(cv2.resize(mask,(40,30))))
#print(err)
# self.pub_cmpressed.publish(self.bridge.cv2_to_compressed_imgmsg(img_new))
self.cnt = 0
def main(args=None):
rclpy.init(args=args)
locater = Locater('locater')
rclpy.spin(locater)
locater.destroy_node()
rclpy.shutdown()
它基本的原理是从摄像头节点接收图像，对图像进行一定处理后，由处理后的图像得到前瞻行范围内引导线所在的位置以及误差，之后将要跟随的引导线位置发送到racing_track_center_detection话题下。
关于图像处理：下面展示一下各条代码的作用：
这是一个示例图像：
经过hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)处理得到的hsv：
这个指令是将原本用BGR（Blue, Green, Red）表示的图片变为HSV（色相，饱和度，明度）表示形式，据ChatGPT说“HSV是一种表示颜色的方式，与RGB相比更直观，更容易调整颜色的各个属性。”
经过
lower_yellow = numpy.array([ 10, 70, 30])
upper_yellow = numpy.array([255, 255, 250])
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
处理后得到的mask：
这个指令是将参数在lower_yellow, upper_yellow之间的颜色变为白色，其余设为黑色。
经过
h, w, d = image.shape
search_top = int(h/2)
search_bot = int(h/2 + 20)
mask[0:h, 0:int(w*0.1)] = 0
mask[0:h, int(w*0.9):w] = 0
mask[search_bot:h, int(w*0.1):int(w*0.9)] = 0
mask[0:search_top, int(w*0.1):int(w*0.9)] = 0
处理后的mask：
这一段指令进行了一个遮罩，把前瞻行范围，一定左右范围内的图像保留，其余置为黑色。
接下来的M=cv2.moments(mask)计算了mask的矩，具体地说，M['m00']表示图像区域的零阶矩。零阶矩实际上是图像中所有非零像素的数量，可以看作是图像的总质量。M['m10']表示X轴上的一阶矩（质心在X轴上的位置），M['m01']表示Y轴上的一阶矩（质心在Y轴上的位置）。由此得到的
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
即为前瞻范围内线的位置。
将这个点在原图上标出后：
之后我们自己写了个将打了点的图像压缩并发布话题以供在FoxGlove订阅。
ResNet推理巡线
OpenCV巡线存在很多问题，最主要的就是线的颜色阈值很难调，调不好很容易出现误判，而且受光照变化影响很大。所以我们最终使用的是ResNet推理巡线。
训练的过程和避障部分训练识别锥桶模型的过程差不多。推理完成之后发布一个PerceptionTarget类型的表示赛道中点的红点位置数据到racing_track_center_detection话题下。控制节点订阅这一话题下的信息跟踪这个红点进行巡线即可。
障碍物识别
我们选择使用yolov5对锥桶图像进行学习，识别。
注意，后面的步骤都建议安装WSL在Linux子系统下进行。在Windows里也不是不能搞，但是很容易出现很多不可名状的问题，解决起来很麻烦。
训练的流程参考这篇文章。
训练结束后得到的模型文件是.pt格式的，接下来我们要参考这篇文章将模型转换为可用的.bin模型文件并部署到小车上。
（值得注意的是，这篇文章看到生成.bin文件就行了。关于后面上板运行部分，我们并没有在小车里看见/app/ai_inference/07_yolov5_sample）
在按照上面这篇文章操作之前，我们首先需要安装X3派的算法链，这篇文章里要用到。这是X3派算法链官方安装教程。
但是必须要指出的是，这份官方安装教程有问题。“获取模型转换资料包”这一步下载的资源都是适用于python3.8的，但是“创建模型转换环境”这一步使用的指令却是conda create -n horizon_bpu python=3.6 -y，指定的是python3.6版本。把这一步换成conda create -n horizon_bpu python=3.8 -y就好了。另外，如果安装完成后运行hb_mapper报错，大概率是有的包没装上，运行一下pip install opencv-python-headless -i http://mirrors.aliyun.com/pypi/simple/ --trusted-host mirrors.aliyun.com补上一些包就好了。
（若果你在上述的转换过程中实在遇到了太多不可名状的问题的话，可以试试这篇文章，它是在Windows上用Docker实现的，可能会比上面的方法麻烦亿点点，但是走投无路的话可以试试）
运动控制
最开始我们直接使用这篇文章里的运动控制程序。按照文章配置结束后运行ros2 launch racing_control racing_control.launch.py avoid_angular_ratio:=1.4 avoid_linear_speed:=0.4 follow_angular_ratio:=-1.4 follow_linear_speed:=0.4即可，后面的几个参数在文章中有说明。
这一模块的作用是接收一个表示黑线中点位置的racing_track_center_detection话题和一个表示障碍物信息的racing_obstacle_detection话题并根据这信息发布/cmd_vel话题控制小车运动。
关于/cmd_vel：车的底盘节点启动后会自动接受/cmd_vel这个话题下的信息并根据信息控制小车的运动。
但是这个程序实际跑起来比较抽象，所以我们参考了它的代码逻辑重写了运动控制。
在我们最开始的设想里，程序的逻辑是这样的：
开两个Subscriber，收到racing_track_center_detection和racing_obstacle_detection下的赛道位置信息/障碍物信息后存起来（当然，一直维护最新发布的）；
开一个timer，间隔一定时间根据存的赛道位置、障碍物位置对车进行控制。逻辑是如果障碍物的下端距离图像底部近到了一定程度，则判定为需要避障，这时在障碍物左侧/右侧一定距离生成一个要跟踪的红点，由此绕过障碍物。至于红点在左侧还是右侧则根据障碍物和赛道的相对位置决定。
如果没有障碍物或者障碍物都很远则跟踪赛道红点进行巡线（也就是说避障优先级高于巡线）。
至于跟踪红点，算个和屏幕中心的误差然后调PID就行了。
但是这个方法在后来遇到了极大的问题。最主要的就是，小车刚绕开障碍物之后很容易丢线。我们最开始想的解决办法是累计避障时产生的误差并在避障结束后修正这一误差，但是效果不好，一是避障结束的修正后小车是侧入轨道的，很容易再次丢线冲出轨道；二是遇到连续障碍物的时候寄的更彻底。
所以我们最终的结局方案是要一劳永逸地解决最根本的丢线问题。
要解决这个问题，最基本的问题就是丢线判断。由于不管有没有丢线ResNet都会推理产生一个红点，所以根据发的信息是否出现中断并不可行。我最开始写的一版程序是检测红点附近一定范围内的色块，统计其中黑色色块的数量，若黑色色块所占的比例低于一定阈值则判定为丢线。这个实际运行起来准确率还挺高的，但是我觉得存在一定的隐患。一是它还是要判断“黑色”的时候还是要用颜色阈值，所以这也需要调，也会受光照影响；二是这个方法要单开一个python程序，所以性能并不好。最后总采用的方法是把ResNet推理得到的置信度合在位置信息里一起发出来，若置信度高于一个阈值则判定为没有丢线。
然后就是丢线之后该怎么办。我们想的办法是用小车的位置信息和姿态信息进行丢线时的导航（就是上位机显示虚拟小车用到的那个位置和姿态信息）。小车的位置和姿态信息是底盘节点发布在odom和odom_combined这两个话题下的（odom是纯通过小车的编码器推算的，odom_combined是编码器+陀螺仪IMU综合计算得到的位置信息，其中odom我感觉比较准，odom_combined就比较玄学，当然也可能是我不会用导致的玄学，要是后面发现combined更好用再回来踢我自己）。
这两个话题发的都是nav_msgs/msg/Odometry类型的消息。其中位置信息在msg.pose.pose.position，小车朝向的角度信息在msg.pose.pose.orientation里面。需要注意的是角度信息是用四元数表示的，使用的时候先要把它转换成欧拉角。从四元数到二维平面上的朝向角转换方式如下：
yaw = atan2(2 * (w*z + x*y), 1 - 2*(y**2 + z**2))
之后再用小车的位置信息和终点的位置信息算一下该往哪个方向走，然后PID控制打角就行。
当然，这个odom测算可能不准，建议按照这个先校准一下。
运动控制和避障问题的最终解决方案
根据前面“运动控制”章节所述，我们现在已经可以精准确定小车位置了。既然如此我们突然发现我们好像已经不需要巡线了，直接根据小车的坐标画一条直接到中点的路径，然后让小车按照轨迹走就行了。考虑到巡线的诸多弊端（容易丢线，和避障很难结合等等），我们最终决定采用这种方案。
此时，小车的运动控制和避障合成了一个问题：规划一条轨迹并让小车按照轨迹走。具体而言，它又分成了下面这些小问题需要解决：
-
必须保证小车的位置是准的，然而
odom话题是完全根据车轮的运动推算的，用的是cmd_vel里发布的信息，然而cmd_vel是理想的，我们希望的小车运动情况，实际执行下去并不准确，因此并不好用；odom_combined似乎是odom和陀螺仪结合的，这个陀螺仪我们不知道它是怎么用的，反正小车放着不动角度都会渐渐偏移。因此为了定位的准确，我们修改了底盘里odom的计算方法，这个是队友写的，我不太清楚咋搞的，反正现在好像是用的IMU； -
我们知道小车的绝对位置了，但是不知道障碍物的绝对位置。所以我们用了一个叫逆透视的东西（下面的部分可以参考队友写的博客）。小车的镜头角度和镜头高度是固定的，因此我们可以通过某种关系将画面中地面上的点映射为实际坐标（相对小车的）。这种映射可以通过乘一个矩阵实现，矩阵可以用下面的代码测定：
import numpy as np import cv2 # 定义原始图像中四个角的坐标 src_points = np.float32([(210, 204), (482, 204), (272, 167), (409, 167)]) # 定义输出图像中对应的四个角的坐标 dst_points = np.float32([(-17.2, 42.5), (22.8, 42.5), (-17.2, 92.0), (22.8, 92.0)]) matrix = cv2.getPerspectiveTransform(src_points, dst_points) print(matrix)简单来说，找个四方的东西（比如一张A4纸）放到镜头前面的地上，找出图像上四个角点的像素坐标，再量出四个角点相对于小车的现实坐标，填到上面的代码里运行就可以了。
得出的矩阵这样使用：
std::pair<int, int> inverse_perspective_point(int x, int y) { std::vector<std::vector<double>> matrix = { {-8.46875535e-02, -9.04096702e-03, 2.95338002e+01}, {-2.94831011e-17, 5.97390039e-02, -3.66614618e+01}, {-0.00000000e+00, -7.72487947e-03, 1.00000000e+00} }; std::vector<double> homogeneous_point = {static_cast<double>(x), static_cast<double>(y), 1.0}; std::vector<double> transformed_point(3, 0.0); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { transformed_point[i] += matrix[i][j] * homogeneous_point[j]; } } transformed_point[0] /= transformed_point[2]; transformed_point[1] /= transformed_point[2]; int out_x = static_cast<int>(round(transformed_point[0])); int out_y = static_cast<int>(round(transformed_point[1])); return {out_x, out_y}; }比如说，将障碍物在画面中底边中点的像素坐标扔到上面的函数里，就可以得到障碍物相对于小车的实际坐标。
注意，这里说的是相对于小车的坐标（不妨称为local坐标），我们还要根据小车的位置和角度推出障碍物的实际绝对坐标（称为global）。用的是下面的代码（画个图推一下就能推出来，懒得细讲了）：
std::pair<double, double> local_to_global(double car_x, double car_y, double theta, double c_x, double c_y, int type) { c_y+=23.0; double cone_dis = (type == 0) ? sqrt(c_x * c_x + c_y * c_y) + 14.0 : sqrt(c_x * c_x + c_y * c_y); double global_x = car_x * 100.0 + cone_dis * cos(theta - atan2(c_x, c_y)); double global_y = car_y * 100.0 + cone_dis * sin(theta - atan2(c_x, c_y)); return {global_x, global_y}; }这样我们就得到障碍物实际的绝对坐标了。
需要注意一点：小车处理图像、YOLO推理的时间是远长于接收
odom信息的，因此收到障碍物信息时接收的odom在时间线上和YOLO的图片是错开了的。所以我们需要将障碍物信息和odom都打上时间戳，将一段时间内的odom都存到一个队列里，按照时间戳将图像和odom对应计算。 -
路径规划。我们知道障碍物和小车的位置了，那么接下来就要给小车规划一条前往终点的路径了。
要搞搜索，首先咱们需要一张图。
首先，任务一和任务三的场地大约2m×5m，我们将每1cm划分为1格，将场地离散化。
接下来建图，将每个小格与距离它一定距离（即每次搜索的步幅，我们取10cm）的所有格之间建边（实际操作上并不是通过邻接表之类的真的去建图，而是搜索时每次搜到一个格后，进行下一步时仅考虑距离它10cm的格）。障碍物一定范围内（例如40cm）的所有格子都是不可被拓展的。
搜索策略方面，我们采取A*搜索，估价函数为目前已经走过的距离加上到终点的直线距离。显然该估价函数一定是小于最短路径长度的，因此它是正确的A*算法，保证找到的是最短路径。
示意图大概下面这样：
红点是搜索路径，绿点是搜索终点，黑色是障碍物及其覆盖的区域，蓝色是可以由某个红点拓展到的点。画面比例有点问题，凑活着看吧。
核心代码如下所示，一些细节写注释里了：
struct MyNode {
std::pair<int, int> pos;
double f;
MyNode(std::pair<int, int> pos, double f) : pos(pos), f(f) {}
bool operator<(const MyNode &other) const {
return f > other.f;
}
};
std::vector<std::pair<int, int>> PathPlanner(std::vector<std::vector<float>> &Map, const std::pair<int, int> &start, const std::pair<int, int> &des, int sign, double yaw) {
std::vector<std::pair<int, int>> path;
std::priority_queue<MyNode> priority_queue;
std::pair<int, int> now_loc;
std::pair<int, int> now_des;
std::pair<int, int> now_start;
if (sign == 0 || sign == 1) {
if (start.second > des.second) {
return {des};
}
}
//下面这四行在防止越界
now_start = {std::min(start.first, map_height - (sign==0||sign==1?35:31)), std::min(start.second, map_width - 50)};
now_start = {std::max(now_start.first, 0), std::max(now_start.second, (sign==0||sign==1?6:0))};
now_des = {std::min(des.first, map_height - (sign==0||sign==1?35:31)), std::min(des.second, map_width - 50)};
now_des = {std::max(now_des.first, 20), std::max(now_des.second, 0)};
//如果起点被障碍物遮住了，就找最近的没被遮住的点
if (Map[now_start.first][now_start.second] != 0.0f) {
now_start = find_nearest_free_point(Map, now_start);
}
//std::cout<<"Now Loaction:"<<now_start.first<<" "<<now_start.second<<std::endl;
now_loc=now_des; //出于实际经验我们是从终点往小车位置搜的
priority_queue.push(MyNode(now_loc, 0.0));
std::vector<std::vector<std::pair<int, int>>> pre(map_height, std::vector<std::pair<int, int>>(map_width, {-1, -1}));
std::vector<std::vector<double>> visited(map_height, std::vector<double>(map_width, -1.0));
visited[now_loc.first][now_loc.second] = 0.0;
std::pair<int, int> tmp = now_start;
int cnt = 0;
while (!priority_queue.empty()) {
cnt++;
if (cnt > 40000) { //限制了一下步数
tmp = now_start;
//std::cout<<cnt<<std::endl;
break;
}
MyNode current_node = priority_queue.top();
priority_queue.pop();
now_loc = current_node.pos;
if (get_dis(now_loc, now_start) <= 10.0) {
tmp = now_loc;
break;
}
for (int angle = 0; angle < 360; angle += 30) {
int i = static_cast<int>(now_loc.first + 10 * cos(angle * M_PI / 180.0)); //{i,j}是距离小车10cm的点
int j = static_cast<int>(now_loc.second + 10 * sin(angle * M_PI / 180.0));
if (i >= (sign==0||sign==1?0:24) && i < map_height-35 && j >= (sign==0||sign==1?15:0) && j < map_width-50) { //限制了一下小车不能离边界太近，要不然容易撞墙
if (Map[i][j] == 0.0f && (visited[i][j] == -1.0 || visited[now_loc.first][now_loc.second] + 10.0 < visited[i][j])) {
double now_dis=get_dis({i, j}, now_start);
if(sign==0 && now_dis<=35 && fabs(atan2(i-now_start.first,j-now_start.second)-yaw)>40 * M_PI / 180.0) continue; //搜索到靠近小车的时候限制了和小车角度的夹角，防止出现太急的弯
if((sign==5||sign==6)&&i<=now_des.first+4) continue;
pre[i][j] = now_loc;
double f = visited[now_loc.first][now_loc.second] + 10.0 + now_dis; //A*估价函数
priority_queue.push(MyNode({i, j}, f));
visited[i][j] = visited[now_loc.first][now_loc.second] + 10.0;
}
}
}
}
if (pre[tmp.first][tmp.second].first == -1) {
path.push_back(tmp);
}
while (pre[tmp.first][tmp.second].first != -1 && get_dis({tmp.first,tmp.second},now_des)>15.0) { //从搜索终点往回推路径
if (sign == 0 || sign == 1) {
path.push_back(tmp);
} else {
path.push_back({tmp.first, tmp.second});
}
tmp = pre[tmp.first][tmp.second];
}
//std::cout<<cnt<<std::endl;
return path;
}
路径规划挺费CPU的，所以一定要写成c艹然后编译吸氧，并且两次路径规划之间间隔可以很长，毕竟一次规划出来的就是全部路径（完美情况下在起点就能看见所有锥桶，甚至只进行一次规划就行）。我们要防的是一个锥桶遮住了另一个锥桶，绕过第一个锥桶才看见第二个且第二个在原本路径上的情况。大致计算了一下，速度为0.8m/s时600ms计算一次路径就能避免上面所述的情况，将第二个桶及时更新并计算出新路。
- 让小车按照规划的路径运动：这个是队友写的，具体的我说不清楚，反正基本意思就是找路径里面小车前方的某个点，计算小车和那个点的角度，与小车的实际角度取误差，就可以PID控制了。
