在AI操控機(jī)器人系列第一期的手勢(shì)控制教程中，身為地平線資深程序員的奶爸，使用地平線發(fā)布的機(jī)器人開(kāi)發(fā)平臺(tái)TogetherROS軟件棧，搭建了手勢(shì)操控機(jī)器人。
當(dāng)孩子的小伙伴們還是帶個(gè)手套或者使用遙控器指揮機(jī)器人跑跑時(shí)，手勢(shì)控制這么AI的機(jī)器人讓小朋友得意了很長(zhǎng)一段時(shí)間。（點(diǎn)擊一下，內(nèi)容回顧）
可是好景不長(zhǎng)，小朋友對(duì)待玩具都是喜新厭舊的，這就給我提了新的需求，小小年紀(jì)就展現(xiàn)了當(dāng)產(chǎn)品經(jīng)理的潛力。
“能不能讓機(jī)器人跟在我后面跑，就像咱們家狗狗那樣呢？”
為了體現(xiàn)老爸的厲害，這個(gè)人體跟隨機(jī)器人必須搞起（其實(shí)是機(jī)器人開(kāi)發(fā)平臺(tái)里面豐富的算法讓我這么有底氣）。

一、功能介紹

大家先看一下最終實(shí)現(xiàn)的功能。

從跟隨效果來(lái)看，當(dāng)人移動(dòng)時(shí)，機(jī)器人能夠迅速跟隨人移動(dòng)，得益于地平線旭日??X3派（以下簡(jiǎn)稱X3派）上5Tops算力的BPU可以實(shí)現(xiàn)低延遲（30ms左右）、高幀率（滿幀30fps）、遠(yuǎn)距離（>8m）、低系統(tǒng)負(fù)載（4核CPU只占用了單核<40%）的算法推理能力。

開(kāi)始碼代碼前，先來(lái)簡(jiǎn)單分析一下人體跟隨機(jī)器人需要具備哪些基本模塊。

傳感

對(duì)于人體跟隨這類(lèi)基于視覺(jué)的AI算法應(yīng)用，首先需要有視覺(jué)傳感器來(lái)捕捉圖像數(shù)據(jù)。

感知

獲取到傳感器發(fā)布的圖像后，通過(guò)視覺(jué)算法進(jìn)行推理，檢測(cè)到人體，實(shí)現(xiàn)感知的能力。

交互

檢測(cè)到人體之后，需要定義并實(shí)現(xiàn)對(duì)應(yīng)不同人體檢測(cè)結(jié)果的功能應(yīng)用，如人體在機(jī)器人的右邊需要控制機(jī)器人向右轉(zhuǎn)動(dòng)。

控制

根據(jù)“交互”模塊輸出的控制指令，實(shí)現(xiàn)對(duì)機(jī)器人的機(jī)械控制。

機(jī)器人本體

當(dāng)然還需要一個(gè)具備運(yùn)動(dòng)能力的機(jī)器人本體，接收控制指令并控制電機(jī)運(yùn)動(dòng)，實(shí)現(xiàn)最終的機(jī)器人跟隨人體的效果。

地平線發(fā)布的機(jī)器人平臺(tái)TogetherROS軟件棧內(nèi)置了豐富易用的機(jī)器人開(kāi)發(fā)組件，包含搭建一個(gè)智能機(jī)器人應(yīng)用（如人體跟隨機(jī)器人和第一期的機(jī)器人手勢(shì)控制）所涉及到的所有功能模塊，完全開(kāi)源免費(fèi)，并允許開(kāi)發(fā)者二次開(kāi)發(fā)，接下來(lái)開(kāi)始搞起。

二、準(zhǔn)備工作

準(zhǔn)備搭建機(jī)器人人體跟隨應(yīng)用案例的硬件設(shè)備和軟件包。

硬件
硬件包括：
①X3派

②F37 MIPI攝像頭

③機(jī)器人

X3派和TogetherROS適配了本末雙足機(jī)器人和小R科技的麥輪小車(chē)。這邊為了讓孩子覺(jué)得機(jī)器人足夠高大上，就用本末的輪足機(jī)器人來(lái)介紹，沒(méi)有的同學(xué)也不用擔(dān)心，文章后面會(huì)介紹自己搭建機(jī)器人的方式。
④其他配件

a.USB Type C接口電源線。至少搭配5V@2A適配器，用于X3派供電。

b.串口線，連接方式如下：

c.TF存儲(chǔ)卡和讀卡器。旭日X3派開(kāi)發(fā)板采用TF存儲(chǔ)卡作為系統(tǒng)啟動(dòng)介質(zhì)，推薦使用至少8GB容量、速率C10以上的TF存儲(chǔ)卡，以便滿足Ubuntu系統(tǒng)及更多應(yīng)用功能包對(duì)存儲(chǔ)空間的需求。

安裝系統(tǒng)
參考旭日X3派的用戶手冊(cè)的安裝系統(tǒng)章節(jié)，安裝完成（或者已經(jīng)安裝過(guò)）需要更新系統(tǒng)。

