rospyによるROSパッケージ開発
ROS勉強記第4回は、自分の手でROSパッケージを作成する術を習得します。NEXTAGEのROSインタフェースでは、すでにROSトピックは思いつくものすべてが元々あるように感じるため、ここではROSサービスの開発を扱います。第2回で、
残念ながら setTargetPose はあっても、他のサービスはないことがわかってしまいました。ですが、hrpsysのソースコードを探ってみると、
/SequencePlayerServiceROSBridge/setJointAngles # 関節角を設定する /SequencePlayerServiceROSBridge/setTargetPose # 逆運動学で姿勢を設定する /ForwardKinematicsServiceROSBridge/getCurrentPose # 現在姿勢を取得するの3つのサービスを組み合わせると、goInitial, goOffPose, setTargetPoseRelative は実装できそうです。次回以降のROS勉強記で実際に実装してみようと思います。
と述べていますので、今回はこの3つのサービスを例にとって実装します。ソースコードはGitHubにアップしましたので、説明は省略してまずは使ってみるという方は、catkin_makeしてrosservice callしてみてください。
https://github.com/ros-japan-users/nextage_ros_seqplay_util
僕自身、ROSプログラミングはまだまだ初心者なので、「ここはこうしたほうが良いよ！」という部分がありましたら、Pull Requestウェルカムです。
ROSパッケージの開発
まずはパッケージを作成します。今回はSequencePlayerのユーティリティパッケージということで、名前をnextage_ros_seqplay_utilとしました。
$ cd /path/to/catkin_ws/src
$ catkin_create_pkg nextage_ros_seqplay_util rospy tf nextage_ros_bridge nextage_description
catkin_create_pkgの引数で、依存パッケージを指定しています。各パッケージを簡単に説明します。
- rospy：ROSインタフェースのPython実装
- tf：ロボットの座標系変換
- nextage_description：NEXTAGE OPENのモデル記述
- nextage_ros_bridge：NEXTAGE OPENの制御ソフトウェア
ここからサービスを実装していくのですが、説明を端折るため、実装済みのソースコードをGitHubからクローンしてくることにしましょう。その上で、重要な箇所を抜き出して説明します。
$ rm -r nextage_ros_seqplay_util # catkin_create_pkgで先ほど作ったものを削除する
$ git clone https://github.com/ros-japan-users/nextage_ros_seqplay_util.git
$ cd ..
$ catkin_make
catkin_makeが成功すると、nextage_ros_seqplay_util/srvにあるサービスの定義ファイルがコンパイルされ、Python用のサービスプロキシが生成されます。
srvファイルの記述
https://github.com/ros-japan-users/nextage_ros_seqplay_util/blob/master/srv/goPose.srv
ROSサービスgoInitial, goOffPoseの入出力の定義ファイルgoPose.srvを記述します。setTargetPoseRelativeはsetTargetPoseとインタフェースを合わせるため、hrpssys_ros_bridge/OpenHRP_SequencePlayerService_setTargetPoseの定義ファイルを利用しました。
ROSサービスプロキシの設定
SequencePlayerROSBridgeとForwardKinematicsServiceROSBridgeがROSサービスとして提供しているsetTargetPose, setJointAnglesOfGroup, getCurrentPoseをPythonのメソッドself.set_target_poseのように呼び出せるようにマッピングしています。
setTargetPoseRelativeの実装
座標系変換パッケージtfの関数euler_from_matrixを使って、現在のエンドエフェクタの回転行列をXYZ, Roll, Pitch, Yawに分解して、それらに対して、setTargetPoseReiativeで渡される直交座標の差分量を足しあわせ、setTargetPoseを呼び出しています。tfパッケージのおかげで簡単に実装できました。
ROSサービスの登録
実装したサービスを/nextage_ros_seqplay_util/setTargetPoseRelative, /nextage_ros_seqplay_util/goInitial, /nextage_ros_seqplay_util/goOffPoseという名前で登録します。
シミュレータでの動作確認
hrpsys viewerを使って、実装したサービスの動作確認を行います。
ターミナル1
$ roscore
ターミナル2
$ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch
ターミナル3
$ rosrun nextage_ros_seqplay_util seqplay_util_server.py
ターミナル4
$ rosservice list /nextage_ros_seqplay_util
/nextage_ros_seqplay_util/get_loggers
/nextage_ros_seqplay_util/goInitial
/nextage_ros_seqplay_util/goOffPose
/nextage_ros_seqplay_util/setTargetPoseRelative
/nextage_ros_seqplay_util/set_logger_level
$ rosservice call /nextage_ros_seqplay_util/goInitial 2.0
$ rosservice call /SequencePlayerServiceROSBridge/waitInterpolation
$ rosservice call /nextage_ros_seqplay_util/setTargetPoseRelative "larm" "[0, 0, 0.1]" "[0, 0, 0]" 2.0
$ rosservice call /SequencePlayerServiceROSBridge/waitInterpolationOfGroup "larm"
$ rosservice call /nextage_ros_seqplay_util/goOffPose 2.0
waitInterpolationは、前の動作生成コマンドで発行した動作軌道をロボットが再生し終えるまで待つ機能を提供します。2秒で初期姿勢に移動して、2秒で左手を0.1m上に上げ、2秒でサーボオフ姿勢に移動するはずです。動きましたか？
見た目にはやっていることは勉強記第2回のnextage.pyを使った動作生成と同じですが、操作方法がhrpsysを直接呼び出したもの（実際にはOpenRTMを挟んでいますが）から、ROSインタフェースのみでの操作に変わっています。
ロボット実機での動作確認
勉強記第4回でNEXTAGE OPENのロボット実機との接続も扱う予定でしたが、時間がとれないため、第5回に延長させてください。来週頭には投稿したいと思います。
Comments powered by Disqus.