pythonでROS1のservice

やること

• ROS1でサービスを使ってみる

やったこと

チュートリアルをやってみて、追加しないといけなかったことを記載しています。(チュートリアルも途中から見るとだめで、ちゃんと前後関係把握しないとだめですね。。。)

前回の環境の続きで始めます。

~/rospython/src/ros_pubsubtestにて、 サービスのパッケージをコピーしてくる

roscp rospy_tutorials AddTwoInts.srv srv/AddTwoInts.srv

結果 ros_bubsubtest/srv/AddTwoInts.srvが生成される。

ros_pubsubtest/CMakeList.txtにおいて、

add_service_files(
   FILES
   AddTwoInts.srv 
)

をコメント解除&追記する

その後、~/rospythonにて、catkin_makeを実行する すると、以下のファイルが生成されているはず。

~/rospython/devel/lib/python2.7/dist-packages/
.
└── ros_pubsubtest
    ├── __init__.py
    ├── __init__.pyc
    ├── msg
    │   ├── __init__.py
    │   ├── __init__.pyc
    │   ├── _pubsub.py
    │   └── _pubsub.pyc
    └── srv # 新たに生成されたフォルダ
        ├── _AddTwoInts.py
        └── __init__.py

では、サービスのソースコードを書いていく。(chmod +xで実行権限を与えるのを忘れない)

#! /usr/bin/python

import sys
import rospy
from ros_pubsubtest.srv import *

def add_two_ints_client(x, y):
    rospy.wait_for_service('add_two_ints')
    try:
        add_two_ints = rospy.ServiceProxy("add_two_ints", AddTwoInts)
        resp1 = add_two_ints(x, y)
        return resp1.sum
    except rospy.ServiceExpection, e:
        print "Service call failed: %s"%e

def usage():
    return "%s [x y]"%sys.argv[0]

if __name__ == "__main__":
    if len(sys.argv) == 3:
        x = int(sys.argv[1])
        y = int(sys.argv[2])
    else:
        print usage()
        sys.exit(1)
    print "Requesting %s+%s"%(x, y)
    print "%s + %s = %s"%(x, y, add_two_ints_client(x, y))

#! /usr/bin/python

from ros_pubsubtest.srv import *
import rospy

def handle_add_two_ints(req):
    print "Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b))
    return AddTwoIntsResponse(req.a + req.b)

def add_two_ints_server():
    rospy.init_node("add_two_ints_server")
    s = rospy.Service("add_two_ints", AddTwoInts, handle_add_two_ints)
    print "Ready to add two ints."
    rospy.spin()

if __name__== "__main__":
    add_two_ints_server()

いつものように、~/rospython/catkin_makeを実行し、ターミナル1でroscoreを起動後、ターミナル2, 3それぞれで、

# ターミナル2
rosrun ros_pubsubtest add_two_ints_client.py 10 8
# ターミナル3
rosrun ros_pubsubtest add_two_ints_server.py

実行する。

結果は、

# ターミナル2
Requesting 10+8
# server起動までは待ちが入る
10 + 8 = 18

# ターミナル3
Returning [10 + 8 = 18]

となる。

無事にサービスを作ることができた。

参考になる本

ROS2ではじめよう 次世代ロボットプログラミング【電子書籍】[ 近藤豊 ]

プログラミングROS Pythonによるロボットアプリケーション開発 [ Morgan Quigley ]