系統(tǒng)配置
配置X3派的無(wú)線網(wǎng)絡(luò)，參考X3派的用戶手冊(cè)的無(wú)線網(wǎng)絡(luò)章節(jié)。
無(wú)線網(wǎng)絡(luò)配置成功之后，查詢IP地址：

可以看到，X3派的無(wú)線網(wǎng)路分配的IP地址為192.168.1.147，下面開(kāi)始使用這個(gè)地址和root賬號(hào)（密碼為root）通過(guò)ssh遠(yuǎn)程連接到X3派，登錄成功后的狀態(tài)如下：

安裝TogetherROS

使用apt命令通過(guò)DEB包安裝TogetherROS。
登錄旭日X3派ssh root@192.168.1.147，執(zhí)行命令sudo apt install hhp，安裝過(guò)程如下：

root@ubuntu:~# sudo apt install hhp Reading package lists... Done Building dependency tree Reading state information... Done The following NEW packages will be installed: hhp0 upgraded, 1 newly installed, 0 to remove and 52 not upgraded. Need to get 384 MB of archives. After this operation, 512 MB of additional disk space will be used. Get:1 http://42.62.85.28/ubuntu-ports focal/main arm64 hhp arm64 1.0.1 [384 MB] Fetched 384 MB in 6min 43s (954 kB/s) Selecting previously unselected package hhp.(Reading database ... 110406 files and directories currently installed.) Preparing to unpack .../archives/hhp_1.0.1_arm64.deb ... Unpacking hhp (1.0.1) ... Setting up hhp (1.0.1) ... Generating locales (this might take a while)... en_US.ISO-8859-1... done en_US.UTF-8... done Generation complete.

查看/opt目錄下文件：

root@ubuntu:/userdata# ls /opt/ tros

可以看到TogetherROS已安裝在/opt目錄下，說(shuō)明安裝成功。至此，準(zhǔn)備工作結(jié)束，下面開(kāi)始使用這些硬件設(shè)備和軟件包搭建機(jī)器人人體跟隨App。

三、使用介紹

接下來(lái)開(kāi)始搞各個(gè)功能模塊，再一步步組裝起來(lái)，分別驗(yàn)證其功能。

安裝和測(cè)試F37攝像頭

將F37攝像頭通過(guò)MIPI線連接到X3派上，連接方式如下：

直接使用TogetherROS中的ROS package測(cè)試F37的功能。以下通過(guò)腳本啟動(dòng)攝像頭采集，圖像編碼，web展示功能包，實(shí)現(xiàn)將F37攝像頭采集到的圖像進(jìn)行編碼后，通過(guò)PC端web瀏覽器實(shí)時(shí)查看采集到的圖像。 登錄旭日X3派ssh root@192.168.1.147后，在終端（以下默認(rèn)終端都是通過(guò)ssh登錄）中輸入以下命令啟動(dòng)：

# 配置 TogetherROS 環(huán)境： source /opt/tros/local_setup.bash # 啟動(dòng)腳本 ros2 launch websocket hobot_websocket.launch.py

啟動(dòng)成功的終端狀態(tài)如下：

攝像頭開(kāi)始采集圖像并對(duì)外發(fā)布圖像消息。PC瀏覽器（chrome/firefox/edge）輸入X3派IP地址，即可查看F37實(shí)時(shí)采集到的圖像效果：

說(shuō)明F37攝像頭已安裝成功并能夠?qū)ν獍l(fā)布圖像消息。

測(cè)試人體檢測(cè)算法和交互功能
X3派上打開(kāi)一個(gè)終端，啟動(dòng)人體檢測(cè)和控制的腳本：

# 配置TogetherROS環(huán)境 source /opt/tros/setup.bash # 從TogetherROS的安裝路徑中拷貝出運(yùn)行示例需要的配置文件。 cp -r /opt/tros/lib/mono2d_body_detection/config/ . #啟動(dòng)launch文件 ros2 launch body_tracking hobot_body_tracking_without_gesture.launch.py

啟動(dòng)成功后，當(dāng)有人體出現(xiàn)在F37攝像頭前時(shí)，終端輸出如下信息：

[body_tracking-5] [INFO] [1657615485.941625685] [TrackingManager]: track_id: 1, frame_ts: 485910, tracking_sta(0:INITING, 1:TRACKING, 2:LOST): 1, gesture: 0, y pixel to robot: 2, present_rect: 540 6 954 542 [body_tracking-5] [INFO] [1657615485.941724902] [TrackingManager]: UpdateTrackAngle: frame_ts: 485910, track_id: 1, angel_with_robot: 45 [body_tracking-5] [INFO] [1657615485.942362166] [TrackingManager]: rotate direction: 0, step: 0.500000 [body_tracking-5] [WARN] [1657615485.942528139] [RobotCmdVelNode]: RobotCtl, angular: 0 0 -0.5, linear: 0 0 0, pub twist ts: 1657615485942515

