LoginSignup
14
11

More than 5 years have passed since last update.

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

Last updated at Posted at 2016-04-03

ChoregrapheとROSの間でTopicのやりとり
 PepperをROSと接続する方法については,色々と試してみたんですが,今のところ私の中でベストプラクティスになっている方法について共有しておきます.

環境は,
Ubuntu 14.04
ROS Indigo
Choregraphe 2.3

ROS側

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

launch_rosbridge_server.command
roslaunch rosbridge_server rosbridge_websocket.launch

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

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に追加する必要があります.このとき,現在のプロジェクトのディレクトリ位置を先に取得しておく必要があってややこしいのですが,結果的に以下のようになります.

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形式)をやりとりするだけです.プロトコルについては,こちらを参照してください.
 Publishは,

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

 Subscribeは,

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>
}

こんなかんじ.

 クラスにまとめると,

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)
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のプロジェクトを作成したので,参考にしてください.

補遺

 自分が若干ハマったところ.

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

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
14
11