ロボットプログラミングⅡ-2021:ROS2演習5-サービス通信しよう!(Python)

この記事は金沢工業大学 ロボティクス学科で2021年後学期開講中のロボットプログラミングⅡ用です.

今回は、ROS2のもう一つの通信方式であるサービスとそれをPythonで実現する方法を学びましょう!

コンテンツ

  • サービス通信

参考サイト

概 要

サービス(service)はROSの通信方法の一つで、双方向通信に使います。ある仕事をお願いするクライアント(client)とそれを処理して返すサーバー(server)からなります。今回、作成する簡単なプログラムは、[ROS2演習20-2021:亀で遊ぼう!]でrqtで使ったspawnサービスをサーバーにリクエスト(request、要求)するクライアントとそれに対して亀の名前をリスポンド(respoint、応答)するサーバーです。リクエストとリスポンドは.srvファイルで決まります。次の手順でやります。

  1. パッケージの作成
  2. サーバーノードの作成
  3. クライアントノードの作成

ハンズオン

1.パッケージの作成

ROS2演習3-2021でパッケージを作ったように、~/ワークスペース名/srcディレクトリに移動し、ros2 pkg createコマンドでsimple_serviceパッケージとsimple_serverノードを作成する。今回新しく出た--dependenciesのオプションは依存関係のあるモジュールをpackage.xmlexample_interfacesに追記してくれる。

  • $ cd ~/colcon_ws/src
  • $ ros2 pkg create --build-type ament_python --node-name simple_server  simple_service --dependencies rclpy  turtlesim std_msgs  

ソースコードを書く前にmy_serviceパッケージをビルドしておく。

  • $ cd ~/colcon_ws
  • $  colcon build --symlink-install

2.  srvの定義確認

この例ではturtlesimモジュールを使っており、リクエストとレスポンスのデータ型を定義しているsrvファイル(/opt/ros/foxy/share/turtlesim/srv/Spawn.srv)を次に示す。—の上4行がクライアントからのリクエスト、下1行がサーバーのレスポンスのデータ型を表す。float32は32ビットのfloat型、stringは文字型である。この他にint64(64ビット整数型)、float64(64ビット浮動小数点型)などもある。ここで、リクエストの(x, y, theta)は亀の位置と向き、nameは名前(空文字の場合は一意の名前をつけてくれる)、レスポンスの名前は亀の名前である。

float32 x
float32 y
float32 theta
string name # Optional.  A unique name will be created and returned if this is empty
---
string name

3.サーバーノードの作成

では、依頼された仕事を処理するサーバープログラムを作ります。次のプログラムをgeditなどを使いsimple_server.pyというファイル名を付けて~/colcon_ws/src/simple_service/simple_service/simple_server.pyとして保存する。

# ファイル名 simple_server.py
import                                                        # ROS2のPythonモジュールをインポート
from turtlesim.srv import Spawn  # turtlesim.srvモジュールからSpawnクラスをインポート
from rclpy.node import Node         # rclpy.nodeモジュールからNodeクラスをインポート

class SimpleService(Node):
""" サーバークラス。クライアンからリクエストされた情報を端末に表示し、名前をリスポンスするサ
"""
    def __init__(self):
        """コンストラクタ。ノード名はsimple_server。サービスの生成。
        super().__init__('simple_server') # スーパークラスNodeのコンストラクタを呼び出してノード名をつける
        self.srv = self.create_service(Spawn, 'spawn', self.spawn_callback) # (型名、サービス名、コールバック関数)
 
    def spawn_callback(self, request, response):
        """コールバック関数。リクエストデータを受け取り、nameが空文字の場合は"kame"という名前をレスポンスとして送り返す
        if request.name == '':
            response.name = 'kame'
        else:
            response.name = request.name
                     
        self.get_logger().info('Request:x=%f, y=%f, theta=%f, name=%s\nResponse:name=%s' % (request.x, request.y, request.theta, request.name, response.name))  # 端末にリクエストとレスポンスデータを表示

        return response


def main(args=None):
    rclpy.init(args=args)                                # rclpyモジュールの初期化
    simple_service = SimpleService()     # ノードの作成
    rclpy.spin(simple_service)                   # コールバック関数の呼び出し
    rclpy.shutdown()                                      # rclpyモジュールの終了処理



if __name__ == '__main__':
    main()

ソースコードにコメントを掲載していますが、必要があれば補足説明します。

4.クライアントノードの作成
次に仕事を依頼するのクライアントのソースコートを作成する。以下のソースコードをsimple_client.pyという名前で~/colcon_ws/src/simple_service/simple_service/simple_client.pyとして保存する。

# ファイル名 simple_client.py
import sys # 端末から入力引数を受け取るために必要
from turtlesim.srv import Spawn
import rclpy
from rclpy.node import Node