以上log顯示機(jī)器人在跟隨目標(biāo)編號(hào)為1（track_id: 1）的人體（tracking_sta: 1），人體不在機(jī)器人的正前方，和機(jī)器人之間的角度為45度，跟隨交互模塊控制機(jī)器人以0.5r/s的速度向人體方向轉(zhuǎn)動(dòng)（rotate direction: 0, step: 0.500000）。 在PC端瀏覽器上會(huì)實(shí)時(shí)渲染顯示攝像頭采集到的圖像，人體、人頭、人臉和人手檢測(cè)框和目標(biāo)編號(hào)結(jié)果，人體骨骼關(guān)鍵點(diǎn)以及實(shí)時(shí)的性能統(tǒng)計(jì)數(shù)據(jù)，如下圖：

其中瀏覽器下方顯示的fps為實(shí)時(shí)的AI推理輸出的幀率，和F37攝像頭采集圖像的幀率30fps一致。ai_delay（單位為毫秒）為28，表示的是單幀推理的延遲，從傳感Node發(fā)布圖像數(shù)據(jù)開(kāi)始，經(jīng)過(guò)感知算法Node推理后，發(fā)布包含人體檢測(cè)結(jié)果的AI msg的耗時(shí)。 板端使用top命令查看系統(tǒng)資源占用如下圖所示，其中mono2d_body_detection為人體檢測(cè)算法推理進(jìn)程，此時(shí)CPU使用率為38.9%。

經(jīng)過(guò)以上測(cè)試，說(shuō)明在X3派上，TogetherROS的人體檢測(cè)算法運(yùn)行成功，并且算法可以實(shí)現(xiàn)低延遲（30ms）、高幀率（滿幀30fps）、遠(yuǎn)距離（>8m）、低系統(tǒng)負(fù)載（單核CPU占用<40%）的算法推理效果，體現(xiàn)了X3 BPU強(qiáng)大的算力。

人體檢測(cè)算法測(cè)試完成后，接著測(cè)試人體跟隨功能。X3派上另起一個(gè)終端，執(zhí)行如下命令查詢X3派上的話題列表：

# 配置TogetherROS環(huán)境 source /opt/tros/setup.bash ros2 topic list

輸出如下：

其中/cmd_vel話題是人體跟隨控制節(jié)點(diǎn)發(fā)布的控制命令消息。在當(dāng)前終端執(zhí)行ros2 topic echo /cmd_vel命令查詢X3派上的話題信息，當(dāng)人體出現(xiàn)在F37攝像頭前并且偏離正前方時(shí)，終端輸出如下：

可以看到，此時(shí)人體跟隨控制節(jié)點(diǎn)發(fā)布發(fā)布出的/cmd_vel話題的angular z數(shù)據(jù)為-0.5，表示以0.5r/s的速度轉(zhuǎn)動(dòng)。 說(shuō)明人體識(shí)別算法和交互功能的軟件包已安裝成功，并能夠通過(guò)/cmd_vel話題對(duì)外發(fā)布機(jī)器人運(yùn)動(dòng)控制消息。

將X3派安裝到機(jī)器人上

將X3派安裝到機(jī)器人上，并測(cè)試在X3派上通過(guò)發(fā)布/cmd_vel話題控制機(jī)器人運(yùn)動(dòng)的功能。連接了F37攝像頭的X3派直接固定在機(jī)器人上，并將機(jī)器人的USB控制接口插到X3派上。 安裝效果如下：

對(duì)于支持使用ROS開(kāi)發(fā)的機(jī)器人，一般會(huì)提供一個(gè)基于ROS開(kāi)發(fā)的機(jī)器人運(yùn)動(dòng)控制Node，功能為訂閱/cmd_vel話題的控制消息（ROS2中定義的用于機(jī)器人控制的消息，消息類(lèi)型為geometry_msgs/msg/Twist），根據(jù)控制協(xié)議，通過(guò)USB等接口向機(jī)器人發(fā)送運(yùn)動(dòng)控制指令，實(shí)現(xiàn)控制機(jī)器人運(yùn)動(dòng)的目的。

本文使用的本末雙足機(jī)器人使用的是USB接口，并提供了運(yùn)行在X3派上的運(yùn)動(dòng)控制package，package訂閱到/cmd_vel話題的控制消息后，通過(guò)USB向機(jī)器人下發(fā)控制指令，實(shí)現(xiàn)對(duì)機(jī)器人的控制。 在X3派上啟動(dòng)本末雙足機(jī)器人運(yùn)行控制Node。打開(kāi)一個(gè)終端，執(zhí)行如下命令：

# 配置TogetherROS環(huán)境 source /opt/tros/setup.bash #啟動(dòng)本末機(jī)器人運(yùn)動(dòng)控制package ros2 run diablo_sdk ros_bridge_example

執(zhí)行成功后終端中輸出如下信息：

X3派上重新打開(kāi)一個(gè)終端，通過(guò)發(fā)布/cmd_vel話題消息控制機(jī)器人以0.3r/s的速度轉(zhuǎn)動(dòng)：

# 配置TogetherROS環(huán)境 source /opt/tros/setup.bash ros2 topic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.3}}'

執(zhí)行成功后終端中輸出如下信息：

機(jī)器人收到控制指令后轉(zhuǎn)動(dòng)的效果如下：

