ROSBridge - ROS系统与非ROS外部系统的通信的C++客户端实现
Posted scruffybear
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了ROSBridge - ROS系统与非ROS外部系统的通信的C++客户端实现相关的知识,希望对你有一定的参考价值。
文章目录
小结
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
实现及测试
启动服务器端测试环境
#启动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
以上是关于ROSBridge - ROS系统与非ROS外部系统的通信的C++客户端实现的主要内容,如果未能解决你的问题,请参考以下文章