深入Originbot：PID人脸追踪
1.前言
OriginBot小车 基于 旭日X3pi 和 tros 开发，为了能够编写出更优秀的 'App' 释放小车的潜能，我们需要熟读 旭日X3pi 和 tros 的用户手册以便更好地理解本文内容;
在用户手册中，需重点了解以下内容：
x3派面向生态开发者的产品定位，以及如何通过 Ai工具链 与 机器人开发平台(tros) 发挥5Tops算力；
x3派系统安装、配置，了解 x3派 的硬件接口，体验基础图像采集和Ai推理功能；
了解 tros 基于 ros 新增的功能模块，例如 Hobot Sensor（机器人常用传感器）、Hobot Codec（视频编码器）、Hobot Render（Web端可视化）等;
体验 tros 图像采集->图像编解码->图像展示 功能;
了解 Boxs 算法仓库 和 Apps 应用实例；
学习 小车人体跟随(body_tracking) App 源码；
您可以通过 '地平线程序员奶爸' 的教程来深入学习 tros 手势控制app和人体追踪app:
开发者说 | 地平线程序员奶爸带你玩转机器人开发平台 —— 第一期 手势控制（附直播课视频）
开发者说 | 地平线程序员奶爸带你玩转机器人开发平台 —— 第二期 人体跟随（附直播课视频）
2.白话
说白了，旭日x3pi为我们机器人开发提供了硬件平台，tros为我们机器人开发提供了软件平台，二者需结合起来一起使用;
硬件平台为我们提供了很高的算力，而软件平台则为发挥硬件的潜能提供了丰富的 demo；
我们可以借助这些demo（Boxs和Apps），高效、便捷（复制、粘贴）的进行机器人开发，打造具有竞争力的智能机器人产品。
3.创建人脸追踪功能包
通过ssh连接上OriginBot小车，cd进 /userdata/dev_ws/src/originbot 目录，创建 originbot_face_tracking 功能包:
这里的命名规则与Originbot其他功能包一致
4 编写Launch文件
在功能包路径下新建 launch 目录，编写 ob_face_tracking.launch.py 文件欲实现的系统框图如下：
其中椭圆形框内为Node名，矩形框内为topic名。
节点名与功能对照下图：
传感节点使用了 tros Hobot Ssnros组件中的 /mipi_cam Node，参数指定使用 GC4663 摄像头，发布的图片格式和分辨率可以直接用于算法推理，其中通过共享内存的方式发布图片，可以极大地降低系统负载和传输延迟。
感知节点使用了 tros 中的 Boxs 算法仓库，订阅 Hobot Sensor（mipi_cam Node）发布的图像数据后，通过视觉算法进行推理，检测出人体关键部位；
人体检测和跟踪算法Node 订阅 Hobot Senso r发布的图像消息，利用 BPU 处理器进行AI推理，发布包含人体、人头、人脸、人手框和人体关键点检测结果的AI msg，并通过多目标跟踪（multi-target tracking，即MOT）功能，实现检测框的跟踪和ID分配。
交互节点，人体检测结果订阅节点 /ai_msg_sub_node，订阅人体检测和跟踪节点发布的 Ai msg 消息，执行跟踪策略。根据人脸在画面中的位置通过比例控制器计算出小车旋转角度；
控制节点/ob_tracking_Robot_CmdVel，发布消息名为'/cmd_vel'的命令控制小车做旋转运动；
WEB展示，由 /hobot_codec 将 /mipi_cam 发布的 ’nv12‘ 格式的图像转换为 ’jpeg‘ 格式，以便 /websocket 展示。同时， /websocket 节点订阅 /hobot_mono2d_body_detection 消息用于图像渲染；
ob_face_tracking.launch.py 文件的代码如下：
5 功能包的实现
功能包中的 py 文件如下：
功能包程序设计如下：
略缩图如下：
main.py 代码如下：
smart_subscriber.py 代码如下:
face_tracking.py 代码如下:
6 程序设计难点
难点1：Python 多线程 Queue：
Python Queue是线程安全的不同于C++，因此可以直接使用get()、put() 而不需要使用锁Lock()；
难点2：PID人脸追踪器：
PID(proportion integration differentiation) 控制器只了P（proportion），因此为比例控制器，类似于一次函数；
PID(5.0, 0.0, 0.0, setpoint=0) # P 经过调试后被设置为 5，setpoint 目标点为 0，即 NDC 空间下图像的中点;
pid.sample_time = 0.1 # 设置采样率可以简化 PID 的计算，推荐设置；采样率与函数的调用频率无关；
Q： 什么是 NDC 空间？
在计算机图像中，NDC空间是指归一化设备坐标（Normalized Device Coordinates）空间。
摄像头的分辨率各不相同，因此产生了 NDC，它可以将输入的图像归一化至 [-1 ,1]区间方便计算。err = float(((rect.x_offset + rect.width / 2) / self.track_cfg_.img_width - 0.5) * 2)本例算法中，在NDC空间下，0.5即为图像中心点。
您可以代入真实的人脸坐标，进行计算，以便理解算法；
参考：
奇乐编程学院 - 人脸追踪
阿凯爱玩机 - 云台人脸追踪
难点3：Ai msg 消息的解析
通信接口在 \opt\tros\share\ai_msgs\msg\ 路径下：
PerceptionTargets.msg 接口如下：
Target.msg 接口如下：
Roi.msg 接口如下：
BUG：经解析后，笔者认为这里有一个数据结构错误，但不影响程序使用；
PerceptionTargets.msg 中有一个 Target[] targets 数组，其中这个 target 的类型可以是人、车、动物、物体，这个 target 下有一个 Roi[] rois，这个 Roi 包含人体框、人头框、人脸框、人手框多个元素；
然而在实际使用中发现：
rois[]数组中只包含一个元素，数据伪代码如下：
我认为正确的数据结构如下：
正确 √
错误 ❌
6 编译运行
ssh连接小车，安装 py 功能包依赖库simple_pid：
1、使用 pip 安装 simple_pid 库：
2、执行功能包根目录下的 .\install.sh 脚本自动安装依赖：
cd 进 /userdata/dev_ws/ 目录，执行如下命令编译功能包；
使用 Launch 文件启动人脸追踪功能：
启动小车底盘：
7 执行效果
