使用deepstream對自己模型進行加速推理以及與ROS通信

背景：deepstream檢測到的結果：框位置和目標類型、概率值需要進一步傳遞到ros節點中分析使用，本例是採用TCP/IP通信協議將這些數據發送出去

來源：使用darknet框架，利用yolov3-tiny模型在nvidia jetson nano上進行目標檢測推理的時候，幀率較低，約6ps，不能滿足實際任務需求。慶幸的是Nvidia提供了很多加速工具，典型的如tensorRT和deepstream。

1.deepstream使用過程

1.1 安裝

如果是使用nvidia的設備如nano　，如在燒錄系統時不自定義安裝的話，deepstream會一併被安裝上。
　　如果沒有安裝，可按照如下方式進行安裝：

1.1.1 安裝依賴：

$ sudo apt install \
    libssl1.0.0 \
    libgstreamer1.0-0 \
    gstreamer1.0-tools \
    gstreamer1.0-plugins-good \
    gstreamer1.0-plugins-bad \
    gstreamer1.0-plugins-ugly \
    gstreamer1.0-libav \
    libgstrtspserver-1.0-0 \
    libjansson4=2.11-1

1.1.2 安裝librdkafka

sudo apt-get install librdkafka1=0.11.3-1build1

1.1.3 安裝DeepStream SDK

從這裏下載，然後通過如下執行解壓縮：

$ tar -xpvf deepstream_sdk_v4.0.2_jetson.tbz2

之後依次執行如下指令進行安裝：

$ cd deepstream_sdk_v4.0.2_jetson
$ sudo tar -xvpf binaries.tbz2 -C /
$ sudo ./install.sh
$ sudo ldconfig

1.2 例子－加速官方模型進行推理

可以首先運行這個例子，來嚐鮮，之後來解釋其實際實現過程:
###　1.2.1 加速yolov3-tiny模型並推理:

cd /home/inano/deepstream/sources/objectDetector_Yolo
deepstream-app -c deepstream_app_config_yoloV3_tiny.txt

如果一切運行正常,且是第一次運行(目錄中沒有deepstream生成的加速模型),deepstream會下載yoloV3_tiny的權重文件，配置參數等文件，然後接着會生成加速模型，這個可能會需要一段時間，之後便會顯示實時的圖像及檢測效果(含檢測框)。若非第一次運行，即deepstream檢測到目錄中已經有加速模型，則直接開始進行推理，耗時也會短很多。

可以看到deepstream使用過程很簡單,通過使用deepstream-app來加載配置文件就可以，所以一個關鍵的點就是這個配置文件：

####　1.2.1.1 deepstream_app_config_yoloV3_tiny.txt文件
文件的有效部分如下,一般需要修改的參數如註釋所示:

[application]
enable-perf-measurement=1
perf-measurement-interval-sec=5
#gie-kitti-output-dir=streamscl

[tiled-display]
enable=1
rows=1
columns=1
width=1280            # 在推理中，要使用的圖像大小 
height=720
gpu-id=0
#(0): nvbuf-mem-default - Default memory allocated, specific to particular platform
#(1): nvbuf-mem-cuda-pinned - Allocate Pinned/Host cuda memory, applicable for Tesla
#(2): nvbuf-mem-cuda-device - Allocate Device cuda memory, applicable for Tesla
#(3): nvbuf-mem-cuda-unified - Allocate Unified cuda memory, applicable for Tesla
#(4): nvbuf-mem-surface-array - Allocate Surface Array memory, applicable for Jetson
nvbuf-memory-type=0

[source0]
enable=1
#Type - 1=Camera　V4L2 2=URI 3=MultiURI
type=3　　　　　　　　　　　　　　　　　　　　　　　　# 如果要用攝像頭進行檢測，就將type設置爲０
uri=file://../../samples/streams/sample_1080p_h264.mp4
num-sources=1
gpu-id=0
# (0): memtype_device   - Memory type Device
# (1): memtype_pinned   - Memory type Host Pinned
# (2): memtype_unified  - Memory type Unified
cudadec-memtype=0

[sink0]
enable=1
#Type - 1=FakeSink 2=EglSink 3=File
type=2
sync=0
source-id=0
gpu-id=0
nvbuf-memory-type=0

