WebSocket是一种网络传输协议,可在单个TCP连接上进行全双工通信,位于OSI模型的应用层。

WebSocket使得客户端和服务器之间的数据交换变得更加简单,允许服务端主动向客户端推送数据。在WebSocket API中,浏览器和服务器只需要完成一次握手,两者之间就可以建立持久性的连接,并进行双向数据传输。


在这里插入图片描述

一、配置

rosbridge_suite:Rosbridge provides a JSON API to ROS functionality for non-ROS programs. There are a variety of front ends that interface with rosbridge, including a WebSocket server for web browsers to interact with. Rosbridge_suite is a meta-package containing rosbridge, various front end packages for rosbridge like a WebSocket package, and helper packages.

rosbridge实现ros通过Websocket与其他前端通讯,前端建立通讯后,通过定义的json实现topic的交互。

roslibjs:实现了ROS中的部分功能,如Topic,Service,URDF等;

ros2djs:提供了二维可视化的管理工具,可以用来在Web浏览器中显示二维地图;

ros3djs:提供了三维可视化的管理工具,可以在Web端显示三维模型。

sudo apt-get install ros-kinetic-rosbridge-suite
git clone https://github.com/RobotWebTools/roslibjs.git
git clone https://github.com/RobotWebTools/ros2djs
git clone https://github.com/RobotWebTools/ros3djs

二、Protocol

JSON
JSON(JavaScript Object Notation) 是一种轻量级的数据交换格式

JSON建构于两种结构:

“名称/值”对的集合(A collection of name/value pairs):不同的语言中,它被理解为对象(object),记录(record),结构(struct),字典(dictionary),哈希表(hash table),有键列表(keyed list),或者关联数组 (associative array)。
值的有序列表(An ordered list of values):在大部分语言中,它被理解为数组(array)。

{ "programmers": [ 
    { "firstName": "Brett", "lastName":"McLaughlin", "email": "aaaa" },
	{ "firstName": "Jason", "lastName":"Hunter", "email": "bbbb" },
	{ "firstName": "Elliotte", "lastName":"Harold", "email": "cccc" }
	],
  "authors": [
	{ "firstName": "Isaac", "lastName": "Asimov", "genre": "science fiction" },
	{ "firstName": "Tad", "lastName": "Williams", "genre": "fantasy" },
	{ "firstName": "Frank", "lastName": "Peretti", "genre": "christian fiction" }
	],
  "musicians": [
	{ "firstName": "Eric", "lastName": "Clapton", "instrument": "guitar" },
	{ "firstName": "Sergei", "lastName": "Rachmaninoff", "instrument": "piano" }
	] 
}

rosbridge
在rosbridge定义了标准的JSON协议用于通讯rosbridge v2.0 Protocol Specification。

Message compression / transformation:

fragment - a part of a fragmented message
png - a part of a PNG compressed fragmented message
Rosbridge status messages:

set_status_level - a request to set the reporting level for rosbridge status messages
status - a status message
Authentication:

auth - a set of authentication credentials to authenticate a client connection
ROS operations:

advertise – advertise that you are publishing a topic
unadvertise – stop advertising that you are publishing topic publish - a published ROS-message
subscribe - a request to subscribe to a topic
unsubscribe - a request to unsubscribe from a topic
call_service - a service call
advertise_service - advertise an external service server
unadvertise_service - unadvertise an external service server
service_request - a service request
service_response - a service response

例子
发布器

{ "op": "advertise",
  (optional) "id": <string>,
  "topic": <string>,
  "type": <string>
}

topic – the string name of the topic to advertise
type – the string type to advertise for the topic
If the topic does not already exist, and the type specified is a valid type, then the topic will be established with this type.
If the topic already exists with a different type, an error status message is sent and this message is dropped.
If the topic already exists with the same type, the sender of this message is registered as another publisher.
If the topic doesnt already exist but the type cannot be resolved, then an error status message is sent and this message is dropped.

