【Package】RosBridge——打通Ros与非Ros环境的数据壁垒 #2 C++端实现功能ppianpak / rosbridgecpp

1 概览

在上一篇文章【Package】RosBridge——打通Ros与非Ros环境的数据壁垒 中，我介绍了如何通过RosBridge来进行非Ros与Ros系统之间的数据通信，并且实现了一半的通信架构，即图片的左半部分，这一篇，我会通过介绍GitHub上的开源项目ppianpak / rosbridgecpp来实现架构的右半部分，测试完整的通信结构效果。
ppianpak / rosbridgecpp实现了一个高效基于C++的与RosBridge通信的Client，使用rapidjson来进行json相关操作。该包纯包含头文件，不需要其他依赖，移植起来非常方便，需要CMake3.0+。下面来看看如何使用。
2 使用

 

ppianpak / rosbridgecpp包的文件结构如下：
其中，rosbridge_ws_client.cpp和rosbridge_ws_client.hpp实现了一个简单的demo，包括的功能有Topic订阅与发布，service的订阅与发布，下面来看看代码：

 

/*
 *  Created on: Apr 16, 2018
 *      Author: Poom Pianpak
 */

#include "rosbridge_ws_client.hpp"

#include 

RosbridgeWsClient rbc("localhost:9090");

void advertiseServiceCallback(std::shared_ptr /*connection*/, std::shared_ptr in_message)
{
  // message->string() is destructive, so we have to buffer it first
  std::string messagebuf = in_message->string();
  std::cout << "advertiseServiceCallback(): Message Received: " << messagebuf << std::endl;

  rapidjson::Document document;
  if (document.Parse(messagebuf.c_str()).HasParseError())
  {
    std::cerr << "advertiseServiceCallback(): Error in parsing service request message: " << messagebuf << std::endl;
    return;
  }

  rapidjson::Document values(rapidjson::kObjectType);
  rapidjson::Document::AllocatorType& allocator = values.GetAllocator();
  values.AddMember("success", document["args"]["data"].GetBool(), allocator);
  values.AddMember("message", "from advertiseServiceCallback", allocator);

  rbc.serviceResponse(document["service"].GetString(), document["id"].GetString(), true, values);
}

void callServiceCallback(std::shared_ptr connection, std::shared_ptr in_message)
{
  std::cout << "serviceResponseCallback(): Message Received: " << in_message->string() << std::endl;
  connection->send_close(1000);
}

void publisherThread(RosbridgeWsClient& rbc, const std::future& futureObj)
{
  rbc.addClient("topic_publisher");

  rapidjson::Document d;
  d.SetObject();
  d.AddMember("data", "Test message from /ztopic", d.GetAllocator());

  while (futureObj.wait_for(std::chrono::milliseconds(1)) == std::future_status::timeout)
  {
    rbc.publish("/ztopic", d);
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  }

  std::cout << "publisherThread stops()" << std::endl;
}

void subscriberCallback(std::shared_ptr /*connection*/, std::shared_ptr in_message)
{
  std::cout << "subscriberCallback(): Message Received: " << in_message->string() << std::endl;
}

int main() {
  rbc.addClient("service_advertiser");
  rbc.advertiseService("service_advertiser", "/zservice", "std_srvs/SetBool", advertiseServiceCallback);

  rbc.addClient("topic_advertiser");
  rbc.advertise("topic_advertiser", "/ztopic", "std_msgs/String");

  rbc.addClient("topic_subscriber");
  rbc.subscribe("topic_subscriber", "/ztopic", subscriberCallback);

  // Test calling a service
  rapidjson::Document document(rapidjson::kObjectType);
  document.AddMember("data", true, document.GetAllocator());
  rbc.callService("/zservice", callServiceCallback, document);

  // Test creating and stopping a publisher
  {
    // Create a std::promise object
    std::promise exitSignal;

    // Fetch std::future object associated with promise
    std::future futureObj = exitSignal.get_future();

    // Starting Thread & move the future object in lambda function by reference
    std::thread th(&publisherThread, std::ref(rbc), std::cref(futureObj));

    // Wait for 10 sec
    std::this_thread::sleep_for(std::chrono::seconds(10));

    std::cout << "Asking publisherThread to Stop" << std::endl;

    // Set the value in promise
    exitSignal.set_value();

    // Wait for thread to join
    th.join();
  }

  // Test removing clients
  rbc.removeClient("service_advertiser");
  rbc.removeClient("topic_advertiser");
  rbc.removeClient("topic_subscriber");

  std::cout << "Program terminated" << std::endl;
}