[osd]
enable=1
gpu-id=0
border-width=1
text-size=15
text-color=1;1;1;1;
text-bg-color=0.3;0.3;0.3;1
font=Serif
show-clock=0
clock-x-offset=800
clock-y-offset=820
clock-text-size=12
clock-color=1;0;0;0
nvbuf-memory-type=0

[streammux]
gpu-id=0
##Boolean property to inform muxer that sources are live
live-source=0
batch-size=1
##time out in usec, to wait after the first buffer is available
##to push the batch even if the complete batch is not formed
batched-push-timeout=40000
## Set muxer output width and height
width=1920
height=1080
##Enable to maintain aspect ratio wrt source, and allow black borders, works
##along with width, height properties
enable-padding=0
nvbuf-memory-type=0

# config-file property is mandatory for any gie section.
# Other properties are optional and if set will override the properties set in
# the infer config file.
[primary-gie]
enable=1
gpu-id=0
#model-engine-file=model_b1_fp32.engine
labelfile-path=labels.txt
batch-size=1
#Required by the app for OSD, not a plugin property
bbox-border-color0=1;0;0;1
bbox-border-color1=0;1;1;1
bbox-border-color2=0;0;1;1
bbox-border-color3=0;1;0;1
gie-unique-id=1
nvbuf-memory-type=0
config-file=config_infer_primary_yoloV3_tiny.txt　　　　　#加載的配置文件(這個裏面要設置我們想要加速的模型，以及模型相關的配置文件)

[tests]
file-loop=0

1.2.1.2　config_infer_primary_yoloV3_tiny.txt文件

該文件中要指定的有效信息很多，若模型文件，模型配置文件等。

[property]
gpu-id=0
net-scale-factor=1
#0=RGB, 1=BGR
model-color-format=0
custom-network-config=yolov3-tiny.cfg   #　模型配置文件
model-file=yolov3-tiny.weights　　　　　　#  模型權重文件

model-engine-file=model_b1_fp32.engine　 #  這裏可以直接給出engine模型，就不用每次浪費大量時間來轉換模型了，v3尤其浪費時間。每次改動需要把其屏蔽，執行時打開
labelfile-path=labels.txt　　　　　　　　  # 標籤文件，即模型對應的要檢測的圖片列表
## 0=FP32, 1=INT8, 2=FP16 mode
network-mode=0　　　　　　　　　　　　　　　 # 網絡計算要使用的浮點數類型，這個要看你GPU設備是否支持以及你的算力要求，默認爲FP32
num-detected-classes=80　　　　　　　　　　# 模型分類的個數
gie-unique-id=1
is-classifier=0
maintain-aspect-ratio=1
parse-bbox-func-name=NvDsInferParseCustomYoloV3Tiny　　
#　下面加載的是用於檢測後處理的一個動態庫，是deepstream前端檢測和後端後處理之間的一個接口該動態庫是將nvdsinfer_custom_impl_Yolo目錄下的文件編譯得到的，nvdsinfer_custom_impl_Yolo主要是面向後處理的，如檢測完後，矩形框繪製，檢測類型顯示，閾值設置等。如果需要修改這些參數，則修改完後需要重新編譯。　
custom-lib-path=nvdsinfer_custom_impl_Yolo/libnvdsinfer_custom_impl_Yolo.so

1.2.2 加速yolov3並推理:

安裝同樣的運行方式可以查看yolov3加速推理的效果。

$ cd /home/inano/deepstream/sources/objectDetector_Yolo
$ deepstream-app -c deepstream_app_config_yoloV3.txt

1.3 實踐－加速自己的模型完成推理

根據上述配置文件的描述，我們想加速我們自己訓練好的yolov3-tiny模型並使用攝像頭實時推理,需要進行如下幾步驟:

1.3.1 準備必要的文件

需要將我們自己的模型文件xf_yolov3-tiny.weights,xf_yolov3-tiny.cfg,imagelist.txt拷貝到/home/inano/deepstream/sources/objectDetector_Yolo目錄中，這些是加速必須的文件。

1.3.2 修改deepstream_app_config_yoloV3_tiny.txt文件

