eSEATでROSのactionlibを使う

ソフトウェア関連 >> RTコンポーネント関連 >> eSEAT_v2.5 >> SEATMLファイルの書き方 >> ROSノードの作成(eSEAT) >> eSEATでROSのactionlibを使う

eSEATでROSのactionlibを使う

ROS(ROS1のみですが）には、actionlibというパッケージで非同期サービス(action)を実装している機能があります。actionは、サービスの延長のような感じです。
eSEATでもactionのサポートをしたいと思います。現在のところまだ完全ではありませんが、SimpleActionServer, SimpleActionClientを作成できるようになっています。

eSEATにおけるactionもactionサーバーとactionクライアントがあり、adaptorを設定することでSimpleActionServerおよびSimpleActionClientを生成します。
SEATMLにおいては、type属性を ros_action_server または ros_action_client を設定します。
ここでは、ROSのactionlibチュートリアルにあるFibonacciのサーバーとクライアントをSEATMLで作成していきます。

eSEATでROSのactionlibを使う

actionサーバー（Fibonacci_server.seatml）を実装する

eSEATでactionサーバーを実装するには、SEATMLファイルの作成とactionを定義する .actionファイル(Fibonacci.action)の作成とコンパイルが必要です。

Fibonacci.actionファイルの作成

ROSのaction機能を使うには、actionを設定する .actionファイルが必要になります。通常は、catkinなどのROSのビルド環境を使うのですが、ここではcatkinを使わないで実装していきます。

actionlibのチュートリアルにあるFibonacciは、actionlib_tutorialsパッケージにありましたので、ここでもパッケージ名は、actionlib_tutorialsにします。

 # source /opt/ros/kinetic/setup.bash
 # source /usr/local/eSEAT/setup.bash
 # cd ~/work
 # gen_ros_pkg actionlib_tutorials
 # roscp actionlib_tutorials Fibonacci.action ros/packages/actionlib_tutorials/action

Fibonacci.actionは、

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

となっています。

Fibonacci.actionファイルのコンパイル

Fibonacci.actionファイルのコンパイルですが、eSEATでは、gen_action_msgと gen_ros_msgでactionファイルをコンパイルすることができます。

gen_action_msgコマンドは、 .actionファイルをactionに必要な６つの.msgファイルを生成します。
gen_ros_msgコマンドは、.msgファイルをPythonスクリプトに変換します。

Fibonacci.actionファイルのコンパイルには、下のように各コマンドを実行します。

 # source /opt/ros/kinetic/setup.bash
 # source /usr/local/eSEAT/setup.bash
 # cd ~/work
 # gen_action_msg actionlib_tutorials Fibonacci
 # gen_ros_msg

以上で、 ros/lib/site-packages/actionlib_tutorials/msg/　にFibonacciのactionに必要なファイルが生成されていることを確認して下さい。

SEATMLを作成する

actionの定義が終わりましたので、Fibonacci_server.seatmlを実装していきます。

 # source /usr/local/eSEAT/setup.bash
 # cd ~/work
 # gen_seatml Fibonacci_server

次に、適当なエディタでFibonacci_server.seatmlを開いて、action_serverの<adaptor>要素を設定します。サービスの実装をexecute_cb関数とすると

     <adaptor name="fibonacci" type="ros_action_server"
             action_type="actionlib_tutorials.Fibonacci"
             callback="execute_cb" />

となります。ただし、execute_cb関数は、<adaptor>要素の定義の前に<script>要素で関数定義をする必要があります。execute_cb関数は、fibonacci_server.pyから下記のようになります。

    <script>
      def execute_cb(goal):
        seat.setRate(1)
        success = True
        server=seat.getRosActionServer('fibonacci')
        if server :
          _feedback = server.newActionFeedback()
          _feedback.sequence = []
          _feedback.sequence.append(0)
          _feedback.sequence.append(1)
        
          seat.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (server.getActionName(), goal.order, _feedback.sequence[0], _feedback.sequence[1]))
        
          for i in range(1, goal.order):
            if server.isPreemptRequested():
              seat.loginfo('%s: Preempted' % server.getActionName() )
              server.setPreempted()
              success = False
              break
            _feedback.sequence.append(_feedback.sequence[i] + _feedback.sequence[i-1])
            server.publishFeedback(_feedback)
            seat.sleep()
          if success:
            _result=server.newActionResult()
            _result.sequence = _feedback.sequence
            seat.loginfo('%s: Succeeded' % server.getActionName() )
            server.setSucceeded(_result)
        return
    </script>

以上で、Fibonacci_server.seatmlの実装は終了です。

actionサービスの内容を rule要素で書く

上記の例では、actionサービスを予め関数で定義する方法を示しました。現在（2018-11-19)には、<state>要素の下の<rule>要素でもactionサービスを定義できるようになっています。actionサービスを<rule>要素で定義する場合には、adaptor要素でcallback属性を指定せずに、下のように変更する必要があります。

     <adaptor name="fibonacci" type="ros_action_server"
             action_type="actionlib_tutorials.Fibonacci" />

次に、actionサービスですが、下のように<rule>要素にsource属性で、adaptorのname属性（action_id）を指定し、<key>要素を ros_action で追加する必要があります。これは eSEATの内部で<key>要素とsource属性でマッチングを行っているからです。

  <rule source="fibonacci">
    <key>ros_action</key>
    <script>
      def execute_cb(goal):
        seat.setRate(1)
        success = True
        server=seat.getRosActionServer('fibonacci')
        if server :
          _feedback = server.newActionFeedback()
          _feedback.sequence = []
          _feedback.sequence.append(0)
          _feedback.sequence.append(1)
        
          seat.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (server.getActionName(), goal.order, _feedback.sequence[0], _feedback.sequence[1]))
        
          for i in range(1, goal.order):
            if server.isPreemptRequested():
              seat.loginfo('%s: Preempted' % server.getActionName() )
              server.setPreempted()
              success = False
              break
            _feedback.sequence.append(_feedback.sequence[i] + _feedback.sequence[i-1])
            server.publishFeedback(_feedback)
            seat.sleep()
          if success:
            _result=server.newActionResult()
            _result.sequence = _feedback.sequence
            seat.loginfo('%s: Succeeded' % server.getActionName() )
            server.setSucceeded(_result)
        return
    </script>
  </rule>

