ROSBridge - ROS系统与非ROS外部系统的通信的C++客户端实现

Posted 2022-08-12 scruffybear

文章目录

• 小结
• 回顾
• • rosserial
  • ROSBridge
  • ROSBridge Client的C++实现
  • ROSBridge Client的其它实现
• 实现及测试
• • 启动服务器端测试环境
  • 初始化ROSBridge客户端
  • ROSBridge测试/listener的topic
  • ROSBridge测试/cmd_vel的topic
  • ROSBridge测试/add_two_ints服务
  • ROSBridge测试/set_bool服务
• 参考

小结

ROSBridge可以实现ROS系统与非ROS外部系统的通信，文档比较多比较杂，在Windows客户端的实现大多是Java, Python, JS等，这里对C++客户端使用Websockets通信进行了测试，C++ ROSBridge的客户端有轻量级，解耦合的优点。

回顾

rosserial

参考使用rosserial实现ROS与Windows的service服务通信，使用rosserial也是可以实现ROS系统与非ROS外部系统的通信。

ROSBridge

有关ROSBridge的官方文档可以参见: ROS Wiki - rosbridge_suite

ROSBridge Client的C++实现

用C++实现ROSBridge的客户端主要有两个Github,
1，Github: Sanic/rosbridge2cpp 这个实现不支持Websockets，所以没有进行测试。
2，本文主要参考了Github-C++ Rosbridge client uses Websockets and Rapidjson: ppianpak/rosbridgecpp
Rapidjson主要参考Rapidjson Tutorial。

另外也借鉴了Rosbridge的JS客户端的实现：RobotWebTools/roslibjs/examples/simple.html。

具体使用的ROSBridge的数据协议详情可以参考：Github: RobotWebTools/rosbridge_suite: rosbridge v2.0 Protocol Specification

ROSBridge Client的其它实现

Github: C# rosbridge-csharp-connection
ROSBridgeClient in Java
Github: ROSBridgeTestclient in Python

实现及测试

启动服务器端测试环境

参考ROS的roslibjs基本功能使用测试:

#启动roscore
roscore
#在/listener这个topic中发布一个"Hello, World"消息
rostopic pub /listener std_msgs/String "Hello, World"
#监听/cmd_vel的消息
rostopic echo /cmd_vel
#启动add_two_ints_server服务
rosrun rospy_tutorials add_two_ints_server
#启动rosbridge_server 
roslaunch rosbridge_server rosbridge_websocket.launch

初始化ROSBridge客户端

RosbridgeWsClient rbc("192.168.238.129:9090");

这里192.168.238.129是服务器地址，端口9090。

ROSBridge测试/listener的topic

发布/listener的topic并订阅/listener的topic:

  rbc.addClient("topic_advertiser");
  rbc.advertise("topic_advertiser", "/listener", "std_msgs/String");
  rbc.addClient("topic_subscriber");
  rbc.subscribe("topic_subscriber", "/listener", subscriberCallback);

subscriberCallback()如下：

void subscriberCallback(std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message)

  std::cout << "subscriberCallback(): Message Received: " << in_message->string() << std::endl;

先前服务器已经发布了rostopic pub /listener std_msgs/String "Hello, World"，客户端会有以下输出：

topic_subscriber: Sending message: "op":"subscribe", "topic":"/listener"
subscriberCallback(): Message Received: "op": "publish", "topic": "/listener", "msg": "data": "Hello, World"

也可以通过ROSBridge C++客户端的代码进行发布：

void publisherThread(RosbridgeWsClient& rbc, const std::future<void>& futureObj)

  rbc.addClient("topic_publisher");

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

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

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

也就是客户端自己发布，自己接收，ROSBridge C++客户端输出如下：

publish_client: Opened connection
publish_client: Sending message: "op":"publish", "topic":"/listener", "msg":"data":"Test message from John /listener"

有关发送消息的结构是string data如下：

john@ubuntu:~$ rostopic list
/client_count
/cmd_vel
/connected_clients
/listener
/rosout
/rosout_agg
john@ubuntu:~$ rostopic info /listener
Type: std_msgs/String

