ROSを独自のプロトコルへブリッジする

### 2019/02/11 追記

`rosbridge` と検索して出てくる日本語情報のトップにいるようなので、ちょっと宣伝をば。

私が所属している aptpod という会社は、WebSocketを用いたPubSubブローカーを自社開発している会社でありまして（もうちょっと詳しい説明は[アドベントカレンダー](https://qiita.com/ryskiwt/items/551f5ad60d8c4cd14588)に書きました）、下記の記事は、本当は自社PubSubブローカーへのつなぎ込み（記事では Socket.io へのつなぎ込みになっていますが）のために調べた内容でした。

最近ではロボットのROSデータをリモートで可視化したい、というお客様もちらほらで始めているようですので、もし（ROSに限らず）大容量データの伝送・可視化でお困りのことがあれば、ぜひ [弊社](https://www.aptpod.co.jp/) までご相談ください。


はじめに
====================

ROSメッセージを他のプロトコルへブリッジングするRosbridgeというROSモジュールがあります。パッケージに同梱されているrosbridge_serverには、UDP, TCP, WebSocketをトランスポートプロトコルとして使用するサーバープログラムがあり、特にWebSocketに関しては、jsライブラリであるroslibjsを利用することにより、とても簡単にブラウザをROSに参加させることができます。

[rosbridge_suite - ROS Wiki](http://wiki.ros.org/rosbridge_suite)

今回は、UDP, TCP, WebSocketといったパッケージ同梱のプロトコル以外のプロトコル（今回はSocket.io）へrosbridgeをブリッジングするため、Rosbridgeプロトコル（rosbridge_library）を直接利用する方法を考えてみます。Rosbridgeプロトコルを使用することで、pubsidh/subscribe/service...などオペレーション種別にいちいち自前実装しなくて良くなったり、いちいち各メッセージのモジュールをimportしなくて良くなるので、とても簡単にブリッジができます。

ROSに関する日本語資料がまだまだ少ない状況なので、誰かの助けになれば幸いです。


想定
====================

- 別のSocket.ioサーバーが立っているものとする
- 上記サーバーに接続し、サーバーから送られてくるメッセージをROSへブリッジする
- 受信したROSメッセージを上記サーバーにブリッジする

実装
====================

概要
--------------------

- RosbridgeProtocolをインスタンス化する
- 外部プロトコルの受信時コールバック関数内で`RosbridgeProtocol.incoming`を実行する
- `RosbridgeProtocol.outgoing`内で外部プロトコルの送信関数を実行する


ソースコード
--------------------

```py:mybridge.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-

import sys
import json
import rospy
import socketIO_client
from rosbridge_library.rosbridge_protocol import RosbridgeProtocol

# ############################
# arguments
# ############################
host = sys.argv[1]
port = int(sys.argv[2])


# ############################
# init RosBridgeProtocol
# ############################
rospy.init_node('mybridge')
client_id_seed = 0;
protocol = RosbridgeProtocol(client_id_seed)


# ############################
# init Socket.io
# ############################
socketIO = socketIO_client.SocketIO(host, port)

def on_connect(*args):
    rospy.loginfo('socket.io connected.')

def on_disconnect(*args):
    rospy.loginfo('socket.io disconnected.')

socketIO.on('connect',    on_connect)
socketIO.on('disconnect', on_disconnect)


# ############################
# on websocket message
# ############################
def on_message(*args):
    message = json.loads(args[0])
    protocol.incoming(message)

socketIO.on('ws-receive', on_message)


# ############################
# on ros message
# ############################
def outgoing_func(message):
    msg = json.loads(message)
    socketIO.emit('ws-send', json.dumps(msg))

protocol.outgoing = outgoing_func


# ############################
# start Socket.io
# ############################
socketIO.wait()
```

実行
--------------------

```sh
$ python mybridge.py 192.168.0.1 80
```


コード解説
--------------------

### RosbridgeProtocolの初期化

```py
rospy.init_node('mybridge')
client_id_seed = 0;
protocol = RosbridgeProtocol(client_id_seed)
```

- 特に呼び出しませんが、`rospy.init_node`は実行していないとエラーになります。
- `client_id_seed`はよく分かっていませんが、`0`で良さそうです（同梱のサーバープログラムでも`0`を使用している）


### SocketIO-Clientの初期化

```py
socketIO = socketIO_client.SocketIO(host, port)

def on_connect(*args):
    rospy.loginfo('socket.io connected.')

def on_disconnect(*args):
    rospy.loginfo('socket.io disconnected.')

socketIO.on('connect', on_connect)
socketIO.on('disconnect', on_disconnect)
```

SocketIO-Clientを初期化しています。特に特別なことはしていません。

### SocketIO-Clientの受信時コールバック内で`incoming`を実行

```py
def on_message(*args):
    message = json.loads(args[0])
    protocol.incoming(message)

socketIO.on('ws-receive', on_message)
```

- SocketIO-Clientで`'ws-receive'`イベントを受信した際のコールバック`on_message`内にて、`incoming`を呼び出します。
- `incoming`は、[ROSBRIDGE_PROTOCOL.md](https://github.com/RobotWebTools/rosbridge_suite/blob/groovy-devel/ROSBRIDGE_PROTOCOL.md)に従う`message`を渡せば、よしなにROSメッセージへ変換して送出してくれます。

### `outgoing`内でSocketIO-Clientの送信関数を実行

```py
def outgoing_func(message):
    msg = json.loads(message)
    socketIO.emit('ws-send', json.dumps(obj))

protocol.outgoing = outgoing_func
```

- RosbridgeProtocolの`outgoing`をオーバーライドします。
- `outgoing`は、ROSメッセージのを受信するたびに呼び出される関数です。
- `message`は[ROSBRIDGE_PROTOCOL.md](https://github.com/RobotWebTools/rosbridge_suite/blob/groovy-devel/ROSBRIDGE_PROTOCOL.md)に従うJSON文字列になります。
- SocketIO-Clientから、`'ws-send'`イベントとして送信します。

```py
socketIO.wait()
```

SocketIO-Clientの受信を待ちます。RosbridgeProtocolの方は、waitingのための関数実行は特に必要ありません。


Rosbridgeプロトコル
--------------------

基本的には[ROSBRIDGE_PROTOCOL.md](https://github.com/RobotWebTools/rosbridge_suite/blob/groovy-devel/ROSBRIDGE_PROTOCOL.md)に沿ったメッセージを送信してやればOKです。下記に例を示しますが、詳細なオプションについては、リンク元を参照してください。

### publish
publishしたい場合は、下記のような`message`をサーバー側から送り込みます。下記のサンプルは、`geometry_msgs/Twist`型のトピック`/cmd_vel`の例です。

```json
{
  "op": "publish",
  "topic": "/cmd_vel",
  "msg": {
    "linear"  : { "x" : 0, "y" : 0, "z" : 0 },
    "angular" : { "x" : 0, "y" : 0, "z" : 0 },
  }, 
}
```

### subscribe
subscribeしたい場合は、下記のような`message`をサーバー側から送り込むことで、メッセージのブリッジが開始されます。下記はトピック`/odom`の例です。トピック名だけでOKです。

```json
{
  "op": "subscribe",
  "topic": "/odom",
}
```

一度subscribeしたメッセージをunsubscribeする場合は、下記のようにします。

```json
{
  "op": "unsubscribe",
  "topic": "/odom",
}
```