整个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"
#include <rw/models/Device.hpp>
...
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服务器进行端口监听。

客户端没有发送关节数据之前的仿真环境中机器人状态:

robotstudios模型库_#include


客户端发送发送关节数据之后仿真环境中机器人的状态:

robotstudios模型库_#include_02


从两个终端的输出结果和仿真平台上机器人的可视化状态可以看出,我们前面实现的给RobWorkStudio集成通讯功能是实现了。

总结:本篇博客主要实现了给RobWorkStudio集成简单的通讯功能,有了通讯功能之后,我们就可以给机器人发送数据,当然也可以发送其他的数据,比如相机拍摄的图像或点云数据给RobWorkStudio并进行显示,这样的话我们就可以对机器人的真实作业场景进行更加逼真的建模,从而可以进行运动规划、碰撞检测和轨迹生成、运动仿真等更加高级的操作。