Publishers: 
 * /rostopic_5673_1660196707336 (http://ubuntu:35063/)

Subscribers: None

john@ubuntu:~$ rosmsg show std_msgs/String
string data

john@ubuntu:~$ 

ROSBridge测试/cmd_vel的topic

/cmd_vel这个消息结构比较常用但是有些复杂，需要使用好Rapidjson。
消息结构如下：

john@ubuntu:~$ rostopic info /cmd_vel
Type: geometry_msgs/Twist

Publishers: 
 * /rosbridge_websocket (http://ubuntu:38019/)

Subscribers: 
 * /rostopic_5710_1660196719502 (http://ubuntu:37659/)
john@ubuntu:~$ 
john@ubuntu:~$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

ROSBridge C++客户端实现代码如下：

void publisherTwist(RosbridgeWsClient& rbc)

    rbc.addClient("topic_publisher_Twist");
    
    rapidjson::Document d;
    d.SetObject();
    
    float x = 0.1;
    float y = 0.2;
    float z = 0.3;

    rapidjson::Value data(rapidjson::kObjectType);
    data.AddMember("x", x, d.GetAllocator());
    data.AddMember("y", y, d.GetAllocator());
    data.AddMember("z", z, d.GetAllocator());
    d.AddMember("linear", data, d.GetAllocator());
    
    x = -0.1;
    y = -0.2;
    z = -0.3;
    rapidjson::Value dataAngular(rapidjson::kObjectType);
    dataAngular.AddMember("x", x, d.GetAllocator());
    dataAngular.AddMember("y", y, d.GetAllocator());
    dataAngular.AddMember("z", z, d.GetAllocator());

    d.AddMember("angular", dataAngular, d.GetAllocator());
    rbc.publish("/cmd_vel", d);

    std::cout << "topic_publisher_Twist stops()" << std::endl;

ROSBridge C++客户端输出如下：

publish_client: Opened connection
publish_client: Sending message: "op":"publish", "topic":"/cmd_vel", "msg":"linear":"x":0.10000000149011612,"y":0.20000000298023225,"z":0.30000001192092898,"angular":"x":-0.10000000149011612,"y":-0.20000000298023225,"z":-0.30000001192092898

发布完数据后，ROSBridge服务器端接收：

john@ubuntu:~$ rostopic echo /cmd_vel
WARNING: topic [/cmd_vel] does not appear to be published yet
linear: 
  x: 0.10000000149011612
  y: 0.20000000298023224
  z: 0.30000001192092896
angular: 
  x: -0.10000000149011612
  y: -0.20000000298023224
  z: -0.30000001192092896
---

ROSBridge测试/add_two_ints服务

先前在服务器端启动了add_two_ints服务
john@ubuntu:~$ rosrun rospy_tutorials add_two_ints_server

传输数据格式如下：

john@ubuntu:~$ rosservice info /add_two_ints
Node: /add_two_ints_server
URI: rosrpc://ubuntu:33809
Type: rospy_tutorials/AddTwoInts
Args: a b
john@ubuntu:~$ 

john@ubuntu:~$ rossrv show rospy_tutorials/AddTwoInts
int64 a
int64 b
---
int64 sum

john@ubuntu:~$ 

ROSBridge C++客户端实现代码如下：

  rapidjson::Value dataTwoInts(rapidjson::kObjectType);
  rapidjson::Document documentAddTwoInt(rapidjson::kObjectType);
  int a = 5;
  int b = 6;
  documentAddTwoInt.AddMember("a", a, documentAddTwoInt.GetAllocator());
  documentAddTwoInt.AddMember("b", b, documentAddTwoInt.GetAllocator());


  rbc.callService("/add_two_ints", callServiceCallbackAddtwo, documentAddTwoInt);

callServiceCallbackAddtwo函数如下：

void callServiceCallbackAddtwo(std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message)

    std::string messagebuf = in_message->string();
    std::cout << "callServiceCallbackAddtwo(): 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;
    

    std::cout << "The returned sum is: " << std::to_string(document["values"]["sum"].GetInt64()) << std::endl;

    connection->send_close(1000);

ROSBridge C++客户端输出如下：

call_service_client: Opened connection
call_service_client: Sending message: "op":"call_service", "service":"/add_two_ints", "args":"a":5,"b":6
callServiceCallbackAddtwo(): Message Received: "op": "service_response", "service": "/add_two_ints", "values": "sum": 11, "result": true
The returned sum is: 11

ROSBridge服务器端输出如下：

john@ubuntu:~$ rosrun rospy_tutorials add_two_ints_server
Returning [5 + 6 = 11]

ROSBridge测试/set_bool服务

SetBool服务的数据结构如下，输入为bool data，输出为bool success string message

john@ubuntu:~$ rosservice info /set_bool
Node: /rosbridge_websocket
URI: rosrpc://ubuntu:39413
Type: std_srvs/SetBool
Args: data
john@ubuntu:~$ rossrv show std_srvs/SetBool
bool data
---
bool success
string message

ROSBridge C++客户端发布这个服务：

  rbc.addClient("service_advertiser");
  rbc.advertiseService("service_advertiser", "/set_bool", "std_srvs/SetBool", advertiseServiceCallback);

advertiseServiceCallback函数如下, 有返回就回调。可以看到ROSBridge C++客户端使用document["args"]["data"].GetBool()来查看ROSBridge服务器返回的SetBool的值。再返回给ROSBridge服务器bool success string message的内容。

void advertiseServiceCallback(std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> 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;
  

  bool result = document["args"]["data"].GetBool();

  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);

ROSBridge C++客户端调用SetBool服务：

  rapidjson::Document document(rapidjson::kObjectType);
  document.AddMember("data", true, document.GetAllocator());
  rbc.callService("/set_bool", callServiceCallback, document);

callServiceCallback的回调函数如下：

void callServiceCallback(std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message)

  std::cout << "serviceResponseCallback(): Message Received: " << in_message->string() << std::endl;
  connection->send_close(1000);

ROSBridge C++客户端自发自收的效果如下：
ROSBridge C++客户端调用/set_bool服务：

service_advertiser: Opened connection
service_advertiser: Sending message: "op":"advertise_service", "service":"/set_bool", "type":"std_srvs/SetBool"
call_service_client: Opened connection
call_service_client: Sending message: "op":"call_service", "service":"/set_bool", "args":"data":true

ROSBridge C++客户端接收到了/set_bool服务调用：

advertiseServiceCallback(): Message Received: "op": "call_service", "id": "service_request:/set_bool:1", "service": "/set_bool", "args": "data": true

ROSBridge C++客户端响应/set_bool调用后，进行应答rbc.serviceResponse(document["service"].GetString(), document["id"].GetString(), true, values);
结果如下：

service_response_client: Opened connection
service_response_client: Sending message: "op":"service_response", "service":"/set_bool", "result":true, "id":"service_request:/set_bool:1", "values":"success":true,"message":"from advertiseServiceCallback"

ROSBridge C++客户端接收到/set_bool调用的应答：

serviceResponseCallback(): Message Received: "op": "service_response", "service": "/set_bool", "values": "success": true, "message": "from advertiseServiceCallback", "result": true

参考

我的AI之路(25)–ROSBridge：机器人与外部系统之间的通讯解决方案
我的AI之路(27)–用C++实现机器人与机器人后端控制系统之间的双向通讯
我的AI之路(29)–使用Boost Asio和Rapidjson实现机器人与机器人后端系统之间的自定义协议的通讯
Github: RobotWebTools/rosbridge_suite
ROS Wiki - rosbridge_suite
使用rosserial实现ROS与Windows的service服务通信
Package RosBridge——打通Ros与非Ros环境的数据壁垒
Package RosBridge——打通Ros与非Ros环境的数据壁垒 #2 C++端实现功能ppianpak / rosbridgecpp
ROSBridge简介以及理解使用（上）
ROSBridge简介以及理解使用（下）
转:使用rosbridge协议实现安卓跟ros的解耦
古月居：ROSBridge简介以及理解使用
Github: RobotWebTools/rosbridge_suite: rosbridge v2.0 Protocol Specification
ROS Wiki Rosbridge in C++
Github: Sanic/rosbridge2cpp
Request for help regarding ROS-> C++ Application (windows) communication problem for video transmission.
Github: C# rosbridge-csharp-connection
ROSBridgeClient in Java
Android使用ROSBridge与ROS通信 简单使用
Github: ROSBridgeTestclient in Python
ROS Wiki How to send data in Json format using service client in c++ code
Rapidjson Tutorial
Github-C++ Rosbridge client uses Websockets and Rapidjson: ppianpak/rosbridgecpp
RobotWebTools/roslibjs/examples/simple.html