使用残差神经网络ResNet18检测车道线关键点，关键点与中心的偏差作为PID控制器的输入，随后计算出一个角速度，线速度固定0.2m/s，跑圈时间约45秒，发布twist类型的消息控制小车底盘。
关于相机数据流：mipi相机数据自动经过VPS得到512_512像素（拉伸），再申请一个VPS模块将512_512缩小到224*224，送给BPU推理。
可调节的参数在第38行，self.Kp，参数P，比例控制；第49行，self.Kd，参数D，微分控制；第64行，self.twist.linear.x，线速度。
群内演示视频跑圈时间为22秒，调节这三个参数，同样的代码也可达到22秒的成绩，这里就给各个参赛队留一些发挥空间了。（22几乎是45的二分之一，线速度怎么调就不用我多说了吧，只能暗示到这里了！！）
代码第36行需要修改模型文件的相对路径，模型文件和源代码在本帖末尾的附件中。
注意：不建议使用这么不讲道理的代码，此时这个节点独占了摄像机资源，正确的做法是使用nodeHub的mipi相机节点，将采集的图像数据以ROS标准图像消息或者零拷贝（hbmem）图像消息进行发布，供需要使用图像数据的其他模块订阅。并且这样就可以多个节点订阅摄像头画面，作不同的视觉分析。-
另外NodeHub有基于C++实现的推理和控制，源码也托管在了github，有进一步要求的同学可参考。
赛道检测：https://developer.horizon.cc/nodehubdetail/184587678518566919-
巡线控制：https://developer.horizon.cc/nodehubdetail/184587678518566923-
MIPI相机驱动：https://developer.horizon.cc/nodehubdetail/168958376283445781
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from geometry_msgs.msg import Twist
import cv2
import numpy as np
from time import time
from hobot_vio import libsrcampy
from hobot_dnn import pyeasy_dnn as dnn
class ResNetControlNode(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.cmd_vel_pub = self.create_publisher(Twist, "/cmd_vel", 10) # 创建发布者对象（消息类型、话题名、队列长度）
self.timer = self.create_timer(0.066, self.timer_callback) # 创建一个定时器（单位为秒的周期，定时执行的回调函数）
self.twist = Twist()
## 创建VPS和camera等对象，开始图像采集
# camera对象
self.cap_mipi = libsrcampy.Camera()
self.height1, self.width1 = 512, 512
self.height2, self.width2 = 720, 1280
self.height, self.width = 1440, 2560
ret = self.cap_mipi.open_cam(0, -1, 30, [self.width1, self.width2, self.width], [self.height1, self.height2, self.height])
print("cap_mipi.open_cam State: ", end="")
print(ret)
# vps对象
self.vps = libsrcampy.Camera()
ret = self.vps.open_vps(1, 1, 512, 512, 224, 224)
print("vps State: ", end="")
print(ret)
## 导入ResNet18模型（绝对路径）
self.models = dnn.load('/root/00_com_ws/DataSet3_train1.bin')
## PID相关参数
self.Kp = 2.0
self.Kd = 3.0
self.Target_value = 100.0 # 设定值初始化
self.last_Err = 0.0 # 上一个误差值初始化
self.total_Err = 0.0 # 误差累加初始化
self.output = 0.0 # PID输出初始化
def timer_callback(self): # 创建定时器周期执行的回调函数
# 获取512的图
mipi_data_512 = self.cap_mipi.get_img(2, self.width1, self.height1)
# 转化为224_img
self.vps.set_img(mipi_data_512)
mipi_data_224 = self.vps.get_img(2, 224, 224)
outputs = self.models[0].forward(np.frombuffer(mipi_data_224, dtype=np.uint8))
outputs = outputs[0].buffer
x, y = int(224*outputs[0][0][0][0]), int(224*outputs[0][1][0][0])
## PID角速度控制目标计算
self.Error = self.Target_value - 1.0*x # 计算偏差
self.total_Err = self.total_Err + self.Error # 偏差累加
self.output = self.Kp * self.Error + self.Kd * (self.Error - self.last_Err) # PID运算
self.last_Err = self.Error # 将本次偏差赋给上次一偏差
print("( %03d, %03d ) output = %03d"%(x,y,int(self.output)))
self.twist.linear.x = 0.2
self.twist.angular.z = self.output/114.0
self.cmd_vel_pub.publish(self.twist)
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = ResNetControlNode("resnet_control_node") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
# 偷个懒，省得编译
if __name__ == "__main__":
main()