class SimpleClient(Node):
"""クライアントクラス。端末から入力されたリクエストデータをサーバーへ送る。
"""
    def __init__(self):
    """コンストラクタ。サーバーと同じ型名とサービス名を持つクライアントの生成。
               型名とサービス名が同じでなければサーバーと通信できない。
    """
        super().__init__('simple_client')
        self.cli = self.create_client(Spawn, 'spawn') # クライアントの生成(型名、サービス名)
        while not self.cli.wait_for_service(timeout_sec=1.0): # 1秒に1回サービスが利用できるかチェック
            self.get_logger().info('service not available, waiting again...')
        self.req = Spawn.Request() # リクエストインスタンスの生成。これにリクエストデータが格納される。

    def send_request(self):
        """端末から入力された引数が順番にリクエストインスタントのx, y, theta, nameに代入される。
        """
        self.req.x           = float(sys.argv[1])     #  亀の位置x座標
        self.req.y           = float(sys.argv[2])     #  亀の位置y座標
        self.req.theta   = float(sys.argv[3])    #  亀の向き[rad]        
        self.req.name  = str(sys.argv[4])        #  亀の名前
        self.future         = self.cli.call_async(self.req) # サービスをリクエストして、結果を非同期的に取得する


def main(args=None):
    rclpy.init(args=args)
    simple_client = SimpleClient()     #  クライアンとオブジェクトの生成  
    simple_client.send_request()       # リクエストを送る

    while rclpy.ok():
        """  Futureが実行されているときは、サーバーからのレスポンスをチェックし、レスポンスがあったらその内容を端末に表示する。
        rclpy.spin_once(simple_client) # コールバック関数を1回だけ呼び出す
        if simple_client.future.done():  #  Futureがキャンセルされるか、結果を得るたらfuture.done()がTrueになる。
            try:
                response = simple_client.future.result() # サーバーから非同期的に送られてきたレスポンス
            except Exception as e:                                         #  エラー時の処理
                simple_client.get_logger().info(
                    'Service call failed %r' % (e,))
            else:  #  エラーでないときは、端末にレスポンスである亀の名前を表示する
                simple_client.get_logger().info('Response:name=%s' % response.name)
            break

    simple_client.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

このプログラムは非同期通信のプログラムで、ROS2ではその実装にFutureというPythonのオブジェクトが広く使われている。Futureオブジェクトについては以下を参照して欲しい。

5.  package.xmlの編集

パッケージを作成するときに--dependenciesのオプションを加えて依存関係のあるモジュールを指定したのでpackage.xmlを編集しなくてもビルドできる。6行目のdescription、7行目のmaintainer、8行目のlicenseを適宜変更する。

6.  setup.pyの編集

package.xmlで変更したmaintainer(16行)、maintainer_email(17行)、description(18行)、license(19行)と同じにする。パッケージを作成するときにノードsimple_serverも作成したので、エントリーポイントは23行目のように既に設定されている。新しく作成したsimple_clientノードのエントリーポイントは作成されていないので、23行目の最後に, (カンマ)を挿入し、24行目のように設定する(追加部分は赤文字)。

  • ‘simple_server = simple_service.simple_server:main’,
  • ‘simple_client = simple_service.simple_client:main’


7. ビルド

次のコマンドでパッケージを指定してビルドする。

  • $ cd ~/colcon_ws
  • $ colcon build --symlink-install 

8.実行

(1)  turtlesimの起動

端末を開き、次のコマンドでturtlesimを起動する。下図のようにturtlesimのウインドウが表示される。

  • $ ros2 run turtlesim turtlesim_node

(2) サーバーの起動

次の端末を開き、次のコマンドでbyobuを起動して、Shift-F2キーを押して窓を上下に分割する。その後、上の窓でサーバーを起動する。

  • $ byobu
  • $ source ~/colcon_ws/install/setup.bash
  • $ ros2 run simple_service simple_serve

(3)  クライアントの起動

サーバーを起動した端末の下の窓で、次のコマンドでクライアントを実行する。

  • $ source ~/colcon_ws/install/setup.bash
  • $ ros2 run simple_service simple_client  10  20  3.14  ''

クライアントはキーボードから亀の位置と姿勢、名前(この例は空文字)をサーバーへリクエストする。この場合は空文字を送ったので、サーバーからのリスポンスは亀の名前’kame’となる。亀の名前をつけてリクエストするとその名前がレスポンスされる。

また、TurtleSimのウインドウにクライアントを実行するたびに亀が現れる。

 

成功したら終わり。うまく動かない場合は、打ち間違えや手順に間違いがないか確認し、再度実行しましょう。お疲れ様!

ホームワーク

  1.  コマンドでもサービスを実行できます。以下のコマンドを実行して動作を確認してみよう。
    • $ ros2 service call /spawn turtlesim/srv/Spawn “{x: 2, y: 2, theta: 0.2, name: ”}”
  2.    以下のROS2チュートリアルを読んで、ROS2のサービスに関する理解を深めよう。
  3.    [ROS2演習2-2021:亀で遊ぼう!]ではspawnサービスの他に、killサービス、resetサービス、set_penサービス、clearサービスなどがありました。どれか一つを選び、そのサービスをリクエストするクライアントパッケージを作成して、動作を確認してください。

終わり

 

コメント

タイトルとURLをコピーしました