（一行で）PepperをROSと接続するときのベストプラクティス

![ChoregrapheとROSの間でTopicのやりとり](https://pbs.twimg.com/media/CfIBM5iUkAA1PNu.jpg)
　PepperをROSと接続する方法については，色々と試してみたんですが，今のところ私の中でベストプラクティスになっている方法について共有しておきます．

環境は，
Ubuntu 14.04
ROS Indigo
Choregraphe 2.3

#ROS側
　いきなり正解を言ってしまうと，rosbridgeのwebsocketを使います．rosbridgeはlaunchするとwebsocketでROSのTopicを勝手にやり取りしてくれるようになります． **コードに手を加える必要がない！** ので，すごい楽．websocketサーバは，デフォルトだと9090番ポートに立ってます．

```sh:launch_rosbridge_server.command
roslaunch rosbridge_server rosbridge_websocket.launch
```

　launchファイルに記述するなら，以下の一行です．

```xml:chat.launch
<launch>
  <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" />
</launch>
```

#Choregraphe側
　Websocketのクライアントを作るわけなんで，ws4pyのライブラリを使用します．Pythonでwebsocketを使うには，他にもwebsocket.pyとか取り回しの楽なライブラリがあるわけなんですが，ws4pyを選んだ理由は・・ただのGPL回避だったりします．

　ChoregrapheのPythonから追加のライブラリを呼ぶためには，Choregrapheプロジェクトに追加したライブラリのディレクトリをPYTHONPATHに追加する必要があります．このとき，現在のプロジェクトのディレクトリ位置を先に取得しておく必要があってややこしいのですが，結果的に以下のようになります．

```python:add_pythonpath.py
import os, sys
self.framemanager = ALProxy("ALFrameManager")
libdir = os.path.join(self.framemanager.getBehaviorPath(self.behaviorId), "./ws4py")
sys.path.append(libdir)
```

　あとは，rosbridgeがあけてるwebsocketのサーバーに接続してメッセージ（JSON形式）をやりとりするだけです．プロトコルについては，[こちら](https://github.com/RobotWebTools/rosbridge_suite/blob/groovy-devel/ROSBRIDGE_PROTOCOL.md)を参照してください．
　Publishは，

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

　Subscribeは，

```js:subscribe.js
{ "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>
}
```

こんなかんじ．

　クラスにまとめると，

```python:RosPublisher.py
class RosPublisher(WebSocketClient):
  def __init__(self, sock, topic, mtype, protocols=['http-only', 'chat'], extensions=None, environ=None, heartbeat_freq=None):
    WebSocketClient.__init__(self, sock, protocols, extensions, environ, heartbeat_freq)
      self.topic = topic
      self.mtype = mtype

  def opened(self):
    message = "{\"op\": \"advertise\", " \
               "\"topic\": \"" + self.topic + "\", " \
               "\"type\": \"" + self.mtype + "\"" \
              "}"
    self.send(message)

  def closed(self, code, reason=None):
    message = "{\"op\": \"unadvertise\", " \
               "\"topic\": \"" + self.topic + "\"" \
              "}"
    self.send(message)
    print "Closed down:", code, reason

  def send_message(self, json):
    message = "{\"op\": \"publish\", " \
               "\"topic\": \"" + self.topic + "\", " \
               "\"msg\": " + json + "" \
              "}"
    self.send(message)
```

```python:RosSubscriber.py
class RosSubscriber(WebSocketClient):
  def __init__(self, sock, topic, mtype, clbk, protocols=['http-only', 'chat'], extensions=None, environ=None, heartbeat_freq=None):
    WebSocketClient.__init__(self, sock, protocols, extensions, environ, heartbeat_freq)
    self.topic = topic
    self.mtype = mtype
    self.callback = clbk

  def opened(self):
    message = "{\"op\": \"subscribe\", " \
               "\"topic\": \"" + self.topic + "\", " \
               "\"type\": \"" + self.mtype + "\"" \
              "}"
    self.send(message)

  def closed(self, code, reason=None):
    message = "{\"op\": \"unsubscribe\", " \
               "\"topic\": \"" + self.topic + "\"" \
              "}"
    self.send(message)
    print "Closed down:", code, reason

  def received_message(self, m):
    self.callback(m)
```

　こいつらを，ROSトピック読んだり書いたりしたいChoregrapheのボックス内で定義するのが良い．別ファイルにしようとすると，WebSocketClientのimportのタイミングが厄介．このあたりまとめた[Choregrapheのプロジェクトを作成した](https://github.com/yoneken/ros_with_choregraphe)ので，参考にしてください．

##補遺
　自分が若干ハマったところ．

- std_msgs/Boolのときのメッセージは，'{"msg": true}'か'{"msg": false}'．Trueや"true"ではない．
- std_msgs/Stringは，受け取るとstr型じゃなくてunicode型．encode('utf-8')してstr型に直す．