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は使いませんでしたね。


という感じです。

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

actionlibを理解する(Server編)

ではactionlibをやっていきましょう。

$ roscd sandbox
$ roscreate-pkg learning_actionlib actionlib roscpp rospy roslib std_msgs actionlib_msgs

$ rosmake learning_actionlib
$ roscd learning_actionlib
して開始です。

今回はアクションサーバを作ります。
http://www.ros.org/wiki/actionlib_tutorials/Tutorials/SimpleActionServer(ExecuteCallbackMethod)

その前にここを見てactionlibの概念をおさえましょう。
http://www.ros.org/wiki/actionlib

概要を見ると、actionlibとは処理に時間がかかるサービス(srv)みたいなもんだ、とあります。
ROSのserviceでは呼び出すと処理が終わるまでロックします。
これに対しactionlibではロックせずに、処理待ちなどを行います。
このサーバーとクライアント構造を簡単につくるのがactionlibになります。
じゃあ、なんで最初からそういうのができるようなフレームワークにしなかったんだ?
という気がしますが、シンプルにするとこうなるんでしょうか?
callbackを使って通常のノードと接続し、ROSのメッセージプロトコルでActionClientとActionServerは通信します。



クライアントとサーバをつなぐメッセージは以下の3つの情報を含みます。

  • Goal・・・・目標値。ロボットの目標移動地点など。
  • Feedback・・サーバーの目標に対する進捗状況を示す。現在のロボット位置など。
  • Result・・・達成した結果を返す。ロボットの到達位置・姿勢など。
この3つを定義する.actionファイルから.msgファイルを生成し、これらを使ってActionClient, ActionServerを実装する、という流れになります。

では早速。以下をaction/Fibonacci.actionとして保存します。(mkdir actionしてください)
ファイル名でもろばれですね。



#goal definition
int32 order  
---
#result definition
int32[] sequence 
---
#feedback
int32[] sequence 


以下のようにしてactionファイルをコンパイルします。

$ roscd learning_actionlib
$ rosrun actionlib_msgs genaction.py . Fibonacci.action

もしくはCMakeLists.txtのrosbuild_initより前に以下のように書いて、


rosbuild_find_ros_package(actionlib_msgs)
include(${actionlib_msgs_PACKAGE_PATH}/cmake/actionbuild.cmake)
genaction()

rosbuild_genmsg()

も有効にしておくと自動でコンパイルしてくれます。


これで以下のように7つのメッセージが生成されました。
$ ls msg
FibonacciAction.msg          FibonacciActionResult.msg  FibonacciResult.msg
FibonacciActionFeedback.msg  FibonacciFeedback.msg
FibonacciActionGoal.msg      FibonacciGoal.msg

FibonacciAction.msgからすべてのメッセージがつながっています。

ではサーバーの実装に移りましょう。
サーバーだからサービスを提供する側ですね。
以下をsrc/fibonacci_server.cppとして保存してください。



#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <learning_actionlib/FibonacciAction.h>

class FibonacciAction
{
protected:
    
  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<learning_actionlib::FibonacciAction> as_;
  std::string action_name_;
  // create messages that are used to published feedback/result
  learning_actionlib::FibonacciFeedback feedback_;
  learning_actionlib::FibonacciResult result_;

public:
    
  FibonacciAction(std::string name) : 
    as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1)),
    action_name_(name)
  {
  }

  ~FibonacciAction(void)
  {
  }

  void executeCB(const learning_actionlib::FibonacciGoalConstPtr &goal)
  {
    // helper variables
    ros::Rate r(1); 
    bool success = true;

    // push_back the seeds for the fibonacci sequence
    feedback_.sequence.clear();
    feedback_.sequence.push_back(0);
    feedback_.sequence.push_back(1);

    // publish info to the console for the user
    ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
        
    // start executing the action
    for(int i=1; i<=goal->order; i++)
    {        
      // check that preempt has not been requested by the client
      if (as_.isPreemptRequested())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        // set the action state to preempted
        as_.setPreempted();
        success = false;
        break;
      }
      feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
      // publish the feedback 
      as_.publishFeedback(feedback_);
      // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
      r.sleep(); 
    }

    if(success)
    {
      result_.sequence = feedback_.sequence;
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }


};


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

  FibonacciAction fibonacci(ros::this_node::getName());
  ros::spin();

  return 0;
}