說(shuō)明機(jī)器人能夠按照發(fā)布的控制命令消息正確的實(shí)現(xiàn)運(yùn)動(dòng)。

對(duì)于其他機(jī)器人應(yīng)該怎么安裝
如果手里是其他的移動(dòng)機(jī)器人，例如有一個(gè)使用樹(shù)莓派或者Jetson Nano作為上位機(jī)的機(jī)器人，也可以將X3派安裝在機(jī)器人上，代替樹(shù)莓派或者Jetson Nano，實(shí)現(xiàn)控制機(jī)器人運(yùn)動(dòng)。 安裝方法如下：
①編譯可以運(yùn)行在X3派上的運(yùn)動(dòng)控制package；

a.X3派上安裝ROS2軟件系統(tǒng)構(gòu)建和編譯工具：

apt update apt-get install python3-catkin-pkg pip3 install empy pip3 install -U colcon-common-extensions

b.將原先運(yùn)行在樹(shù)莓派或者Jetson Nano上的機(jī)器人運(yùn)動(dòng)控制ROS2 package源碼拷貝到X3派上；

c.在X3派上，package源碼工程所在路徑下，直接使用source /opt/tros/setup.bash; colcon build命令編譯package；

d.如果原先運(yùn)動(dòng)控制package是基于ROS1開(kāi)發(fā)，源碼需要適配到ROS2。只需要適配"cmd_vel"話題消息的訂閱和處理，如果原先的ROS1 package中有其他功能，可以先不關(guān)注。

②安裝

a.將X3派固定在機(jī)器人上，如果空間有限，可以將原先的樹(shù)莓派或者Jetson Nano拆除；

b.使用USB Type C給X3派供電，如果機(jī)器人上無(wú)Type C供電輸出，也可以使用移動(dòng)電源（輸出至少5V&直流 2A）給X3派供電；

c.將機(jī)器人的USB控制接口插到X3派上。

③測(cè)試

a.X3派上啟動(dòng)新編譯的機(jī)器人運(yùn)動(dòng)控制package；

b.X3派上重新打開(kāi)一個(gè)終端，通過(guò)發(fā)布/cmd_vel話題消息控制機(jī)器人以0.3r/s的速度轉(zhuǎn)動(dòng)：

# 配置TogetherROS環(huán)境 source /opt/tros/setup.bash ros2 topic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.3}}'

如果機(jī)器人正常轉(zhuǎn)動(dòng)，說(shuō)明X3派安裝成功。

四、完整的機(jī)器人人體跟隨效果

下面開(kāi)始測(cè)試完整的機(jī)器人人體跟隨功能。

X3派上打開(kāi)一個(gè)終端，啟動(dòng)人體檢測(cè)和控制腳本

# 配置TogetherROS環(huán)境 source /opt/tros/setup.bash # 從TogetherROS的安裝路徑中拷貝出運(yùn)行示例需要的配置文件。 cp -r /opt/tros/lib/mono2d_body_detection/config/ . #啟動(dòng)launch文件 ros2 launch body_tracking hobot_body_tracking_without_gesture.launch.py

X3派上打開(kāi)一個(gè)終端，啟動(dòng)機(jī)器人運(yùn)動(dòng)控制Node

# 配置TogetherROS環(huán)境 source /opt/tros/setup.bash #啟動(dòng)本末機(jī)器人運(yùn)動(dòng)控制Node ros2 run diablo_sdk ros_bridge_example

機(jī)器人跟隨人移動(dòng)

五、原理分析

第4章節(jié)中，在X3派的兩個(gè)終端中分別啟動(dòng)了hobot_body_tracking_without_gesture.launch.py腳本和運(yùn)動(dòng)控制Node，實(shí)現(xiàn)了機(jī)器人跟隨人體的效果，本章節(jié)對(duì)實(shí)現(xiàn)的原理進(jìn)行分析。

App運(yùn)行時(shí)的Node和Topic信息
對(duì)于一個(gè)基于ROS開(kāi)發(fā)的App，首先會(huì)想到這個(gè)App在運(yùn)行時(shí)有哪些Node，這些Node發(fā)布和訂閱了哪些Topic，以及這些Node之間的關(guān)聯(lián)。
在X3派上使用ros2的命令行工具查詢?cè)O(shè)備上運(yùn)行的Node和Topic信息：

root@ubuntu:~# source /opt/tros/setup.bash root@ubuntu:~# ros2 node list /ai_msg_sub_node /hobot_codec1658324595406906403 /horizon_tracking_RobotCmdVel /mipi_cam /mono2d_body_det /ros_bridge_example /tracking_strategy_parameter_node /transform_listener_impl_5594bf54a0 /websocket root@ubuntu:~# ros2 topic list /cmd_vel /hbmem_img080a1309022201080401012021072312 /hobot_mono2d_body_detection /image_jpeg /image_raw /imu/data_raw /odom /parameter_events /quat_odom /raw_odom /rosout /tf /tf_static

