Note: This tutorial assumes that you have completed the previous tutorials: ROS tutorials.

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_serverライブラリを用いてフィボナッチアクションサーバーを作ります。この例のアクションサーバーではフィボナッチ数列を生成し、ゴールは数列の大きさとし、フィードバックはそれまでの実行結果の数列とし、リザルトは最終的な数列とします。

Tutorial Level: BEGINNER

Next Tutorial: Write a simple action client

Contents

アクションメッセージを作る
シンプルなアクションサーバーを書く
1. コード
2. コードの説明
アクションのコンパイルと実行
アクションサーバーにゴールを送る

アクションメッセージを作る

アクションを書く前に、ゴールとリザルト、そしてフィードバックのメッセージを作ることが重要です。アクションメッセージは.actionファイルがら自動的に生成されます。アクションファイルの詳細については actionlib のドキュメンテーションを参照してください。このファイルはアクションのためのゴールとリザルト、フィードバックトピックの型とフォーマットを定義します。 learning_actionlib/action/Fibonacci.actionを作りエディタで開いて、以下を記述してください。

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

makeの過程で自動的にメッセージファイルを生成するには、CMakeLists.txtに少し記述を足す必要があります。

以下のようにactionlib_msgs パッケージをfind_packageマクロの引数に追加してください（もしcatkin_create_packageをCMakeLists.txtの生成に使用しているのであれば、これはすでに追加されているかもしれません）。:
```
find_package(catkin REQUIRED COMPONENTS actionlib_msgs)
```
- CMakeはfind_package actionlib_msgs を必要としていることに注意してください。（message_generationは明示的に記載する必要はありません。actionlib_msgsによって暗に参照されます。）
add_action_filesマクロを使って、生成したいアクションを宣言してください。:
```
add_action_files(
  DIRECTORY action
  FILES Fibonacci.action
)
```
generate_messagesマクロを呼びます。actionlib_msgsや、std_msgsなど他のメッセージへの依存関係の記述を忘れないでください。:
```
generate_messages(
  DEPENDENCIES actionlib_msgs std_msgs  # Or other packages containing msgs
)
```
以下のようにactionlib_msgsをcatkin_packageマクロに追記します。:
```
catkin_package(
  CATKIN_DEPENDS actionlib_msgs
)
```
- catkin_packageはactionlib_msgsへのCATKIN_DEPENDのみを指示します。自動的にmessage_runtimeへの依存が内包されます。

次のコマンドによりactionファイルからmsgファイルを生成します。またその結果がこちらです。

$ cd ../.. # Go back to the top level of your catkin workspace
$ catkin_make
$ ls devel/share/learning_actionlib/msg/
FibonacciActionFeedback.msg  FibonacciAction.msg        FibonacciFeedback.msg
FibonacciResult.msg          FibonacciActionGoal.msg    FibonacciActionResult.msg  FibonacciGoal.msg
$ ls devel/include/learning_actionlib/
FibonacciActionFeedback.h  FibonacciAction.h        FibonacciFeedback.h  FibonacciResult.h
FibonacciActionGoal.h      FibonacciActionResult.h  FibonacciGoal.h

makeの過程で自動的にメッセージファイルを生成するには、CMakeLists.txtにrosbuild_initより前に以下を追記する必要があります。

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

また、genaction.pyスクリプトによって生成されるメッセージファイルからヘッダファイルを確実に生成させる必要があります。そのために、もしまだであれば、CMakeLists.txtの下の方にある以下の行をコメントアウトしてください。

rosbuild_genmsg()

手動でメッセージファイルを生成するには、actionlib_msgsパッケージのgenaction.pyスクリプトを使用してください。

シンプルなアクションサーバーを書く

