1.前言

OriginBot小車 基于?旭日X3pi?和?tros??開發(fā)，為了能夠編寫出更優(yōu)秀的 'App' 釋放小車的潛能，我們需要熟讀?旭日X3pi?和?tros?的用戶手冊以便更好地理解本文內(nèi)容;

在用戶手冊中，需重點了解以下內(nèi)容：

• x3派面向生態(tài)開發(fā)者的產(chǎn)品定位，以及如何通過?Ai工具鏈?與?機器人開發(fā)平臺(tros)?發(fā)揮5Tops算力；

• x3派系統(tǒng)安裝、配置，了解 x3派 的硬件接口，體驗基礎(chǔ)圖像采集和Ai推理功能；

• 了解 tros 基于 ros 新增的功能模塊，例如 Hobot Sensor（機器人常用傳感器）、Hobot Codec（視頻編碼器）、Hobot Render（Web端可視化）等;

• 體驗 tros 圖像采集->圖像編解碼->圖像展示 功能;

• 了解 Boxs 算法倉庫 和 Apps 應(yīng)用實例；

• 學(xué)習(xí)?小車人體跟隨(body_tracking)?App 源碼；

您可以通過 '地平線程序員奶爸' 的教程來深入學(xué)習(xí) tros 手勢控制app和人體追蹤app:

開發(fā)者說 | 地平線程序員奶爸帶你玩轉(zhuǎn)機器人開發(fā)平臺 —— 第一期 手勢控制（附直播課視頻）

開發(fā)者說 | 地平線程序員奶爸帶你玩轉(zhuǎn)機器人開發(fā)平臺 —— 第二期 人體跟隨（附直播課視頻）

2.白話

說白了，旭日x3pi為我們機器人開發(fā)提供了硬件平臺，tros為我們機器人開發(fā)提供了軟件平臺，二者需結(jié)合起來一起使用;

硬件平臺為我們提供了很高的算力，而軟件平臺則為發(fā)揮硬件的潛能提供了豐富的 demo；

我們可以借助這些demo（Boxs和Apps），高效、便捷（復(fù)制、粘貼）的進行機器人開發(fā)，打造具有競爭力的智能機器人產(chǎn)品。

3.創(chuàng)建人臉追蹤功能包

通過ssh連接上OriginBot小車，cd進 /userdata/dev_ws/src/originbot 目錄，創(chuàng)建 originbot_face_tracking 功能包:

這里的命名規(guī)則與Originbot其他功能包一致

4 編寫Launch文件

在功能包路徑下新建 launch 目錄，編寫 ob_face_tracking.launch.py 文件欲實現(xiàn)的系統(tǒng)框圖如下：

其中橢圓形框內(nèi)為Node名，矩形框內(nèi)為topic名。

節(jié)點名與功能對照下圖：

• 傳感節(jié)點使用了 tros Hobot Ssnros組件中的 /mipi_cam Node，參數(shù)指定使用 GC4663 攝像頭，發(fā)布的圖片格式和分辨率可以直接用于算法推理，其中通過共享內(nèi)存的方式發(fā)布圖片，可以極大地降低系統(tǒng)負(fù)載和傳輸延遲。

• 感知節(jié)點使用了 tros 中的 Boxs 算法倉庫，訂閱 Hobot Sensor（mipi_cam Node）發(fā)布的圖像數(shù)據(jù)后，通過視覺算法進行推理，檢測出人體關(guān)鍵部位；

• 人體檢測和跟蹤算法Node 訂閱 Hobot Senso r發(fā)布的圖像消息，利用 BPU 處理器進行AI推理，發(fā)布包含人體、人頭、人臉、人手框和人體關(guān)鍵點檢測結(jié)果的AI msg，并通過多目標(biāo)跟蹤（multi-target tracking，即MOT）功能，實現(xiàn)檢測框的跟蹤和ID分配。