1.3.2.1 修改圖像來源

我們圖像來源於攝像頭,即將deepstream_app_config_yoloV3_tiny.txt文件中，type=3　修改爲　　type=１;

1.3.2.2 修改圖像大小

由於nano算力有限約0.47t,爲了想使推理加速後有高的幀率，將圖像大小修改，即將即將deepstream_app_config_yoloV3_tiny.txt文件中;width=1280　height=720,修改爲width=640　height=360

1.3.3 修改config_infer_primary_yoloV3_tiny.txt文件

1.3.3.1 指定模型配置文件

由於文件就在本文件夾中，所以不需要使用什麼絕對路徑和相對路徑，直接修改，即：
將文件中　　custom-network-config=yolov3-tiny.cfg　　　修改爲　　　custom-network-config=xf_yolov3-tiny.cfg

1.3.3.2 指定模型文件

由於文件就在本文件夾中，所以不需要使用什麼絕對路徑和相對路徑，直接修改，即：
將文件中　　model-file=yolov3-tiny.weights　　　修改爲　　　model-file=xf_yolov3-tiny.weights

1.3.3.3 指定圖片集

將labelfile-path=labels.txt 修改爲　　　labelfile-path=imagelist.txt

1.3.3.4 修改類型個數

將num-detected-classes=80　　　修改爲　　num-detected-classes=12 (模型對應的imagelist對應12種物品)

2.與ROS進行集成

如果想要使用檢測完的結果，如根據識別結果進行其他的操作。就需要我們將裏面的檢測框位置、類型、置信度等信息提取出來給其他節點使用。我們這裏的需求是將其結果傳遞給機器人動作執行節點，即需要將檢測結果發送到ros網絡中。在這裏我們通過socket通信作爲傳輸方式。編寫一箇中間節點，一邊通過socket接收數據，另一邊通過ros的話題和服務發佈出去。

而通過分析瞭解到　後處理功能全部在nvdsinfer_custom_impl_Yolo文件夾中，其中我們需要的數據都在nvdsparsebbox_Yolo.cpp文件中，所以我們修改這個文件就可以了。操作步驟如下：

2.1 修改程序：nvdsinfer_custom_impl_Yolo.cpp

2.1.1 添加頭文件

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <string.h>
#include <sstream>

2.1.2 函數decodeYoloV3Tensor()函數

末尾 "return binfo;“上方添加以下代碼：

std::cout<<"There are "<<binfo.size()<<" kinds of object" <<endl;
if(binfo.size()>0)
{
    string send_result = "AA";
    send_result = send_result + int_to_string(binfo.size());
    for(int i=0;i<binfo.size();i++)
    {
        std::cout <<"class::"<<binfo[i].classId <<" location:"<<" x:"<<binfo[i].left<<" y:"<<binfo[i].top<<" w:"<<binfo[i].width<<" h:"<<binfo[i].height <<" confidence:"<<binfo[i].detectionConfidence<<std::endl;
        send_result = send_result +"BB"+int_to_string(binfo[i].classId)+int_to_string(binfo[i].left)+int_to_string(binfo[i].top)+int_to_string(binfo[i].width)+int_to_string(binfo[i].height)+"CC";
    }
    send_result = send_result + "DD";
    socket_write(send_result);
}

2.1.3 位置3：添加自定義函數int_to_string()

在decodeYoloV3Tensor()函數上方空白處添加自定義函數：int_to_string(int)

string int_to_string(int a)
{
    stringstream ss;
    string str;
    ss << a;
    ss >> str;
    if(str.size()<4)
    {
        string s3(4-str.size(),'0');
        str = s3+str;
    }
    return str;
}

2.1.4 位置4：添加自定義函數socket_write()

在decodeYoloV3Tensor()函數上方空白處添加自定義函數：socket_write(std::string s)

