ROSBridge - ROS系统与非ROS外部系统的通信的C++客户端实现
文章目录
- 小结
- 回顾
-
- 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