分布解读下主要部分：

RosbridgeWsClient rbc("localhost:9090");

初始化对象rbc，构造参数需要设置成之前RosBridge中的对应ip，如果本机测试就使用localhost即可。
然后给出了自定义的Service和Topic操作定义：

void advertiseServiceCallback(std::shared_ptr /*connection*/, std::shared_ptr in_message)
void callServiceCallback(std::shared_ptr connection, std::shared_ptr in_message)
void subscriberCallback(std::shared_ptr /*connection*/, std::shared_ptr in_message)
void publisherThread(RosbridgeWsClient& rbc, const std::future& futureObj)

在使用中需要注意的是，每个相关操作都需要新生成一个线程并给出对应的ClientName：

rbc.addClient("service_advertiser");
rbc.addClient("topic_advertiser");
rbc.addClient("topic_subscriber");

在类的内部，作者维护了一个map可以查询到名字和Client的对应关系，因此之后对client的操作就是提供注册时的clientName即可。
然后给出Client相应操作的配置：

rbc.advertise("topic_advertiser", "/ztopic", "std_msgs/String");

这个既是注册名字为“topic_advertiser”的client是发布topic用的，其中发布的topic名为/ztopic，数据类型是std_msgs/String。这样一个操作就注册好了，需要注意的是，clientName需要唯一，之后的操作如果不在这里注册也是无法使用的。
同样的，订阅话题的方法如下：

rbc.subscribe("topic_subscriber", "/ztopic", subscriberCallback);

然后，作者在代码段中给出了一个发布10秒话题并结束Client的操作示例：

// Test creating and stopping a publisher
  {
    // Create a std::promise object
    std::promise exitSignal;

    // Fetch std::future object associated with promise
    std::future futureObj = exitSignal.get_future();

    // Starting Thread & move the future object in lambda function by reference
    std::thread th(&publisherThread, std::ref(rbc), std::cref(futureObj));

    // Wait for 10 sec
    std::this_thread::sleep_for(std::chrono::seconds(10));

    std::cout << "Asking publisherThread to Stop" << std::endl;

    // Set the value in promise
    exitSignal.set_value();

    // Wait for thread to join
    th.join();
  }

如果想要循环发布，就使用

rapidjson::Document d;
d.SetObject();
d.AddMember("data", "Test message from /ztopic", d.GetAllocator());

while (true)
{
  rbc.publish("/ztopic", d，"topic_advertiser");
 usleep(1000000);
}

  

其中，话题和clientName要与之前注册的时候符合，d是rapidjson中的document对象，其中保存了包装好的json数据。
3 效果

编译后打开【Package】RosBridge——打通Ros与非Ros环境的数据壁垒 中介绍的roslaunch并运行rosbridge_ws_client即可实现开头给出的通信架构：

上图实现了topic“/ztopic” 的“自攻自受”，并且运行了10秒后停止了发送。可以看到发送的数据被订阅的线程又接收了回来。
下图是在无ros环境中发送虚拟的激光数据并在ros环境中接收的运行结果，发送和接收在局域网中不同的机器上：

可以看到这个架构运行效果还是可以的，当需要调试显示或者其他的ros工具时，可以发送至ros环境中进行快速调试。