2009年12月31日木曜日

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
とすると、トピックも可視化できます。

すごい。。。

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

0 件のコメント:

コメントを投稿