{ "op": "unadvertise",
  (optional) "id": <string>,
  "topic": <string>
}

topic – the string name of the topic being unadvertised
If the topic does not exist, a warning status message is sent and this message is dropped

If the topic exists and there are still clients left advertising it, rosbridge will continue to advertise it until all of them have unadvertised

If the topic exists but rosbridge is not advertising it, a warning status message is sent and this message is dropped

{ "op": "publish",
  (optional) "id": <string>,
  "topic": <string>,
  "msg": <json>
}

topic - the string name of the topic to publish to
msg - the message to publish on the topic
If the topic does not exist, then an error status message is sent and this message is dropped
If the msg does not conform to the type of the topic, then an error status message is sent and this message is dropped
If the msg is a subset of the type of the topic, then a warning status message is sent and the unspecified fields are filled in with defaults

订阅器

{ "op": "subscribe",
  (optional) "id": <string>,
  "topic": <string>,
  (optional) "type": <string>,
  (optional) "throttle_rate": <int>,
  (optional) "queue_length": <int>,
  (optional) "fragment_size": <int>,
  (optional) "compression": <string>
}

type – the (expected) type of the topic to subscribe to. If left off, type will be inferred, and if the topic doesn’t exist then the command to subscribe will fail
topic – the name of the topic to subscribe to
throttle_rate – the minimum amount of time (in ms) that must elapse between messages being sent. Defaults to 0
queue_length – the size of the queue to buffer messages. Messages are buffered as a result of the throttle_rate. Defaults to 0 (no queueing).
id – if specified, then this specific subscription can be unsubscribed by referencing the ID.
fragment_size – the maximum size that a message can take before it is to be fragmented.
compression – an optional string to specify the compression scheme to be used on messages. Valid values are “none”, “png”, “cbor”, and “cbor-raw”.

三、应用

启动websocket

roslaunch rosbridge_server rosbridge_websocket.launch

注意:发布消息时rosbridge_server会进行类型推断,因此对应topic必须已在master中注册(启动订阅器节点或者创建advertise),否则消息无法发布或者报错无法推断类型。

web

创建html并直接通过浏览器打开即可

<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />

<script type="text/javascript" src="http://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
<script type="text/javascript" src="http://static.robotwebtools.org/roslibjs/current/roslib.min.js"></script>

<script type="text/javascript" type="text/javascript">
  // Connecting to ROS
  var ros = new ROSLIB.Ros({
    url : 'ws://localhost:9090'
  });

  //判断是否连接成功并输出相应的提示消息到web控制台
  ros.on('connection', function() {
    console.log('Connected to websocket server.');
  });

  ros.on('error', function(error) {
    console.log('Error connecting to websocket server: ', error);
  });

  ros.on('close', function() {
    console.log('Connection to websocket server closed.');
  });

  // Publishing a Topic
  var cmdVel = new ROSLIB.Topic({
    ros : ros,
    name : '/cmd_vel',
    messageType : 'geometry_msgs/Twist'
  });//创建一个topic,它的名字是'/cmd_vel',,消息类型是'geometry_msgs/Twist'

  var twist = new ROSLIB.Message({
    linear : {
      x : 0.1,
      y : 0.2,
      z : 0.3
    },
    angular : {
      x : -0.1,
      y : -0.2,
      z : -0.3
    }
  });//创建一个message

  function func()//在点击”Publish”按钮后发布消息,并对消息进行更改
  {
    cmdVel.publish(twist);//发布twist消息
    twist.linear.x = twist.linear.x + 0.1;
    twist.linear.y = twist.linear.y + 0.1;
    twist.linear.z = twist.linear.z + 0.1;
    twist.angular.x = twist.angular.x + 0.1;
    twist.angular.y = twist.angular.y + 0.1;
    twist.angular.z = twist.angular.z + 0.1;
  }

  // Subscribing to a Topic
  var listener = new ROSLIB.Topic({
    ros : ros,
    name : '/chatter',
    messageType : 'std_msgs/String'
  });//创建一个topic,它的名字是'/chatter',,消息类型是'std_msgs/String'

  function subscribe()//在点击”Subscribe”按钮后订阅'/chatter'的消息,并将其显示到网页中
  {
     listener.subscribe(function(message) {
       document.getElementById("output").innerHTML = ('Received message on ' + listener.name + ': ' + message.data);
     });
  }

  function unsubscribe()//在点击”Unsubscribe”按钮后取消订阅'/chatter'的消息
  {
     listener.unsubscribe();
  }

