Clients within an Action: rclcppの場合

まず、rclcppの場合について説明します。ソースコードは、このレポジトリに置きました。

• https://github.com/TechMagicKK/client_within_action_cpp

以下では、チュートリアルの書き方からスタートして、徐々に変更を加えていくスタイルで解説します。変更するごとに、client_within_action_1, client_within_action_2,…と名付けています。それぞれのノードを試してみるには、

としたのち、

として、対象のノードを立ち上げます。その後、

のように、コマンドラインから/proxyにsend_goalを送ります。うまく動作するならば、action_tutorial_cppのfibonacci_action_serverのActionが呼ばれ、その結果が/proxyから返ってくるはずです。

client_within_action_1.cpp

handle_acceptedコールバックの中で、チュートリアルの通り、actionクライアントのコールバックをすべて設定してasync_send_goal()を実行するパターンです。しかしこれだと、handle_acceptedコールバックの中でactionクライアントの結果を待てないので、代理サーバーとしての機能を果たせませんね。

client_within_action_2.cpp

send_goalを呼び出した後、resultを受け取るまで、handle_acceptedコールバックの中で待ちたいわけです。send_goalの引数としてresultの参照を設定し、初期値をnullptrとしておきます。send_goalのコールバックが呼ばれて値が設定されるまで、handle_acceptedコールバックの中でbusy waitループで待ってみましょう。

しかしこの場合、actionの結果は返ってこず、正しく動かないです。唯一のExetutorがこのbusy waitでブロックされるため、send_goalで設定した各コールバックが呼ばれることは無く、デッドロック状態に陥るからです。

client_within_action_3.cpp

send_goalのコールバックを並列で処理してほしいので、MultiThreadedExecutorを使うように変更してみました。しかし、これもやはり動きません。

これはcallback groupが原因です。既定のcallback_groupはMutuallyExclusiveなので、handle_acceptedコールバックの実行中にほかのコールバックイベントを処理することはできないのです。そのため、2の場合と同じく、handle_acceptedコールバックはずっと終わらないのです。

client_within_action_4.cpp

ではどうすればいいかというと、callback groupをReentrantに変更します。こうすると、とりあえず動くようになりました！

callback groupをReentrantに設定すると、handle_acceptedコールバックを処理している間にも、他のスレッドにより別のコールバックが処理されるようになります。send_goalで設定したコールバックがほかのスレッドで処理され、resultが更新されるので、handle_acceptedコールバックもbusy waitループから抜けることができるのです。

client_within_action_5.cpp

get_resultで結果を受け取るのにコールバックを用いると、複数のasync_send_goal()の要求に対応するのがけっこう面倒になります。また、busy wait ループでは、タイムアウト処理が欲しくなりますが、自分で書くのも冗長です。

ところでasync_send_goal()はrclcpp::Task::futureを返します。futureということは、これをチュートリアルにあるようにspin_until_future_complete()で待てば、良い感じに結果が来るのを待てるのでは、と思いませんか？私は思いました。

しかし、これはうまくいかないのです。そのようにした場合、このプログラムはコールバック実行時に、以下の例外を発生します。

じつは、spin_until_future_complete()のようなspin()系の関数はリエントラントではなく、spin()の中でspin()系の関数を呼ぶことは、そもそも想定もサポートされていないのです。もし呼んだ場合、rclcppでは上のような例外が発生して、そのことを知らせてくれます。

client_within_action_6.cpp

ではどうしましょうか。Task::futureにはwait_for()関数が用意されていて、これを使うとfutureを待つことができます。内部ではExecutorがブロックして待つだけなので、busy waitとそれほど大差ないのですが、sleep関数などを使ってbusy waitするよりもresultに対する応答が多少早くなることは期待できます。タイムアウトも設定できるので、rclcppの場合、おそらくこれがベストプラクティスではないかと思っています。