今回は長いですね・・・。

順にみていきましょう。




#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
actionlib/server/simple_action_server.hを使って簡単に作りましょう、ということですね。




#include <learning_actionlib/FibonacciAction.h>
これはさっき作ったメッセージファイルから自動で生成されます。
メッセージの定義ファイルです。





protected:
    
  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<learning_actionlib::FibonacciAction> as_;
  std::string action_name_;
  // create messages that are used to published feedback/result
  learning_actionlib::FibonacciFeedback feedback_;
  learning_actionlib::FibonacciResult result_;

これがメンバ変数。SimpleActionServerのインスタンスを持っちゃっています。
learning_actionlib::FibonacciFeedbackとFibonacciResultのインスタンスも持ちます。




FibonacciAction(std::string name) :
    as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1)),
    action_name_(name)
  {
  }
これがコンストラクタ。SimpleActionServerを初期化します。
引数はノードハンドラ(nh_)、サーバの名前(name)、コールバック(boost::bind())です。
コールバックの中身が実体になります。




void executeCB(const learning_actionlib::FibonacciGoalConstPtr &goal)
  {
で、これがコールバックの宣言部分で、goalへのポインタを引数に取ります。
中身は以下のようにつづきます。



// helper variables
    ros::Rate r(1);
    bool success = true;

    // push_back the seeds for the fibonacci sequence
    feedback_.sequence.clear();
    feedback_.sequence.push_back(0);
    feedback_.sequence.push_back(1);

    // publish info to the console for the user
    ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
1Hzで、まずfeedbackに、0と1をつめてます。




// start executing the action
    for(int i=1; i<=goal->order; i++)
    {
      // check that preempt has not been requested by the client
      if (as_.isPreemptRequested())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        // set the action state to preempted
        as_.setPreempted();
        success = false;
        break;
      }


クライアントが途中でActionを止めたくなったときに、止められるようにisPreemptRequested()で停止したときの処理を行います。setPreempted()を呼び出す必要があります。





feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
      // publish the feedback
      as_.publishFeedback(feedback_);
      // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
      r.sleep();
    }
ここがメインの処理(といっても二つの数字を足しているだけですが・・・)
途中で停止させる演出のためにわざと遅くして1Hzになっています。
publishFeedback()によりfeedbackチャネルに書き込まれます。




if(success)
    {
      result_.sequence = feedback_.sequence;
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }
ユーザにより停止されなければここが実行されて、
setSucceeded()で結果がresultというチャネルで書き込まれます。




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

  FibonacciAction fibonacci(ros::this_node::getName());
  ros::spin();

  return 0;
}
メインプログラムはFibonacciActionのインスタンスを作っておしまいです。
ros::spin()でコールバックを待ちます。


ではビルドしましょう。



rosbuild_add_executable(fibonacci_server src/fibonacci_server.cpp)
を加えて、さらに
rosbuild_genmsg()をコメントインします。
これでメッセージがコンパイルされます。

そしてmakeしてください。

$ roscore
$ rosrun learning_actionlib fibonacci_server
まだクライアントがないので、なにも起きません。
$ rostopic list -v
として、トピックが用意されていることだけ確認しましょう。

$ rxgraph -t
とすると、トピックも可視化できます。

すごい。。。

では次回はこれを使うクライアントを作りましょう。

今後の進め方 その3

いい加減地味なのにも飽きてきました。