</script>
</head>

<body>
  <h1>Simple roslib Example</h1>
  <p>Check your Web Console for output.</p>
  <p id = "output"></p>
  <button onclick = "func()">Publish</button>
  <button onclick = "subscribe()">Subscribe</button>
  <button onclick = "unsubscribe()">Unsubscribe</button><br />
</body>
</html>

python

# python3 
import asyncio
import websockets
import json
from uuid import uuid4

async def main_loop():
    async with websockets.connect('ws://localhost:9090') as websocket:
        # 若无发布器预先初始化以实现类型推断
        adv = {'op':'advertise',
               'topic':'/chatter',
               'type':'std_msgs/String'}
        await websocket.send(json.dumps(adv))

        # 发送消息
        data = {'data':'hello'}
        msg = {'op':'publish',
               'topic':'/chatter',
               'msg':data}
        await websocket.send(json.dumps(msg))
        
        # sub msg
        new_uuid = str(uuid4())
        sub_pro = {'op':'subscribe',
                   'id':new_uuid,
                   'topic':'/chatter',
                   'type':'std_msgs/String',
                   'queue_length':5}
        await websocket.send(json.dumps(sub_pro))

        while True:
            try:
                json_message = await websocket.recv()
                recv_message = json.loads(json_message)
                print(recv_message['msg']['data'])
            except:
                print("connected loss")
                return
        
asyncio.get_event_loop().run_until_complete(main_loop())

qt

qt通过Qt WebSockets与rosbridge_websocket通讯,实现信息交互,参考simple_ros_client_application

#ifndef WEBSOCKET_NODE_H
#define WEBSOCKET_NODE_H

#include <iostream>
#include <string>

#include <QJsonDocument>
#include <QJsonObject>
#include <QThread>
#include <QWebSocket>
#include <QUuid>

class WebsocketNode : public QThread {
    Q_OBJECT
public:
    WebsocketNode();

    virtual ~WebsocketNode();

    /**
     * @brief geometry_msgs/twist
     */
    struct twist
    {
        std::vector<double> linear{0.0,0.0,0.0};
        std::vector<double> angular{0.0,0.0,0.0};
    };

    void run();

    /**
   * @brief websocket连接
   *
   * @param IP
   * @param port
   * @return true
   * @return false
   */
    bool socket_connect(QString IP, QString port);

    /**
   * @brief 断开连接
   *
   * @return true
   * @return false
   */
    bool socket_disconnect();

    /**
   * @brief 发送运动控制命令
   *
   * @param topic_name
   * @param linear
   * @param angular
   * @return true
   * @return false
   */
    bool send_geometry_msgs_twist(QString topic_name, twist msg);

    /**
     * @brief 发送订阅topic
     * @return
     */
    bool send_sub();

signals:

    void state_update(double x, double y);

private slots:
    void onConnected();

    void onDisconnected();

    void onTextMessageReceived(const QString &message);

private:
    // websocket
    QWebSocket *socket_;

    bool is_connect_;

    // sub
    QString pose_sub_id;
};

#endif // WEBSOCKET_NODE_H
#include "websocket_node.h"

WebsocketNode::WebsocketNode() {

    // init websocket
    socket_ = new QWebSocket();
    is_connect_ = false;

    // connect to WebSocket
    connect(socket_, &QWebSocket::connected,this, &WebsocketNode::onConnected);
    connect(socket_, &QWebSocket::disconnected,this, &WebsocketNode::onDisconnected);
    connect(socket_, &QWebSocket::textMessageReceived,this, &WebsocketNode::onTextMessageReceived);
}

