RobWork框架編程（3）：給RobWorkStudio可視化仿真平臺集成通訊功能

整個RobWork機器人框架（包括RobWorkStudio模塊）都不提供進程間通訊的功能，導致與機器人應用相關的算法開發和調試很麻煩，畢竟對於一個功能比較完善的仿真軟件來說，每一次的編譯都是比較耗時的。而我們每次對代碼的修改都是少量的。此時，可以考慮將RobWorkStudio仿真平臺當作一個服務器，即給它集成http通訊的功能。關於機器人軟件通訊功能的，在我之前的一篇博客裏有簡單的介紹。

進程間通信的方式有很多，最簡單應用最廣泛的，估計非TCP/UDP莫屬了。關於這一塊的網絡庫也非常的多，有大而全、功能非常完善的高性能Asio/Boost.Asio/acl，也有一些比如ZeroMQ/NNG/muduo等等的消息庫，我之前在嘗試給RobWorkStudio集成通訊功能的時候，嘗試的第一個庫是ZeroMQ，結果在編譯鏈接的時候失敗，因爲RobWorkStudio有些庫是靜態鏈接，有些庫是動態鏈接，混合鏈接就會出現非常著名的undefined renference xxx問題，而有些庫靜態鏈接就是不好使，比如pthread庫，當然ZeroMQ貌似也這樣。最後，沒辦法，就使用了cpp-httplib這個只有一個頭文件的簡單socket通訊庫，後來發現這個庫確實蠻好的，畢竟體積小，重量輕，方便攜帶，像滿足本文中爲RobWorkStudio提供基本的消息通訊需求還是綽綽有餘的。

怎麼集成呢？把代碼加到哪個地方？
這是我們當前面臨的一個非常現實的問題。
通常，只有在成功加載了場景xml文件之後，即有了WorkCell工作單元和SerialDevice設備之後，我們纔會有通訊需求，因爲這時我們需要更新場景和設備的狀態。
這麼分析下來，具體的代碼添加位置就很明確了，就把通訊相關的代碼放到RobWorkStudio.cpp的openWorkCellFile()函數中，當用戶成功加載了一個場景文件之後，自動啓動服務器，對指定的端口進行監聽。

#include "httplib.h"
...
void RobWorkStudio::openWorkCellFile(const QString &filename) {
	// Always close the workcell.
    closeWorkCell();
    //rw::graphics::WorkCellScene::Ptr wcsene = _view->makeWorkCellScene();
    WorkCell::Ptr wc;
    ...
     //startup the server in another thread
    auto func = [this]() {
        httplib::Server svr;
        if (!svr.is_valid()) {
            printf("server has an error...\n");
            return -1;
        }
        svr.Post("/JointAngles", [&](const httplib::Request & req, httplib::Response & res) {
            std::cout << req.body << "\n";
            QStringList items = QString(req.body.c_str()).split(',');
            std::vector<double> joint_angles;
            for (QString &item : items) {
                joint_angles.push_back(item.toDouble());
            }
            rw::common::Ptr<Device> device = _workcell->findDevice("UR3_2015"); //find the first device
            std::cout << device->getQ(_state) << "\n";
            device->setQ(Q(joint_angles), _state);
            setState(_state); //更新可視化場景
            std::cout << device->getQ(_state) << "\n";
            // construct a reply message
            joint_angles.clear();
            joint_angles = device->getQ(_state).toStdVector();
            std::stringstream ss;
            for (auto &item : joint_angles) {
                ss << item << " ";
            }
            std::string data{"device state updated: "};
            data += ss.str();
            res.set_content(data, "text/plain");
        });
        svr.listen("localhost", 8080);
    };
    std::thread thread(func);
    thread.detach();
}

可以看出，代碼代碼量是很少的，就是在函數內部定義了一個lambda表達式，創建一個線程運行lambda表達式，執行線程分離操作。

編寫完成之後，我們就可以進行代碼編譯，編譯通過之後，啓動RobWorkStudio，加載場景文件。

下面我們來進行一個簡單的功能測試。
爲此，我們需要編寫一個客戶端程序，給服務器發送請求數據，並處理服務器的響應數據（這裏我們就使用簡單的打印輸出，以確保我們收到的數據是準確的，確實是RobWorkStudio服務器傳送回來的）。
CMakeLists.txt

cmake_minimum_required(VERSION 3.5)

project(httplib_client LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
add_executable(httplib_client main.cpp httplib.h)
target_link_libraries(httplib_client pthread)

main.cpp

#include <iostream>
#include "httplib.h"
#include <thread>

using namespace std;
using namespace httplib;

int main() {
    Client cli("localhost", 8080);
    std::string body = "0.000107858, -1.57084, -3.55879e-05, -1.57075, 8.38896e-05, -1.17461e-05";
    auto res = cli.Post("/JointAngles", body, "text/plain");
    if (res) {
        cout << res->status << endl;
        cout << res->get_header_value("Content-Type") << endl;
        cout << res->body << endl;
    }
    return 0;
}

啓動httplib_client客戶端之前，先啓動RobWorkStudio服務器進行端口監聽。
客戶端沒有發送關節數據之前的仿真環境中機器人狀態：

客戶端發送發送關節數據之後仿真環境中機器人的狀態：

從兩個終端的輸出結果和仿真平臺上機器人的可視化狀態可以看出，我們前面實現的給RobWorkStudio集成通訊功能是實現了。

總結：本篇博客主要實現了給RobWorkStudio集成簡單的通訊功能，有了通訊功能之後，我們就可以給機器人發送數據，當然也可以發送其他的數據，比如相機拍攝的圖像或點雲數據給RobWorkStudio並進行顯示，這樣的話我們就可以對機器人的真實作業場景進行更加逼真的建模，從而可以進行運動規劃、碰撞檢測和軌跡生成、運動仿真等更加高級的操作。