bool socket_write(std::string s)
{
    int socket_fd = socket(AF_INET,SOCK_STREAM,0);
    if(socket_fd == -1)
    {
        cout<<"socket create failed"<<endl;
        exit(-1);
    }
    struct sockaddr_in_addr;
    addr.sin_family = AF_INET;
    addr.sin_port = htons(8888);
    addr.sin_addr.s_addr = inet_addr("127.0.0.1");
    int res = connect(socket_fd, (struct sockaddr *)&addr, sizeof(addr));
    if (res == -1)
    {
        cout << "bind 鏈接失敗：" << endl;
        exit(-1);
    }
    const char *p = s.c_str();  
    write(socket_fd,p, s.size());
    close(socket_fd);
}

2.2 創建ROS節點：detection_server

本節點作爲一個過渡節點，一方面作爲上述TCP通信的服務端，實時接收客戶端發送過來的流數據，然後讀取並解析;另一方面作爲話題發佈器，將解析到的位置信息和類型信息通過話題的形式發佈出去。程序如下：

/*used for deepstream*/
#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <iostream>
#include <string.h>
#include <waste_clasify/UarmAction.h>
#include <waste_clasify/Uarm.h>
#include <iostream>
/*注意使用eigen的時候路徑在/usr/include/eigen3下
 也可執行如下指令：
 cp -rf /usr/include/eigen3/Eigen /usr/include/Eigen -R
*/
#include <eigen3/Eigen/Dense>

using namespace std;
using namespace Eigen;

#define BUF_SIZE 255

struct Pose_3D
{
 float x;
 float y;
 float z;
} pose_3d;

Pose_3D position_to_3D(int U, int V)
{
 // //單目相機標定結果，f及光心座標c
 Matrix<float, 3, 3> Nei_Can;
 Nei_Can << 386.157194, 0.000000, 159.067975, 0.000000, 388.063524, 129.470492, 0, 0, 1;
 // //手眼標定結果，旋轉及平移矩陣
 Matrix<float, 4, 4> Wai_Can;
 Wai_Can << -47.2253736, 808.943874, -934.623712, 486.125510, 896.251615, 28.4136536, -178.435138, 36.1651555, -3.05148887, 5.81405247, -1133.56463, 279.001094, 0, 0, 0, 1;

 float Z = 0.1625;

 //像素座標系座標
 Matrix<float, 3, 1> Pos_XS;
 Pos_XS << 0,0,0;  //初始化爲0
 Pos_XS << Z*U,Z*V,Z*1;

 //計算相機座標系的三維座標：
 Matrix<float, 3, 1> Pos_TX3D;//在圖像座標系下座標
 Pos_TX3D << 0,0,0;//初始化爲0
 Pos_TX3D = Nei_Can.inverse() * Pos_XS;


 //計算在機械臂基座標系座標：
 Matrix<float, 4, 1> Pos_TX3D_QC;//在圖像座標系下座標，本行是爲了轉爲齊次形式，便於矩陣相乘
 Pos_TX3D_QC << 0,0,0,0;//初始化爲0
 Pos_TX3D_QC << Pos_TX3D(0,0),Pos_TX3D(1,0),0.214,1;

 Matrix<float, 4, 1> Pos_World;
 Pos_World << 0,0,0,0;//初始化爲0
 Pos_World= Wai_Can*Pos_TX3D_QC;//在機器人基座標系下座標
 Pose_3D pos_world_3d;
 pos_world_3d.x = Pos_World(0, 0);
 pos_world_3d.y = Pos_World(1, 0);
 pos_world_3d.z = 30;
 return pos_world_3d;
}

int string_to_int(string s)
{
 stringstream ss;
 ss << s;

 int i;
 ss >> i;
 return i;
}

int outof0(string str)
{
 int i = 0;
 while (str[i] == '0')
 {
     i += 1;
 }
 str.erase(0, i);
 int num = string_to_int(str);
 return num;
}