查詢到X3派上運(yùn)行著多個(gè)Node，這些ROS2 Node之間是基于pub&sub機(jī)制通信，通過(guò)topic將這些Node串聯(lián)起來(lái)形成一個(gè)pipeline。
此App運(yùn)行時(shí)Node以及Topic信息比較多，看不出Node之間的關(guān)聯(lián)?？梢栽赑C端通過(guò)rqt（PC端需要安裝ROS2 Foxy版本，rqt，以及PC需要和X3派處于同一網(wǎng)段）的Node Graph功能可以可視化的展示X3派上運(yùn)行的Node，Node發(fā)布和訂閱的topic，以及Node之間的連接關(guān)系，如下圖（其中橢圓形框內(nèi)為Node名，矩形框內(nèi)為topic名）：

可以看到，整個(gè)graph（pipeline）以mipi_cam Node（圖像采集和發(fā)布）為起點(diǎn)，websocket Node（序列化圖片和AI結(jié)果，用于可視化展示）和ros_bridge_example Node（機(jī)器人運(yùn)動(dòng)控制）為終點(diǎn)，起點(diǎn)和終點(diǎn)之間連接著多個(gè)Node。這些Node中ros_bridge_example Node是通過(guò)ros2 run diablo_sdk ros_bridge_example命令啟動(dòng)，其余Node都是通過(guò)hobot_body_tracking_without_gesture.launch.py腳本啟動(dòng)。

注意：/ai_msg_sub_node節(jié)點(diǎn)訂閱到的/hobot_mono2d_body_detection話題消息實(shí)際是由/horizon_tracking_RobotCmdVel節(jié)點(diǎn)處理，導(dǎo)致顯示的graph不連續(xù)，屬于設(shè)計(jì)上的待完善點(diǎn)

App的Node介紹
對(duì)于復(fù)雜的包含多個(gè)Node的應(yīng)用，ROS2提供了使用啟動(dòng)腳本通過(guò)launch批量啟動(dòng)Node的功能。
機(jī)器人跟隨人體App使用hobot_body_tracking_without_gesture.launch.py腳本來(lái)啟動(dòng)這些Node，腳本內(nèi)容如下：