WebsocketNode::~WebsocketNode() {
}

void WebsocketNode::run() {
}

bool WebsocketNode::socket_connect(QString IP, QString port) {
    if(is_connect_)
    {
        std::cout << "websocket is connect" << std::endl;
        return false;
    }

    QString URL = "ws://" + IP + ":" + port;

    std::cout << URL.toStdString() << std::endl;
    socket_->open(QUrl(URL));

    if (socket_->state() == QAbstractSocket::ConnectedState ||
            socket_->state() == QAbstractSocket::ConnectingState) {
        std::cout << "connect success" << std::endl;
        is_connect_ = true;
        return true;
    } else {
        is_connect_ = false;
        std::cout << "connect fail" << std::endl;
        return false;
    }
}

bool WebsocketNode::socket_disconnect() {
    if(!is_connect_)
    {
        std::cout << "websocket not connect" << std::endl;
        return false;
    }

    socket_->close();
    is_connect_ = false;
    return true;
}

bool WebsocketNode::send_geometry_msgs_twist(QString topic_name,twist msg){
    if (!is_connect_) {
        std::cout << "websocket not connect" << std::endl;
        return false;
    }

    // convert to json
    QJsonObject json;
    QJsonObject msg_json;
    QJsonObject linear_json;
    QJsonObject angular_json;

    json["op"] = "publish";
    json["topic"] = topic_name;
    linear_json["x"] = msg.linear[0];
    linear_json["y"] = msg.linear[1];
    linear_json["z"] = msg.linear[2];
    angular_json["x"] = msg.angular[0];
    angular_json["y"] = msg.angular[1];
    angular_json["z"] = msg.angular[2];

    msg_json["linear"] = linear_json;
    msg_json["angular"] = angular_json;
    json["msg"] = msg_json;

    QJsonDocument doc(json);
    QByteArray docByteArray = doc.toJson(QJsonDocument::Compact);
    QString strJson = QLatin1String(docByteArray);

    // send
    socket_->sendTextMessage(strJson);
    return true;
}

bool WebsocketNode::send_sub() {
    if (!is_connect_)
        return false;

    // sub /turtle1/pose
    QJsonObject sub_json;

    pose_sub_id = QUuid::createUuid().toString();
    sub_json["op"] = "subscribe";
    sub_json["id"] = pose_sub_id;
    sub_json["topic"] = "/turtle1/pose";
    sub_json["type"] = "turtlesim/Pose";
    sub_json["queue_length"] = 5;

    QJsonDocument doc(sub_json);
    QByteArray docByteArray = doc.toJson(QJsonDocument::Compact);
    QString strJson = QLatin1String(docByteArray);

    // send
    socket_->sendTextMessage(strJson);
    return true;
}

void WebsocketNode::onConnected()
{
    // start receive
    send_sub();
}

void WebsocketNode::onDisconnected()
{
    std::cout<<"disconnect success"<<std::endl;
}

void WebsocketNode::onTextMessageReceived(const QString &message)
{
    QJsonDocument doc = QJsonDocument::fromJson(message.toUtf8());
    QJsonObject obj = doc.object();
    QJsonObject msg_obj = obj["msg"].toObject();
    emit state_update(msg_obj["x"].toDouble(),msg_obj["y"].toDouble());
}

四、远程通讯

确保移动的Web端设备与ROS的master处在同一个局域网中,一般来说就是ROS master所在的电脑与Web的移动端连接同一个WIFI。
对websocket进行初始化时,将localhost改为所在局域网中的IP地址,用点分十进制来表示。
在主机运行bridge_server,再在移动的Web端启动websocket client,就能够实现移动的Web端与ROS的交互。

参考

利用Websocket实现ROS与Web的交互

我的AI之路(26)–使用ROSBridge WebSocket和roslibjs构建一个简单的控制机器人的web demo

JSON 数据格式

rosbridge源码阅读与问题解决