Note: This tutorial assumes that you have completed the previous tutorials: writing a simple action server using the execute callback method.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

シンプルなアクションクライアントを書く

Description: このチュートリアルではsimple_action_clientライブラリを用いたフィボナッチアクションクライアントを作ります。この例をプログラムではアクションクライアントを作ってアクションサーバーへゴールを送ります。

Tutorial Level: BEGINNER

コード

まず、learning_actionlib/src/fibonacci_client.cpp をエディタで開き、以下を記述してください。:

   1 #include <ros/ros.h>
   2 #include <actionlib/client/simple_action_client.h>
   3 #include <actionlib/client/terminal_state.h>
   4 #include <learning_actionlib/FibonacciAction.h>
   5 
   6 int main (int argc, char **argv)
   7 {
   8   ros::init(argc, argv, "test_fibonacci");
   9 
  10   // create the action client
  11   // true causes the client to spin its own thread
  12   actionlib::SimpleActionClient<learning_actionlib::FibonacciAction> ac("fibonacci", true);
  13 
  14   ROS_INFO("Waiting for action server to start.");
  15   // wait for the action server to start
  16   ac.waitForServer(); //will wait for infinite time
  17 
  18   ROS_INFO("Action server started, sending goal.");
  19   // send a goal to the action
  20   learning_actionlib::FibonacciGoal goal;
  21   goal.order = 20;
  22   ac.sendGoal(goal);
  23 
  24   //wait for the action to return
  25   bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
  26 
  27   if (finished_before_timeout)
  28   {
  29     actionlib::SimpleClientGoalState state = ac.getState();
  30     ROS_INFO("Action finished: %s",state.toString().c_str());
  31   }
  32   else
  33     ROS_INFO("Action did not finish before the time out.");
  34 
  35   //exit
  36   return 0;
  37 }

コードの説明

さて、コードを噛み砕いていきましょう。

   1 #include <ros/ros.h>
   2 #include <actionlib/client/simple_action_client.h>
   3 #include <actionlib/client/terminal_state.h>
   4 

  • actionlib/client/simple_action_client.h はシンプルアクションクライアントを実装するためのアクションライブラリです。

  • actionlib/client/terminal_state.h は可能なゴール状態を定義します。

   4 #include <learning_actionlib/FibonacciAction.h>
   5 

これは上記のFibonacci.actionファイルから生成されたアクションメッセージを含みます。 これはFibonacciAction.msgファイルから生成されたヘッダファイルです。 メッセージの定義についての詳細は、msgページをご覧ください。

   6 int main (int argc, char **argv)
   7 {
   8   ros::init(argc, argv, "test_fibonacci");
   9 
  10   // create the action client
  11   // true causes the client to spin its own thread
  12   actionlib::SimpleActionClient<learning_actionlib::FibonacciAction> ac("fibonacci", true);

アクションクライアントはアクションの定義から作られます。 アクションの定義はアクションサーバーとどのようなメッセージ型で通信するかを記述します。 アクションクライアントのコンストラクタは2つの引数を取ります。接続するサーバーの名前とスレッドを自動的に回すための真偽値オプションです。 もしスレッドを使いたくなければ(そして、actionlibに暗にthread magicをするようにしてほしければ)、これは都合のよいオプションとなるでしょう。 ここではアクションクライアントをサーバー名と自動スピンオプションを真にしたものを用いて生成しています。

  14   ROS_INFO("Waiting for action server to start.");
  15   // wait for the action server to start
  16   ac.waitForServer(); //will wait for infinite time
  17 

アクションサーバーが起動し実行されていない可能性があるので、アクションクライアントはアクションサーバーが実行するまで待ちます。

  17   ROS_INFO("Action server started, sending goal.");
  18   // send a goal to the action
  19   learning_actionlib::FibonacciGoal goal;
  20   goal.order = 20;
  21   ac.sendGoal(goal);

ここではゴールメッセージが作られます。ゴールの値が設定されアクションサーバーに送信されます。

  24   //wait for the action to return
  25   bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

アクションクライアントはゴールが終えるまで待ちます。 待ち時間のタイムアウトは30秒に設定されています。これはつまり,関数は30秒後以降はgoalが終わっていなければfalseを返すということです。

  26   if (finished_before_timeout)
  27   {
  28     actionlib::SimpleClientGoalState state = ac.getState();
  29     ROS_INFO("Action finished: %s",state.toString().c_str());
  30   }
  31   else
  32     ROS_INFO("Action did not finish before the time out.");
  33 
  34   //exit
  35   return 0;
  36 }

もしタイムアウトより前にゴールが終われば、ゴール状態が返ってきます。 さもなければ、ユーザーにゴールが指定の時間内に得られなかったことを知らせます。

クライアントのコンパイルと実行

以下の行をCMakeLists.txtファイルに追記してください:

add_executable(fibonacci_client src/fibonacci_client.cpp)

target_link_libraries( 
  fibonacci_client
  ${catkin_LIBRARIES}
)

rosbuild_add_executable(fibonacci_client src/fibonacci_client.cpp)
rosbuild_link_boost(fibonacci_client thread)

実行可能にしたら、新しいターミナルでroscoreを起動しましょう。

$ roscore

そして、クライアントを走らせます.:

$ rosrun learning_actionlib fibonacci_client

以下のような結果が見られるでしょう。:

  • [ INFO] 1250806286.804217000: Started node [/test_fibonacci], pid [9414], bound on [aqy], xmlrpc port [35466], tcpros port [55866], logging to [~/ros/ros/log/test_fibonacci_9414.log], using [real] time
    [ INFO] 1250806287.828279000: Waiting for action server to start.

クライアントがきちんと動いていることをチェックするために、配信されているROSトピックをリストアップしてみましょう。:

$ rostopic list -v

以下のような結果が得られるでしょう。:

  • Published topics:
     * /fibonacci/goal [learning_actionlib/FibonacciActionGoal] 1 publisher
     * /fibonacci/cancel [actionlib_msgs/GoalID] 1 publisher
     * /rosout [rosgraph_msgs/Log] 1 publisher
     * /rosout_agg [rosgraph_msgs/Log] 1 publisher
    
    Subscribed topics:
     * /fibonacci/feedback [learning_actionlib/FibonacciActionFeedback] 1 subscriber
     * /rosout [rosgraph_msgs/Log] 1 subscriber
     * /fibonacci/status [actionlib_msgs/GoalStatusArray] 1 subscriber
     * /fibonacci/result [learning_actionlib/FibonacciActionResult] 1 subscriber

もしくは、ノードを見ることもできます。:

$ rxgraph

Groovy以降のバージョンであれば以下のようにしてください。:

$ rqt_graph

fibonacci_client_hydro.png

クライアントがフィードバックと状態、リザルトのチャネルを期待通りに購読していて、またゴールとキャンセルチャネルを期待通り配信しているのがわかります。 クライアントはきちんと起動して実行されています。

サーバーとクライアントを接続する

次のactionlibのチュートリアルのために、Ctrl-Cでアクションクライアントを止め、run the action server and clientページに行きましょう。

Wiki: ja/actionlib_tutorials/Tutorials/SimpleActionClient (last edited 2023-03-08 10:24:22 by Hirotaka Yamada)