• 交互節(jié)點，人體檢測結(jié)果訂閱節(jié)點 /ai_msg_sub_node，訂閱人體檢測和跟蹤節(jié)點發(fā)布的 Ai msg 消息，執(zhí)行跟蹤策略。根據(jù)人臉在畫面中的位置通過比例控制器計算出小車旋轉(zhuǎn)角度；

• 控制節(jié)點/ob_tracking_Robot_CmdVel，發(fā)布消息名為'/cmd_vel'的命令控制小車做旋轉(zhuǎn)運動；

• WEB展示，由 /hobot_codec 將 /mipi_cam 發(fā)布的 ’nv12‘ 格式的圖像轉(zhuǎn)換為 ’jpeg‘ 格式，以便 /websocket 展示。同時， /websocket 節(jié)點訂閱 /hobot_mono2d_body_detection 消息用于圖像渲染；

ob_face_tracking.launch.py 文件的代碼如下：

5 功能包的實現(xiàn)

功能包中的 py 文件如下：

功能包程序設(shè)計如下：

略縮圖如下：

main.py 代碼如下：

smart_subscriber.py 代碼如下:

face_tracking.py 代碼如下:

6 程序設(shè)計難點

難點1：Python 多線程 Queue：

Python Queue是線程安全的不同于C++，因此可以直接使用get()、put() 而不需要使用鎖Lock()；

難點2：PID人臉追蹤器：

• PID(proportion integration differentiation)?控制器只了P（proportion），因此為比例控制器，類似于一次函數(shù)；

• PID(5.0,?0.0,?0.0,?setpoint=0)? ?# P 經(jīng)過調(diào)試后被設(shè)置為 5，setpoint 目標(biāo)點為 0，即 NDC 空間下圖像的中點;

• pid.sample_time = 0.1? ? ? ? ? ? ? # 設(shè)置采樣率可以簡化 PID 的計算，推薦設(shè)置；采樣率與函數(shù)的調(diào)用頻率無關(guān)；

Q： 什么是 NDC 空間？

在計算機圖像中，NDC空間是指歸一化設(shè)備坐標(biāo)（Normalized Device Coordinates）空間。

攝像頭的分辨率各不相同，因此產(chǎn)生了 NDC，它可以將輸入的圖像歸一化至 [-1 ,1]區(qū)間方便計算。err =?float(((rect.x_offset + rect.width /?2) /?self.track_cfg_.img_width -?0.5) *?2)本例算法中，在NDC空間下，0.5即為圖像中心點。

您可以代入真實的人臉坐標(biāo)，進行計算，以便理解算法；

參考：

• 奇樂編程學(xué)院 - 人臉追蹤

• 阿凱愛玩機 - 云臺人臉追蹤

難點3：Ai msg 消息的解析

通信接口在 \opt\tros\share\ai_msgs\msg\ 路徑下：

PerceptionTargets.msg 接口如下：

Target.msg 接口如下：

Roi.msg 接口如下：

BUG：經(jīng)解析后，筆者認(rèn)為這里有一個數(shù)據(jù)結(jié)構(gòu)錯誤，但不影響程序使用；

PerceptionTargets.msg 中有一個 Target[] targets 數(shù)組，其中這個 target 的類型可以是人、車、動物、物體，這個 target 下有一個 Roi[] rois，這個 Roi 包含人體框、人頭框、人臉框、人手框多個元素；
然而在實際使用中發(fā)現(xiàn)：

rois[]數(shù)組中只包含一個元素，數(shù)據(jù)偽代碼如下：

我認(rèn)為正確的數(shù)據(jù)結(jié)構(gòu)如下：

正確 √

錯誤 ?

6 編譯運行

ssh連接小車，安裝 py 功能包依賴庫simple_pid：

1、使用 pip 安裝 simple_pid 庫：

2、執(zhí)行功能包根目錄下的 .\install.sh 腳本自動安裝依賴：

cd 進 /userdata/dev_ws/ 目錄，執(zhí)行如下命令編譯功能包；

使用 Launch 文件啟動人臉追蹤功能：

啟動小車底盤：

7 執(zhí)行效果