ROS进阶——Websocket

最新推荐文章于 2024-10-13 22:18:48 发布

white_Learner

最新推荐文章于 2024-10-13 22:18:48 发布

阅读量5.3k

点赞数 2

分类专栏： ROS 文章标签： websocket 网络协议网络

本文链接：https://blog.csdn.net/Kalenee/article/details/114671201

版权

ROS 专栏收录该内容

31 篇文章 50 订阅

订阅专栏

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