この方式を使うと、eSEATの内部状態が変更になると、ros_actionも変更されることになりますので、ご注意ください。

actionクライアント（Fibonacci_client.seatml）を実装する

次に、Fibonacci_client.seatmlを実装していきます。

 # source /usr/local/eSEAT/setup.bash
 # cd ~/work
 # gen_seatml Fibonacci_client

適当なエディタでFibonacci_client.seatmlを開いて、action_clientの<adaptor>要素を設定します。

     <adaptor name="fibonacci" type="ros_action_client"
             action_type="actionlib_tutorials.Fibonacci" />

最後に、Fibonacciのactionの呼び出しを実装します。actionlib_tutorialsのfibonacci_client.pyでは、actionのgoalを設定し、結果を受け取って終了するというコマンドライン入力のような動作をしています。通常、eSEATは実行すると周期実行の終了待ち状態になりますので、１つのスクリプトを実行して終了するようにするには、<onentry>要素を使います。

  <state name="main_state">
   <onentry>
    <script>
     client=seat.getRosActionClient('fibonacci')
     if client :
       goal=client.newActionGoal(order=10)
       client.setActionGoal(goal)
       result=client.waitActionResult()
       print("  Result==>", result)
     seat.exit()
    </script>
   </onentry>
  </state>

Fibonacci_clientで途中経過を出力する

上記の例だとServiceとほとんど変わらないので、Actionの途中経過を出力できるようにします。途中経過の出力には、コールバック関数が必要になりますので、<general>要素の中で<script>要素として定義します。

  <script>
    def fibonacci_fb(feedback):
      print(feedback)
  </script>

あとは、 seat.getActionGoalメソッドの呼出しの時に、 feedback_cbキーで上記の関数を設定します。最終的に下のようになると思います。

<?xml version="1.0" encoding="UTF-8" ?>
<seatml>
  <general name="Fibonacci_client">
    <adaptor name="fibonacci" type="ros_action_client"
             action_type="actionlib_tutorials.Fibonacci" />
    <script>
    def fibonacci_fb(feedback):
      print(feedback)
    </script>
  </general>

  <state name="main_state">
   <onentry>
    <script>
     client=seat.getRosActionClient('fibonacci')
     if client: 
       goal=client.newActionGoal(order=10)
       client.setActionGoal(goal, feedback_cb=fibonacci_fb)
       result=client.waitActionResult()
       print("  Result==>", result)
    </script>
   </onentry>

   <onexec>
    <script>
     seat.exit()
    </script>
   </onexec>
  </state>
</seatml>

途中経過の出力を rule要素で記述する

上記の例では、途中経過の出力を予め定義した関数の呼び出しで行っていました。現在（2018-11-20)では、feedback_cb, done_cb, active_cbも<rule>要素で記述できるように setActionGoalExメソッドを追加しました。例えば、

  seat.setActionGoal(goal, feedback_cb='fback')

と記述すると、<rule>要素で source属性をactionClientのname属性値で<key>要素として''fback''としている<rule>要素が呼びだされます。

Fibonacciのクライアントサンプルとして、ros_samples/Fibonacci_client1.seatmlを参照していただけば、用法等はわかると思います。

Fibonacci_serverとFibonacci_clientの動作確認

Fibonacci_server.seatmlとFibonacci_clientの動作確認をします。サーバーとクライアントを実行させるために３つのターミナルを使用します。

ターミナル１（roscore）

 # source /opt/ros/kinetic/setup.bash
 # roscore

ターミナル２（Fibonacci_server）

 # source /opt/ros/kinetic/setup.bash
 # cd ~/work
 # eSEAT_Node -l Fibonacci_server.seatml

ターミナル３（Fibonacci_client）

 # source /opt/ros/kinetic/setup.bash
 # cd ~/work
 # eSEAT_Node Fibonacci_client.seatml

ターミナル３では、

WARNING: cannot load logging configuration file, logging is disabled
  Result==> sequence: [0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55]
...Terminate.

と表示されれはOKです。

また、途中経過を出力させた場合には、下のようになります。

$ eSEAT_Node Fibonacci_client.seatml 
WARNING: cannot load logging configuration file, logging is disabled
sequence: [0, 1, 1]
sequence: [0, 1, 1, 2]
sequence: [0, 1, 1, 2, 3]
sequence: [0, 1, 1, 2, 3, 5]
sequence: [0, 1, 1, 2, 3, 5, 8]
sequence: [0, 1, 1, 2, 3, 5, 8, 13]
sequence: [0, 1, 1, 2, 3, 5, 8, 13, 21]
sequence: [0, 1, 1, 2, 3, 5, 8, 13, 21, 34]
sequence: [0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55]
  Result==> sequence: [0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55]
...Terminate.

また、Fibonacciアクションの動作確認は、ROSで実装されているサーバー(fibonacci_server.py）やクライアント(fibonacci_client.py)との接続の確認も行って下さい。

eSEATでROSノードを作る（Service）

ROSノードの作成(eSEAT)

	eSEATでROSのactionlibを使う
Simple Wiki Based Contents Management System
	Home Projects Memo Misc Topics Software