(!) 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.

Pythonでコールバックを用いてシンプルなアクションサーバーを書く

Description: このチュートリアルではシンプルアクションサーバーを用いてフィボナッチアクションサーバーを実装します。この例ではアクションサーバーはフィボナッチ数列を生成し、ゴールは数列の大きさ、フィードバックは計算された数列、リザルトは最終の数列とします。

Tutorial Level: BEGINNER

Next Tutorial: ja/actionlib_tutorials/Tutorials/Writing a Simple Action Client (Python)

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

C++ version tutorialを見てください。 メッセージの生成まで終了したらこちらへ戻ってきてください。

シンプルなサーバーを書く

このチュートリアルのコードと例はactionlib_tutorialsパッケージにあります。 このチュートリアルを始める前にactionlibパッケージを先に読むとよいかもしれません。

コード

以下のコードはactionlib_tutorials/simple_action_servers/fibonacci_server.pyにあります。

アクションサーバーのpython実装はfibonacci actionにあります。

   1 #! /usr/bin/env python
   2 
   3 import roslib; roslib.load_manifest('actionlib_tutorials')
   4 import rospy
   5 
   6 import actionlib
   7 
   8 import actionlib_tutorials.msg
   9 
  10 class FibonacciAction(object):
  11   # create messages that are used to publish feedback/result
  12   _feedback = actionlib_tutorials.msg.FibonacciFeedback()
  13   _result   = actionlib_tutorials.msg.FibonacciResult()
  14 
  15   def __init__(self, name):
  16     self._action_name = name
  17     self._as = actionlib.SimpleActionServer(self._action_name, actionlib_tutorials.msg.FibonacciAction, execute_cb=self.execute_cb)
  18     self._as.start()
  19     
  20   def execute_cb(self, goal):
  21     # helper variables
  22     r = rospy.Rate(1)
  23     success = True
  24     
  25     # append the seeds for the fibonacci sequence
  26     self._feedback.sequence = []
  27     self._feedback.sequence.append(0)
  28     self._feedback.sequence.append(1)
  29     
  30     # publish info to the console for the user
  31     rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1]))
  32     
  33     # start executing the action
  34     for i in xrange(1, goal.order):
  35       # check that preempt has not been requested by the client
  36       if self._as.is_preempt_requested():
  37         rospy.loginfo('%s: Preempted' % self._action_name)
  38         self._as.set_preempted()
  39         success = False
  40         break
  41       self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
  42       # publish the feedback
  43       self._as.publish_feedback(self._feedback)
  44       # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
  45       r.sleep()
  46       
  47     if success:
  48       self._result.sequence = self._feedback.sequence
  49       rospy.loginfo('%s: Succeeded' % self._action_name)
  50       self._as.set_succeeded(self._result)
  51       
  52 if __name__ == '__main__':
  53   rospy.init_node('fibonacci')
  54   FibonacciAction(rospy.get_name())
  55   rospy.spin()

コードの説明

   6 import actionlib

この行でシンプルなアクションを実装するためのアクションライブラリをimportします。

   8 import actionlib_tutorials.msg

ゴールを送ったりフィードバックを受けとったりするのに必要な、いくつかのメッセージをimportします。 このメッセージはアクションの定義から生成されます。

  17     self._as = actionlib.SimpleActionServer(self._action_name, actionlib_tutorials.msg.FibonacciAction, execute_cb=self.execute_cb)
  18     self._as.start()

ここでは SimpleActionServerを作ります。 名前とアクションの型、オプションとして実行するコールバックを渡します。 この例ではコールバックを実行するので、新しくゴールが得られたときに受けとったコールバックが実行に長い時間をかけられるように、スレッドが回されます。

  20   def execute_cb(self, goal):

ここで、新たなゴールを受けとった時毎に実行するコールバック関数を定義します。

  22     r = rospy.Rate(1)
  23     success = True
  24     
  25     # append the seeds for the fibonacci sequence
  26     self._feedback.sequence = []
  27     self._feedback.sequence.append(0)
  28     self._feedback.sequence.append(1)
  29     
  30     # publish info to the console for the user
  31     rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1]))

ここで、アクションの内部が作られます。 この例ではrospy.loginfoが配信されアクションが実行していることをユーザーに知らせます。

  33     # start executing the action
  34     for i in xrange(1, goal.order):
  35       # check that preempt has not been requested by the client
  36       if self._as.is_preempt_requested():
  37         rospy.loginfo('%s: Preempted' % self._action_name)
  38         self._as.set_preempted()
  39         success = False

アクションサーバーの重要な要素の一つとして、アクションクライアントに現在のゴール実行をキャンセルさせるリクエストを送らせられることが挙げられます。 クライアントが現在のゴールを阻止したいというリクエストを送ってきた場合、アクションサーバーはゴールをキャンセルし、後処理を実行して、set_preempted関数を呼ぶべきです。 set_preempted関数はアクションがユーザーのリクエストにより阻止されたことを発信します。 ここでは毎回preemptかどうかをチェックします。 もしくは、阻止リクエストが受信されたときにコールバックを受けとることもできます。

  41       self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
  42       # publish the feedback
  43       self._as.publish_feedback(self._feedback)

ここではフィボナッチ数列をフィードバック変数に入れ、アクションサーバーに提供されるフィードバックチャンネルに配信します。 そして、アクションは繰り返しに入り、フィードバックを配信しつづけます。

  47     if success:
  48       self._result.sequence = self._feedback.sequence
  49       rospy.loginfo('%s: Succeeded' % self._action_name)
  50       self._as.set_succeeded(self._result)

一旦アクションがフィボナッチ数列を計算し終わったら、アクションサーバーはset_succeededを呼ぶことで、アクションクライアントにゴールが満たされたことを知らせます。

  52 if __name__ == '__main__':
  53   rospy.init_node('fibonacci')
  54   FibonacciAction(rospy.get_name())
  55   rospy.spin()

最後に、メイン関数がアクションサーバーを生成し、ノードを回します。

アクションサーバーを実行する

以下のコマンドでアクションサーバーは実行されるでしょう。

rosrun actionlib_tutorials fibonacci_server.py

Wiki: ja/actionlib_tutorials/Tutorials/Writing a Simple Action Server using the Execute Callback (Python) (last edited 2014-10-07 15:06:15 by Moirai)