PepperをROSと接続する方法については,色々と試してみたんですが,今のところ私の中でベストプラクティスになっている方法について共有しておきます.
環境は,
Ubuntu 14.04
ROS Indigo
Choregraphe 2.3
ROS側
いきなり正解を言ってしまうと,rosbridgeのwebsocketを使います.rosbridgeはlaunchするとwebsocketでROSのTopicを勝手にやり取りしてくれるようになります. コードに手を加える必要がない! ので,すごい楽.websocketサーバは,デフォルトだと9090番ポートに立ってます.
roslaunch rosbridge_server rosbridge_websocket.launch
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に追加する必要があります.このとき,現在のプロジェクトのディレクトリ位置を先に取得しておく必要があってややこしいのですが,結果的に以下のようになります.
import os, sys
self.framemanager = ALProxy("ALFrameManager")
libdir = os.path.join(self.framemanager.getBehaviorPath(self.behaviorId), "./ws4py")
sys.path.append(libdir)
あとは,rosbridgeがあけてるwebsocketのサーバーに接続してメッセージ(JSON形式)をやりとりするだけです.プロトコルについては,こちらを参照してください.
Publishは,
{ "op": "publish",
(optional) "id": <string>,
"topic": <string>,
"msg": <json>
}
Subscribeは,
{ "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>
}
こんなかんじ.
クラスにまとめると,
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)
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のプロジェクトを作成したので,参考にしてください.
補遺
自分が若干ハマったところ.
- std_msgs/Boolのときのメッセージは,'{"msg": true}'か'{"msg": false}'.Trueや"true"ではない.
- std_msgs/Stringは,受け取るとstr型じゃなくてunicode型.encode('utf-8')してstr型に直す.