コード

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

   1 #include <ros/ros.h>
   2 #include <actionlib/server/simple_action_server.h>
   3 #include <learning_actionlib/FibonacciAction.h>
   4 
   5 class FibonacciAction
   6 {
   7 protected:
   8 
   9   ros::NodeHandle nh_;
  10   // NodeHandle instance must be created before this line. Otherwise strange error may occur.
  11   actionlib::SimpleActionServer<learning_actionlib::FibonacciAction> as_; 
  12   std::string action_name_;
  13   // create messages that are used to published feedback/result
  14   learning_actionlib::FibonacciFeedback feedback_;
  15   learning_actionlib::FibonacciResult result_;
  16 
  17 public:
  18 
  19   FibonacciAction(std::string name) :
  20     as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false),
  21     action_name_(name)
  22   {
  23     as_.start();
  24   }
  25 
  26   ~FibonacciAction(void)
  27   {
  28   }
  29 
  30   void executeCB(const learning_actionlib::FibonacciGoalConstPtr &goal)
  31   {
  32     // helper variables
  33     ros::Rate r(1);
  34     bool success = true;
  35 
  36     // push_back the seeds for the fibonacci sequence
  37     feedback_.sequence.clear();
  38     feedback_.sequence.push_back(0);
  39     feedback_.sequence.push_back(1);
  40 
  41     // publish info to the console for the user
  42     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]);
  43 
  44     // start executing the action
  45     for(int i=1; i<=goal->order; i++)
  46     {
  47       // check that preempt has not been requested by the client
  48       if (as_.isPreemptRequested() || !ros::ok())
  49       {
  50         ROS_INFO("%s: Preempted", action_name_.c_str());
  51         // set the action state to preempted
  52         as_.setPreempted();
  53         success = false;
  54         break;
  55       }
  56       feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
  57       // publish the feedback
  58       as_.publishFeedback(feedback_);
  59       // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
  60       r.sleep();
  61     }
  62 
  63     if(success)
  64     {
  65       result_.sequence = feedback_.sequence;
  66       ROS_INFO("%s: Succeeded", action_name_.c_str());
  67       // set the action state to succeeded
  68       as_.setSucceeded(result_);
  69     }
  70   }
  71 
  72 
  73 };
  74 
  75 
  76 int main(int argc, char** argv)
  77 {
  78   ros::init(argc, argv, "fibonacci");
  79 
  80   FibonacciAction fibonacci(ros::this_node::getName());
  81   ros::spin();
  82 
  83   return 0;
  84 }

コードの説明

少しずつコードを噛み砕いていきましょう。

   1 #include <ros/ros.h>
   2 #include <actionlib/server/simple_action_server.h>
   3

actionlib/server/simple_action_server.h はシンプルなアクションを実装するために使用するライブラリです.

   3 #include <learning_actionlib/FibonacciAction.h>
   4

これは先のFibonacci.actionファイルから生成されたアクションメッセージをincludeしています。このヘッダファイルはFibonacciAction.msgファイルから自動的に生成されています。メッセージ定義の詳細はmsgページを参照してください。

   7 protected:
   8 
   9   ros::NodeHandle nh_;
  10   // NodeHandle instance must be created before this line. Otherwise strange error may occur.
  11   actionlib::SimpleActionServer<learning_actionlib::FibonacciAction> as_; 
  12   std::string action_name_;
  13   // create messages that are used to published feedback/result
  14   learning_actionlib::FibonacciFeedback feedback_;

これらはaction classのprotectedな変数です. ノードハンドルはコンストラクトされ、アクションのコンストラクト中にアクションサーバーに渡されます。アクションサーバーはアクションのコンストラクタの中でコンストラクトされ、以下のように使用されます。フィードバックメッセージとリザルトメッセージはアクション中で配信するために作られています。

  18   FibonacciAction(std::string name) :
  19     as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false),
  20     action_name_(name)
  21   {
  22     as_.start();

アクションのコンストラクタ中にて、アクションサーバーが作られます。アクションサーバーはノードハンドル、アクション名、オプションとしてコールバックを引数に取ります。この例ではアクションサーバーをコールバック引数を渡して生成しています。

  30   void executeCB(const learning_actionlib::FibonacciGoalConstPtr &goal)

さて、executeCB関数はコンストラクタの中で作られます。コールバック関数はゴールメッセージへのポインタが渡されます。注意: これはboost shared pointerです。ゴールメッセージ型の末尾に"ConstPtr"と付記することで得られます。

  32     // helper variables
  33     ros::Rate r(1);
  34     bool success = true;
  35 
  36     // push_back the seeds for the fibonacci sequence
  37     feedback_.sequence.clear();
  38     feedback_.sequence.push_back(0);
  39     feedback_.sequence.push_back(1);
  40 
  41     // publish info to the console for the user
  42     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]);

ここでアクションの内部が作られます。この例ではROS_INFOを配信してアクションが実行中であることを知らせています。

  43     // start executing the action
  44     for(int i=1; i<=goal->order; i++)
  45     {
  46       // check that preempt has not been requested by the client
  47       if (as_.isPreemptRequested() || !ros::ok())
  48       {
  49         ROS_INFO("%s: Preempted", action_name_.c_str());
  50         // set the action state to preempted
  51         as_.setPreempted();
  52         success = false;
  53         break;
  54       }

アクションサーバーの重要な要素の一つとして、アクションクライアントに現在のゴール実行をキャンセルさせるリクエストを送らせられることが挙げられます。クライアントが現在のゴールを阻止したいというリクエストを送ってきた場合、アクションサーバーはゴールをキャンセルし、後処理を実行して、setPreempted()関数を呼ぶべきです。setPreempted()関数はアクションがユーザーのリクエストにより阻止されたことを発信します。そしてアクションサーバーが阻止リクエストがサーバー実行プログラムに残っていないかチェックする周期を設定します。

  56       feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
  57       // publish the feedback
  58       as_.publishFeedback(feedback_);
  59       // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
  60       r.sleep();
  61     }