int main(int argc, char **argv)
{
 ros::init(argc, argv, "deepstream_server");
 ros::NodeHandle n;
 ros::Publisher chatter_pub = n.advertise<waste_clasify::Uarm>("/waste/detection_result", 1000);

 //1.創建一個socket socket() 函數確定了套接字的各種屬性
 int socket_fd = socket(AF_INET, SOCK_STREAM, 0);
 if (socket_fd == -1)
 {
     cout << "socket 創建失敗： " << endl;
     exit(1);
 }
 //2.準備通訊地址（必須是服務器的）192.168.1.49是本機的IP
 struct sockaddr_in addr;
 addr.sin_family = AF_INET;
 addr.sin_port = htons(8888);                       //將一個無符號短整型的主機數值轉換爲網絡字節順序，即大尾順序(big-endian)
 addr.sin_addr.s_addr = inet_addr("192.168.43.78"); //net_addr方法可以轉化字符串，主要用來將一個十進制的數轉化爲二進制的數，用途多於ipv4的IP轉化。
 //3.bind()綁定
 //參數一：0的返回值（socket_fd）
 //參數二：(struct sockaddr*)&addr 前面結構體，即地址
 //參數三: addr結構體的長度
 //通過 bind() 函數將套接字 serv_sock 與特定的 IP 地址和端口綁定，IP 地址和端口都保存在 sockaddr_in 結構體中
 int res = bind(socket_fd, (struct sockaddr *)&addr, sizeof(addr));
 if (res == -1)
 {
     cout << "bind創建失敗： " << endl;
     exit(-1);
 }
 cout << "bind ok 等待客戶端的連接" << endl;

 //4.監聽客戶端listen()函數，讓套接字處於被動監聽狀態。所謂被動監聽，是指套接字一直處於“睡眠”中，直到客戶端發起請求才會被“喚醒”
 //參數二：進程上限，一般小於30
 listen(socket_fd, 30);
 //5.等待客戶端的連接accept()，返回用於交互的socket描述符
 struct sockaddr_in client;
 socklen_t len = sizeof(client);
 while (1)
 {
     //accept() 函數用來接收客戶端的請求。程序一旦執行到 accept() 就會被阻塞（暫停運行），直到客戶端發起請求。
     int fd = accept(socket_fd, (struct sockaddr *)&client, &len);
     if (fd == -1)
     {
         cout << "accept錯誤\n"
              << endl;
         exit(-1);
     }
     //6.使用第5步返回socket描述符，進行讀寫通信。
     char *ip = inet_ntoa(client.sin_addr);
     cout << "客戶： 【" << ip << "】連接成功" << endl;

     write(fd, "welcome", 7); //必須給客戶端返回一些數據，否則客戶端會認爲沒有完成任務發送，客戶端會阻塞。

     char buffer[BUF_SIZE] = {};
     int size = read(fd, buffer, sizeof(buffer)); //通過fd與客戶端聯繫在一起,返回接收到的字節數
     //第一個參數：accept 返回的文件描述符
     //第二個參數：存放讀取的內容
     //第三個參數：內容的大小

     cout << "接收到字節數爲： " << size << endl;
     cout << "內容: " << buffer << endl;
     string result(buffer);
     waste_clasify::Uarm result_pub;

     int number_detection = outof0(result.substr(2, 4));
     vector<int> possible_indexes;
     vector<float> possible_locations;
     if (number_detection)
     {
         result_pub.data = number_detection;
         string flag = "BB";
         int position = 0;
         int i = 1;
         while ((position = result.find(flag, position)) != string::npos)
         {
             possible_indexes.push_back(position);
             position++;
         }
         cout << possible_indexes.size() << endl;
         for (int i = 0; i < possible_indexes.size(); i++)
         {
             possible_locations.push_back((float)(outof0(result.substr(possible_indexes[i] + 2, 4))));
             //根據返回的矩形框計算質心位置
             int X0 = (int)(outof0(result.substr(possible_indexes[i] + 6, 4)) + outof0(result.substr(possible_indexes[i] + 14, 4)) / 2);
             int Y0 = (int)(outof0(result.substr(possible_indexes[i] + 10, 4)) + outof0(result.substr(possible_indexes[i] + 18, 4)) / 2);
             //計算三維位置
             Pose_3D Pos_Grasp = position_to_3D(X0, Y0);
             possible_locations.push_back(Pos_Grasp.x);
             possible_locations.push_back(Pos_Grasp.y);
             possible_locations.push_back(Pos_Grasp.z);
         }
         result_pub.pose = possible_locations;
         chatter_pub.publish(result_pub);
     }
     //7.關閉sockfd
     close(fd);
     memset(buffer, 0, BUF_SIZE);
 }
 close(socket_fd);
 return 0;
}