ROS勉強記第2回はNEXTAGE OPENのROSインタフェースについて取り上げます。まずは簡単にNEXTAGEのソフトウェア構成について述べます。下図を参照してもらえるとわかるように、ROSインタフェースは

  • hrpsys:ロボットの制御ソフトウェア
  • OpenHRP3:ロボットの動力学計算ソフトウェア
  • OpenRTM:ロボットソフトウェアのミドルウェア(ROSと似た機能を持つ)

という複数の基盤ソフトウェアの上に位置しています。これらのソフトウェアのインタフェースと同等の機能を持つROSトピック、サービスなどを提供するブリッジを介することで、ROSを使ったアプリケーションが実装されています。

ブリッジがどれだけあるか、試しに見てみましょう。

ターミナル1

$ roscore

ターミナル2

$ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch

ターミナル3

$ rqt_graph

動作生成を行うSequencePlayerServiceROSBridgeや、ロボットの現在状態を保持するStateHolderServiceROSBridgeなど、ロボットの制御を司るhrpsysが持つ機能のブリッジが確かに存在することが確認できます。では実際に、このROSインタフェースでNEXTAGE OPENを操作してみましょう。

ROSトピック

ROSトピックで一番わかりやすいのは、関節角をPublishしている /joint_states でしょう。

$ rostopic type /joint_states | rosmsg show
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string[] name
float64[] position
float64[] velocity
float64[] effort

ロボットの現在の関節角がpositionに保存されています。試しに第1回ROS勉強会で教えてもらったrqt_plotで描画してみましょう。動画をご参照ください。position配列のインデックスの3から8が右腕の関節角の値です。

ROSサービス

ROSサービスにどんなサービス(関数)が用意されていて、どのように使えばよいかを知るドキュメントは、残念ながら現在のところほとんどありません。あったら教えて下さい。英語ですが、ROS WikiのチュートリアルにhrpsysのPythonインタフェースの話が少し記述されているので、それがまだわかりやすいかもしれません。

僕は学生時代から同じくhrpsysで動作するHRP2, HRP4, HIRO(NEXTAGEの研究プラットフォーム版、現在はNEXTAGE OPENに代わり、ディスコンに)を触っていて前知識があるため、すんなり解釈できましたが、初学者にとってはかなりギャップがありそうです。より詳しく知りたい場合は、ソースコードを読んで直に理解していくしかありません。hrpsysはオープンソースで開発が進められているので、ソースコードは自由に読むことができます。ユーザがまず初めに読むべきは、ロボットの動作生成を行うSequencePlayerとそれをラップしたhrpsys_config.pyでしょう。

これまでの動画で利用してきたipythonで実行しているnextage.pyは、内部でhrpsys_config.pyをNEXTAGE向けに継承したnextage_client.pyを呼んでいます。

シミュレータで試しながらインタラクティブに動作確認してみましょう。

ターミナル3

$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py
>> nxc.goInitial() # 初期姿勢に移動する
>> import math
>> nxc.setTargetPose('larm', [0.2, 0, 1.2], [3.0, -math.pi/2, -3.0], tm=2.0) # 逆運動学ににより左腕を移動する (x, y, z) = (0.2, 0, 1.2), (roll, pitch, yaw) = (3.0, -π/2, 3.0)
>> nxc.setTargetPoseRelative('larm', 'LARM_JOINT5', dz=-0.1, tm=5) # 左腕のエンドエフェクタLARM_JOINT5をz方向に0.1m下げる
>> nxc.goOffPose() # サーボオフ姿勢に移動する

ロボット実機を動かす場合には、その他に checkEncoders(), servoOn(), servoOff() などを呼ばなくてはなりませんが、これは勉強記第4回で取り上げます。さて、少し使い方がわかったところで、上記の関数をROSサービスから呼んでみましょう。

ターミナル3

$ rosservice list /SequencePlayerServiceROSBridge
/SequencePlayerServiceROSBridge/addJointGroup
/SequencePlayerServiceROSBridge/clear
/SequencePlayerServiceROSBridge/clearNoWait
/SequencePlayerServiceROSBridge/clearOfGroup
/SequencePlayerServiceROSBridge/get_loggers
/SequencePlayerServiceROSBridge/isEmpty
/SequencePlayerServiceROSBridge/loadPattern
/SequencePlayerServiceROSBridge/playPattern
/SequencePlayerServiceROSBridge/playPatternOfGroup
/SequencePlayerServiceROSBridge/removeJointGroup
/SequencePlayerServiceROSBridge/setBasePos
/SequencePlayerServiceROSBridge/setBaseRpy
/SequencePlayerServiceROSBridge/setInitialState
/SequencePlayerServiceROSBridge/setInterpolationMode
/SequencePlayerServiceROSBridge/setJointAngle
/SequencePlayerServiceROSBridge/setJointAngles
/SequencePlayerServiceROSBridge/setJointAnglesOfGroup
/SequencePlayerServiceROSBridge/setJointAnglesWithMask
/SequencePlayerServiceROSBridge/setMaxIKError
/SequencePlayerServiceROSBridge/setMaxIKIteration
/SequencePlayerServiceROSBridge/setTargetPose
/SequencePlayerServiceROSBridge/setWrenches
/SequencePlayerServiceROSBridge/setZmp
/SequencePlayerServiceROSBridge/set_logger_level
/SequencePlayerServiceROSBridge/waitInterpolation
/SequencePlayerServiceROSBridge/waitInterpolationOfGroup

おっと、なんてことでしょう。残念ながら setTargetPose はあっても、他のサービスはないことがわかってしまいました。ですが、hrpsysのソースコードを探ってみると、

/SequencePlayerServiceROSBridge/setJointAngles # 関節角を設定する
/SequencePlayerServiceROSBridge/setTargetPose # 逆運動学で姿勢を設定する
/ForwardKinematicsServiceROSBridge/getCurrentPose # 現在姿勢を取得する

の3つのサービスを組み合わせると、goInitial, goOffPose, setTargetPoseRelative は実装できそうです。次回以降のROS勉強記で実際に実装してみようと思います。とりあえず今回はsetTargetPoseをrosserviceで呼んでみましょう。

ターミナル3

$ rosservice type /SequencePlayerServiceROSBridge/setTargetPose | rossrv show
string name
float64[] xyz
float64[] rpy
float64 tm
---
bool operation_return
$ rosservice call /SequencePlayerServiceROSBridge/setTargetPose 'larm' '[0.2, 0.0, 1.2]' '[-3.14, 0, 3.14]' 2.0 # nxc.setTargetPose('larm', [0.2, 0.0, 1.2], [-math.pi, 0.0, math.pi], tm=2.0) と同じ

ロボットが写真のような姿勢になったら成功です。長くなってきたので、ROS勉強記第2回はこれにてお開きにします。1回空けたROS勉強記第4回では、実機ロボットと接続して自作ROSノードでNEXTAGEを動かす予定です。