ここではフィボナッチシーケンスにフィードバックの値を入れ、アクションサーバーにより提供されるフィードバックチャンネルに配信します。そして、アクションは繰り返しに入り、フィードバックを配信し続けます。

  62     if(success)
  63     {
  64       result_.sequence = feedback_.sequence;
  65       ROS_INFO("%s: Succeeded", action_name_.c_str());
  66       // set the action state to succeeded
  67       as_.setSucceeded(result_);
  68     }
  69   }

一旦アクションがフィボナッチシーケンスを実行し終えたら、アクションはアクションクライアントにアクションが終了したことをsetSucceed()することで知らせます。

  75 int main(int argc, char** argv)
  76 {
  77   ros::init(argc, argv, "fibonacci");
  78 
  79   FibonacciAction fibonacci(ros::this_node::getName());
  80   ros::spin();
  81 
  82   return 0;
  83 }

最後にメイン関数でアクションを生成しノードを回します。アクションはgoalを受け取るまで実行し待ち続けます。

アクションのコンパイルと実行

以下の行をCMakeLists.txtファイルに追記します。:

add_executable(fibonacci_server src/fibonacci_server.cpp)

target_link_libraries(
  fibonacci_server
  ${catkin_LIBRARIES}
)

add_dependencies(
  fibonacci_server
  ${learning_actionlib_EXPORTED_TARGETS}
)

CMakeLists.txtの最小構成は以下のようになるでしょう。:

cmake_minimum_required(VERSION 2.8.3)
project(learning_actionlib)

find_package(catkin REQUIRED COMPONENTS roscpp actionlib actionlib_msgs)
find_package(Boost REQUIRED COMPONENTS system)

add_action_files(
  DIRECTORY action
  FILES Fibonacci.action
)

generate_messages(
  DEPENDENCIES actionlib_msgs std_msgs
)

catkin_package(
  CATKIN_DEPENDS actionlib_msgs
)

include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})

add_executable(fibonacci_server src/fibonacci_server.cpp)

target_link_libraries(
  fibonacci_server
  ${catkin_LIBRARIES}
)

add_dependencies(
  fibonacci_server
  ${learning_actionlib_EXPORTED_TARGETS}
)

rosbuild_add_executable(fibonacci_server src/fibonacci_server.cpp)

ROSのfuerte versionでは、rosbuild_add_executableの前後でboost linkingマクロを使用します。:

rosbuild_add_boost_directories()
rosbuild_add_executable(fibonacci_server src/fibonacci_server.cpp)
rosbuild_link_boost(fibonacci_server thread)

そしていつも通りビルドしましょう。:

$ rosmake learning_actionlib

新しくターミナルを立ち上げ、roscoreをスタートします。:

$ roscore

そして、アクションサーバーを起動します。:

$ rosrun learning_actionlib fibonacci_server

以下のような表示が得られるでしょう。(groovy):

[ INFO] 1250790662.410962000: Started node [/fibonacci], pid [29267], bound on [aqy], xmlrpc port [39746], tcpros port [49573], logging to [~/ros/ros/log/fibonacci_29267.log], using [real] time

アクションが正しく実行できているかチェックしましょう。配信されているtopicの一覧を見てみます。

$ rostopic list -v

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

Published topics:
 * /fibonacci/feedback [learning_actionlib/FibonacciActionFeedback] 1 publisher
 * /fibonacci/status [actionlib_msgs/GoalStatusArray] 1 publisher
 * /rosout [rosgraph_msgs/Log] 1 publisher
 * /fibonacci/result [learning_actionlib/FibonacciActionResult] 1 publisher
 * /rosout_agg [rosgraph_msgs/Log] 1 publisher

Subscribed topics:
 * /fibonacci/goal [learning_actionlib/FibonacciActionGoal] 1 subscriber
 * /fibonacci/cancel [actionlib_msgs/GoalID] 1 subscriber
 * /rosout [rosgraph_msgs/Log] 1 subscriber

または、ノード一覧を見てみましょう。:

$ rxgraph

groovyで実行しているのであれば

$ rqt_graph

fibonacci_server

これはアクションサーバーがフィードバックとステータス、リザルトのチャンネルを期待通り配信しており、またゴールとキャンセルチャンネルが期待通り購読されているのが見受けられます。アクションサーバーはきちんと起動し実行されています。

アクションサーバーにゴールを送る

アクションを使うための次のステップのため、アクションサーバーをCtrl-Cで止めてください。write a simple action clientのページを行きましょう。

ROS 2 Documentation

Wiki

Page

User