2009年12月31日木曜日

actionlibを理解する(Client編)

ではクライアントを書きましょう。

$ roscd learning_actionlib
して、
src/fibonacci_client.cppとして以下を保存します。

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <learning_actionlib/FibonacciAction.h>

int main (int argc, char **argv)
{
  ros::init(argc, argv, "test_fibonacci"); 

  // create the action client
  // true causes the client to spin it's own thread
  actionlib::SimpleActionClient<learning_actionlib::FibonacciAction> ac("fibonacci", true); 

  ROS_INFO("Waiting for action server to start.");
  // wait for the action server to start
  ac.waitForServer(); //will wait for infinite time
 
  ROS_INFO("Action server started, sending goal.");
  // send a goal to the action 
  learning_actionlib::FibonacciGoal goal;
  goal.order = 20;
  ac.sendGoal(goal);
  
  //wait for the action to return
  bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

  if (finished_before_timeout)
  {
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO("Action finished: %s",state.toString().c_str());
  }
  else  
    ROS_INFO("Action did not finish before the time out.");

  //exit
  return 0;
}

クライアントは使うだけなので短いですね。

では中身を見ていきます。

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>

3行目は中では使ってなさそうですけど、
チュートリアルには"defines the possible goal states"とあります。
中身を見ると定数定義っぽいです。

  enum StateEnum
  {
    RECALLED,
    REJECTED,
    PREEMPTED,
    ABORTED,
    SUCCEEDED,
    LOST
  } ;




辺りでしょうか。

   #include <learning_actionlib/FibonacciAction.h>

これはサーバと同じですね。
actionをやりとりするメッセージです。

actionlib::SimpleActionClient<learning_actionlib::FibonacciAction> ac("fibonacci", true); 

これでクライアントをFibonacciAction型で作ります。1つ目は接続先のサーバの名前、2つ目は
自動でスレッドでros::spin()をやらせるかどうかです。trueになっているので、ros::spin()の必要はないです。しかし、クライアントなのになぜspin()が必要なんでしょうか?よく分かりません。

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

これはサーバの応答待ち。引数にタイムアウトを入れられます。
省略すると無限に待ちます。

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

ゴールのorder(繰り返し回数)に20を指定してセットしています。
sendGoalで送信されます。

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

タイムアウト30秒で結果を待ちます。

if (finished_before_timeout)
  {
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO("Action finished: %s",state.toString().c_str());
  }
  else  
    ROS_INFO("Action did not finish before the time out.");

  //exit
  return 0;
}

resultはこのstateに入っているのでしょうか?

ではいつものようにビルドしましょう。

rosbuild_add_executable(fibonacci_client src/fibonacci_client.cpp)
を足して、
makeです。

$ rxgraph -t
すると、



です。

ではサーバとつないでみましょう。
$ roscore
$ rosrun learning_actionlib fibonacci_server
$ rosrun learning_actionlib fibonacci_client
するとしばらくまつと
clientのターミナルに
[ INFO] 1262265014.097766000: Waiting for action server to start.
[ INFO] 1262265015.153827000: Action server started, sending goal.
[ INFO] 1262265035.156137000: Action finished: SUCCEEDED
とでました。

$ rostopic echo /fibonacci/feedback
してから、クライアントを立ち上げると
header:
  seq: 19
  stamp: 1262265148042176000
  frame_id:
status:
  goal_id:
    stamp: 1262265130041426000
    id: /test_fibonacci-1-1262265130.41426000
  status: 1
  text: This goal has been accepted by the simple action server
feedback:
  sequence: (0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233, 377, 610, 987, 1597, 2584, 4181, 6765)
---
header:
  seq: 20
  stamp: 1262265149042190000
  frame_id:
status:
  goal_id:
    stamp: 1262265130041426000
    id: /test_fibonacci-1-1262265130.41426000
  status: 1
  text: This goal has been accepted by the simple action server
feedback:
  sequence: (0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233, 377, 610, 987, 1597, 2584, 4181, 6765, 10946)

という感じで計算途中の様子がみれます。
一方で、

$ rostopic echo /fibonacci/result
とすると、最後に
---
header:
  seq: 2
  stamp: 1262265262841868000
  frame_id:
status:
  goal_id:
    stamp: 1262265242840719000
    id: /test_fibonacci-1-1262265242.840719000
  status: 3
  text:
result:
  sequence: (0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233, 377, 610, 987, 1597, 2584, 4181, 6765, 10946)
とだけ表示されます。



$ rxgraph -t
として見てみると以下のようになっています。
今回はcancelは使いませんでしたね。


という感じです。

今年はこれくらいでおしまいです。

0 件のコメント:

コメントを投稿