import os from launch import LaunchDescription from launch_ros.actions import Node from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python import get_package_share_directory def generate_launch_description(): web_service_launch_include = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( get_package_share_directory('websocket'), 'launch/hobot_websocket_service.launch.py')) ) return LaunchDescription([ web_service_launch_include, # 啟動(dòng)圖片發(fā)布pkg Node( package='mipi_cam', executable='mipi_cam', output='screen', parameters=[ {"out_format": "nv12"}, {"image_width": 960}, {"image_height": 544}, {"io_method": "shared_mem"}, {"video_device": "F37"} ], arguments=['--ros-args', '--log-level', 'error'] ), # 啟動(dòng)jpeg圖片編碼&發(fā)布pkg Node( package='hobot_codec', executable='hobot_codec_republish', output='screen', parameters=[ {"channel": 1}, {"in_mode": "shared_mem"}, {"in_format": "nv12"}, {"out_mode": "ros"}, {"out_format": "jpeg"}, {"sub_topic": "/hbmem_img"}, {"pub_topic": "/image_jpeg"} ], arguments=['--ros-args', '--log-level', 'error'] ), # 啟動(dòng)單目rgb人體、人頭、人臉、人手框和人體關(guān)鍵點(diǎn)檢測(cè)pkg Node( package='mono2d_body_detection', executable='mono2d_body_detection', output='screen', parameters=[ {"ai_msg_pub_topic_name": "/hobot_mono2d_body_detection"} ], arguments=['--ros-args', '--log-level', 'error'] ), # 啟動(dòng)web展示pkg Node( package='websocket', executable='websocket', output='screen', parameters=[ {"image_topic": "/image_jpeg"}, {"image_type": "mjpeg"}, {"smart_topic": "/hobot_mono2d_body_detection"} ], arguments=['--ros-args', '--log-level', 'error'] ), # 啟動(dòng)人體跟隨pkg Node( package='body_tracking', executable='body_tracking', output='screen', parameters=[ {"ai_msg_sub_topic_name": "/hobot_mono2d_body_detection"}, {"activate_wakeup_gesture": 0}, {"img_width": 960}, {"img_height": 544}, {"track_serial_lost_num_thr": 30}, {"move_step": 0.5}, {"rotate_step": 0.5}, {"activate_robot_move_thr": 5} ], arguments=['--ros-args', '--log-level', 'info'] ) ])

腳本中指定了多個(gè)Node，其中每個(gè)Node中的package配置項(xiàng)表示Node名，可以看到腳本中配置的Node名和5.1章節(jié)中查出來(lái)的一致。

在第1章節(jié)，分析了機(jī)器人人體跟隨App所需要具備的功能模塊，下面按照這些功能模塊對(duì)Node進(jìn)行分析。

傳感

使用了TogetherROS中的Hobot Sensor組件中的mipi_cam Node，同時(shí)指定了Node的參數(shù)：

"out_format"：發(fā)布圖片的格式為"nv12"

"image_width"：圖片分辨率寬為960

"image_height"：圖片分辨率高為544

"io_method"：發(fā)布出來(lái)的圖片傳輸方式為"shared_mem"，即共享內(nèi)存方式，對(duì)應(yīng)消息的topic為"hbmem_img"

"video_device"：指定了使用的MIPI攝像頭類(lèi)型為"F37"，表示F37攝像頭

參數(shù)指定了使用F37攝像頭，發(fā)布的圖片格式和分辨率可以直接用于算法推理，其中通過(guò)共享內(nèi)存方式發(fā)布圖片，可以極大地降低系統(tǒng)負(fù)載和傳輸延遲。

感知

使用了TogetherROS中的Boxs算法倉(cāng)庫(kù)中的人體檢測(cè)和跟蹤算法，訂閱Hobot Sensor（mipi_cam Node）發(fā)布的圖像消息，利用BPU處理器進(jìn)行AI推理，發(fā)布包含人體、人頭、人臉、人手框和人體骨骼關(guān)鍵點(diǎn)檢測(cè)結(jié)果的AI msg，并通過(guò)多目標(biāo)跟蹤（multi-target tracking，即MOT）功能，實(shí)現(xiàn)檢測(cè)框的跟蹤和目標(biāo)編號(hào)分配。
Node對(duì)應(yīng)package名為'mono2d_body_detection'，同時(shí)指定了Node的參數(shù)：

"ai_msg_pub_topic_name"：發(fā)布包含人體檢測(cè)框信息的AI感知結(jié)果的topic名為"/hobot_mono2d_body_detection"

人體檢測(cè)和跟蹤算法推理Node（Mono2dBodyDetNode）主要包含三部分邏輯獨(dú)立的功能：

Node初始化和啟動(dòng)

配置算法使用的模型信息，創(chuàng)建消息的發(fā)布者和訂閱者，啟動(dòng)目標(biāo)跟蹤算法引擎；
訂閱消息和算法推理

在注冊(cè)的圖像消息回調(diào)中，處理圖像數(shù)據(jù)并用于算法模型推理，回調(diào)中不等待算法推理完成；
推理結(jié)果的處理和發(fā)布

算法推理完成后，通過(guò)注冊(cè)的回調(diào)PostProcess輸出推理結(jié)果，回調(diào)中對(duì)檢測(cè)結(jié)果按照時(shí)間順序排序（NodeOutputManage）以及進(jìn)行多目標(biāo)跟蹤算法（HobotMot）處理后，發(fā)布算法推理結(jié)果消息。

Node的設(shè)計(jì)和流程邏輯如下圖：

交互

人體跟隨策略Node訂閱人體檢測(cè)和跟蹤算法發(fā)布的包含人體信息的AI msg，根據(jù)人體框和機(jī)器人的位置關(guān)系，發(fā)布前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)的控制消息，實(shí)現(xiàn)控制機(jī)器人運(yùn)動(dòng)。 Node發(fā)布的運(yùn)動(dòng)控制消息為ROS2中定義的消息，topic為“/cmd_vel”，消息類(lèi)型為“geometry_msgs/msg/Twist”，Node對(duì)應(yīng)package名為'body_tracking'，同時(shí)指定了Node的參數(shù)：

"ai_msg_sub_topic_name": 訂閱包含人體檢測(cè)信息的topic名為"/hobot_mono2d_body_detection"

"activate_wakeup_gesture": 喚醒手勢(shì)開(kāi)關(guān)，值為0表示不啟用喚醒手勢(shì)。一般在人較多，環(huán)境復(fù)雜的場(chǎng)景，通過(guò)啟用喚醒手勢(shì)避免誤觸發(fā)人體跟隨功能。

"img_width": 人體檢測(cè)模型輸入圖片像素的寬

"img_height": 人體檢測(cè)模型輸入圖片像素的高

"track_serial_lost_num_thr": 人體連續(xù)消失幀數(shù)閾值，值為30，表示當(dāng)跟隨的人體連續(xù)消失30幀之后會(huì)重新選擇跟隨的人體

"move_step": 平移運(yùn)動(dòng)的步長(zhǎng)（速度），0.5表示移動(dòng)速度為0.5m/s，值越大速度越快

"rotate_step": 旋轉(zhuǎn)運(yùn)動(dòng)的步長(zhǎng)（速度），0.5表示旋轉(zhuǎn)速度為0.5r/s，值越大速度越快

"activate_robot_move_thr": 激活機(jī)器人移動(dòng)的閾值，單位為像素。當(dāng)人體檢測(cè)框的y像素值小于閾值（具體畫(huà)面的上邊界）時(shí)，激活機(jī)器人移動(dòng)。

人體跟隨策略選擇第一次出現(xiàn)的人體作為跟隨人體，如果有多個(gè)人體同時(shí)出現(xiàn)，選擇最大寬度的人體檢測(cè)框作為跟隨人體；已有跟隨人的情況下，其他的人體檢測(cè)結(jié)果都無(wú)效。 Node輸出的log中tracking_sta關(guān)鍵字表示跟隨的狀態(tài)，0表示未找到跟隨人，1表示已有跟隨人，2表示跟隨人消失。Node啟動(dòng)后，未找到跟隨人的情況下，輸出log中tracking_sta值為0；當(dāng)有跟隨人時(shí)，輸出log中tracking_sta值為1，同時(shí)通過(guò)track_id關(guān)鍵字輸出跟隨人的編號(hào)；只有當(dāng)跟隨人消失，即連續(xù)track_serial_lost_num_thr幀（配置為30幀，對(duì)于輸出頻率為30fps的F37攝像頭，為1秒）未檢測(cè)到跟隨人，判斷跟隨人消失，開(kāi)始重新選擇跟隨人，跟隨人消失時(shí)輸出log中tracking_sta值為2。

如果需要啟用喚醒手勢(shì)避免誤觸發(fā)，喚醒手勢(shì)使用方法詳見(jiàn)人體跟隨策略的代碼倉(cāng)庫(kù)。

控制

機(jī)器人運(yùn)動(dòng)控制Node訂閱人體跟隨策略Node發(fā)布的topic為“/cmd_vel”的控制消息，根據(jù)控制協(xié)議，通過(guò)USB總線向機(jī)器人下位機(jī)發(fā)布運(yùn)動(dòng)控制指令。對(duì)于不同類(lèi)型的機(jī)器人，控制協(xié)議不同，對(duì)應(yīng)于不同的運(yùn)動(dòng)控制Node。本文使用的是本末雙足機(jī)器人，對(duì)應(yīng)的運(yùn)動(dòng)控制Node啟動(dòng)方法為ros2 run diablo_sdk ros_bridge_example。此Node單獨(dú)啟動(dòng)，不在啟動(dòng)腳本中。

App的系統(tǒng)設(shè)計(jì)
根據(jù)5.1和5.2章節(jié)的介紹，已經(jīng)知道了機(jī)器人人體跟隨App啟動(dòng)了哪些Node，這些Node的功能，Node之間的關(guān)系，以及使用這些Node如何實(shí)現(xiàn)通過(guò)人體跟隨機(jī)器人的目標(biāo)。下面進(jìn)行理論總結(jié)，介紹此App的系統(tǒng)設(shè)計(jì)。對(duì)于一個(gè)復(fù)雜的機(jī)器人系統(tǒng)，一般在機(jī)器人上配置上位機(jī)和下位機(jī)兩種處理器，機(jī)器人上位機(jī)的計(jì)算能力較強(qiáng)，執(zhí)行復(fù)雜的機(jī)器人上層應(yīng)用，同時(shí)能夠最大程度屏蔽不同類(lèi)型機(jī)器人的底層差異；機(jī)器人下位機(jī)一般使用低成本的MCU處理器，對(duì)機(jī)器人本體上的各類(lèi)傳感器和硬件進(jìn)行數(shù)據(jù)采集/控制。

人體跟隨App由兩部分組成，分別為機(jī)器人和PC端，其中機(jī)器人部分又分為上位機(jī)和下位機(jī)。詳細(xì)組成如下圖：

機(jī)器人上位機(jī)為X3派，運(yùn)行著多個(gè)ROS2 Node，除了5.2章節(jié)介紹的傳感、感知、交互和控制功能，還有JPEG圖像編碼和WEB展示功能，將攝像頭發(fā)布的圖片編碼壓縮，以及將人體檢測(cè)和跟蹤算法發(fā)布的AI數(shù)據(jù)序列化后使用websocket協(xié)議發(fā)布，實(shí)現(xiàn)跨設(shè)備在PC端渲染展示和調(diào)試（機(jī)器人下位機(jī)屬于機(jī)器人本體的一部分，詳細(xì)說(shuō)明略）。

從App的系統(tǒng)設(shè)計(jì)圖中可以看出，搭載了X3派和TogetherROS的機(jī)器人，利用芯片的AI加速能力和TogetherROS中豐富的算法、機(jī)器人開(kāi)發(fā)組件，可以實(shí)現(xiàn)快速開(kāi)發(fā)智能機(jī)器人應(yīng)用的目標(biāo)。

六、FAQ

如何復(fù)現(xiàn)App效果？
復(fù)現(xiàn)App效果涉及到兩部分：
（1）機(jī)器人人體跟隨App
參考第2章準(zhǔn)備工作，在X3派上安裝TogetherROS。
（2）本末雙足機(jī)器人和機(jī)器人運(yùn)動(dòng)控制package
獲取方法詳見(jiàn)產(chǎn)品信息：https://developer.horizon.ai/forumDetail/94246984227025410
除了本末雙足機(jī)器人，X3派和TogetherROS還適配了小R科技的麥輪小車(chē)，也可以使用小R小車(chē)直接體驗(yàn)App效果。

沒(méi)有機(jī)器人的情況下可以體驗(yàn)App效果嗎？
可以體驗(yàn)。 在沒(méi)有機(jī)器人的情況下，可以使用此App控制gazebo仿真環(huán)境下的虛擬機(jī)器人運(yùn)動(dòng)。

如何將App適配到自己的機(jī)器人上？
本文以本末雙足機(jī)器人為例介紹人體跟隨App的效果，App本身不依賴于任何形態(tài)的機(jī)器人，App發(fā)布的運(yùn)動(dòng)控制消息為ROS2中定義的消息（topic為“/cmd_vel”，消息類(lèi)型為“geometry_msgs/msg/Twist”，具體說(shuō)明參考第5章的原理分析）。
如下圖，可以將App的組成劃分成紅色和藍(lán)色虛線框兩個(gè)部分：

（1）紅色虛線框部分
這部分功能不依賴于機(jī)器人，即可以直接移植到任意形態(tài)的機(jī)器人上。
移植方法為將X3派安裝在機(jī)器人上，按照第2章節(jié)的準(zhǔn)備工作，在X3派上安裝攝像頭傳感器和TogetherROS。
（2）藍(lán)色虛線框部分
這部分功能依賴于機(jī)器人，需要針對(duì)性的適配。根據(jù)機(jī)器人的狀態(tài)不同，對(duì)應(yīng)不同的適配方法。
狀態(tài)1，原先機(jī)器人上有上位機(jī)和下位機(jī)，如原先使用樹(shù)莓派或者Jetson Nano作為上位機(jī)，并且上位機(jī)上有機(jī)器人運(yùn)動(dòng)控制Node。需要在X3派上重新編譯機(jī)器人運(yùn)動(dòng)控制Node。
狀態(tài)2，原先機(jī)器人上只有下位機(jī)。需要開(kāi)發(fā)機(jī)器人運(yùn)動(dòng)控制Node后（開(kāi)發(fā)參考components/xrrobot · develop · HHP / app / xr_robot · GitLab (horizon.ai)），在X3派上編譯機(jī)器人運(yùn)動(dòng)控制Node。

App支持哪些攝像頭？
App對(duì)于攝像頭類(lèi)型沒(méi)有要求，地平線機(jī)器人平臺(tái)支持MIPI和USB兩類(lèi)攝像頭。對(duì)于MIPI攝像頭，支持F37和GC4663兩種型號(hào)。

如何調(diào)整機(jī)器人的運(yùn)動(dòng)速度？
修改App啟動(dòng)腳本hobot_body_tracking_without_gesture.launch.py中人體跟隨策略body_tracking Node中的move_step和rotate_step參數(shù)，可以控制機(jī)器人的平移和旋轉(zhuǎn)速度：

# 啟動(dòng)人體跟隨pkg Node( package='body_tracking', executable='body_tracking', output='screen', parameters=[ {"ai_msg_sub_topic_name": "/hobot_mono2d_body_detection"}, {"activate_wakeup_gesture": 0}, {"img_width": 960}, {"img_height": 544}, {"track_serial_lost_num_thr": 30}, {"move_step": 0.5}, {"rotate_step": 0.5}, {"activate_robot_move_thr": 5} ], arguments=['--ros-args', '--log-level', 'info'] )

可以開(kāi)發(fā)一個(gè)python的Node擴(kuò)展App功能嗎？
可以，ROS2支持跨設(shè)備、跨平臺(tái)、跨語(yǔ)言，TogetherROS完全兼容ROS2 Foxy版本，因此也支持這些特性。
例如根據(jù)檢出的人體骨骼關(guān)鍵點(diǎn)，開(kāi)發(fā)交互Node，控制人形機(jī)器人模仿人的肢體動(dòng)作。訂閱hobot_mono2d_body_detection話題消息（包含人體骨骼關(guān)鍵點(diǎn)信息），使用命令可以查詢到話題的詳細(xì)信息：

# ros2 topic info /hobot_mono2d_body_detection Type: ai_msgs/msg/PerceptionTargets Publisher count: 1 Subscription count: 1

消息類(lèi)型為ai_msgs/msg/PerceptionTargets，因此需要從ai_msgs.msg中import PerceptionTargets消息package。
完整的python代碼示例：

import rclpy from rclpy.node import Node from ai_msgs.msg import PerceptionTargets class MinimalSubscriber(Node): def __init__(self): super().__init__('minimal_subscriber') self.subscription = self.create_subscription( PerceptionTargets, 'hobot_mono2d_body_detection', self.listener_callback, 10) self.subscription # prevent unused variable warning def listener_callback(self, msg): self.get_logger().info('I heard: "%s"\n' % msg) def main(args=None): rclpy.init(args=args) minimal_subscriber = MinimalSubscriber() rclpy.spin(minimal_subscriber) # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) minimal_subscriber.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()

使用python開(kāi)發(fā)完Node后，直接在X3派上編譯并運(yùn)行。運(yùn)行并輸出訂閱到的消息：

如何開(kāi)發(fā)一個(gè)自己的算法Node擴(kuò)展App功能？
TogetherROS提供的Hobot DNN簡(jiǎn)化板端AI模型推理與部署，釋放BPU算力，降低AI使用門(mén)檻。同時(shí)內(nèi)置了常用的檢測(cè)、分類(lèi)和分割算法的模型后處理，幫助用戶快速在X3派上集成部署自己的算法。

本文轉(zhuǎn)自地平線開(kāi)發(fā)者社區(qū)

原作者：zhuk

原鏈接：https://developer.horizon.ai/forumDetail/106482197149630465