チュートリアルリストを眺めています。
http://www.ros.org/wiki/AllTutorials


  1. ROS・・・本体
  2. camera_drivers
  3. common・・・actionlib, filterなどROSの根幹をなすものの一つ。
  4. diagnostics・・・自己診断。実機でやるなら必須かな。
  5. geometry・・・すでにやった(tf)
  6. image_common・・・この辺は画像処理
  7. image_pipeline
  8. imu_drivers
  9. joystick_drivers・・・PS3とか使えるみたいなのでそのうちやる。
  10. laser_drivers・・・・北陽とSICKだけみたい。
  11. laser_pipeline・・・パイプラインってのを使うらしい。そのうち軽く見ないと。
  12. motion_planning・・・PR2でのアームの軌道計画。
  13. navigation・・・・自律移動
  14. pr2・・・・計画系をいれない、ロボットを直接動かすレベル。
  15. pr2_mechanism・・・・リアルタイム層の制御。歩行入れるならここのレベルを勉強する必要あり。
  16. pr2_simulator・・・・上のpr2 stackでちゃんとシミュレータが動くのだろうか?互換性があるならpr2 stackだけ勉強すればいいはず。
  17. robot_model・・・・RRDFフォーマットでのロボットモデルの作り方。オリジナルロボットを作るならここ。
  18. ros_experimental・・・・roslispについて。lisperは必見。
  19. ros_pkg_tutorials・・・パッケージのつくり方。作ったソフトを公開とかする段になればまじめに勉強すべきかな。
  20. ros_tutorials・・・・すでにお世話になってるturtlesimとか。
  21. simulator_gazebo・・・3DシミュレーションのgazeboをROSでどう使うか。
  22. simulator_stage・・・・同じく2D移動シミュレータのstageへのインタフェース
  23. slam_gmapping・・・・自律移動の地図自動生成ソフトのgmapping
  24. sound_drivers・・・・音です。
  25. trex・・・・TREXというライブラリへのインタフェース。
  26. vision_opencv・・・・言わずと知れたOpenCVへのインタフェース
以上でチュートリアルの存在するスタックはおしまい。

最初は無限に存在するかのように思えたROSライブラリ群もこんなもんです。
他にチュートリアルが存在しないけど重要そうなのは、
を見ると、
  • kinematics
  • manipulation_common
  • opencv
  • openrave_planning
などがありますが、いずれも上のチュートリアルをやる中で出てくると思うので、
そのつど中を気にしながらやっていきましょう。

ようやく全容がなんとなく見えてきた気がします。

tfでタイムトラベル!(C++)

前回の発展系で、時系列を遡って亀に指令を出してみましょう。

http://www.ros.org/wiki/tf/Tutorials/Time%20travel%20with%20tf%20(C%2B%2B)

前回書き換えた部分(src/turtle_tf_listener.cpp)をさらに以下のようにします。


try{
    ros::Time past = ros::Time::now() - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", "/turtle1",
                              past, ros::Duration(1.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             past, transform);
見れば分かると思いますが、5秒前のturtle1のデータを使ってturtle2に指令値を与えます。



うーん、ぐちゃぐちゃですね。
5秒前の亀1と亀2の位置を使って現在の亀にフィードバックしちゃっているので、
むちゃくちゃな制御になります。

5秒前の亀1のデータと今の亀2の位置のデータをつかって今の亀にフィードバックすれば、
5秒前の亀1を追いかけるはずですね。
実は以下のように、時間は2つのリンク(座標系)にばらばらに指定ができます。
try{
    ros::Time now = ros::Time::now();
    ros::Time past = now - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", now,
                              "/turtle1", past,
                              "/world", ros::Duration(1.0));
    listener.lookupTransform("/turtle2", now,
                             "/turtle1", past,
                             "/world", transform);

"/world"を入れているところには、時間が変わっても動かないリンクを入れる必要があります。
過去の時間でまず/turtle1から/worldへの変換を計算し、現在で/worldと/turtle2の変換を計算します。下図参照。



これで5秒前のところを追いかける亀ができました。


これでtfのチュートリアルは終了です。

座標系(リンク)間の関係を求めるのにtfを使いました。