RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_004日目_Action

#◆ 前回記事
RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_003日目_独自Message・Service　の続き

#◆ はじめに
前回は Raspberry Pi のROS環境へ、 「独自Message」「独自Service」 を実装したため、今回は 「独自のAction」 の実装を試行したい。

#◆ Actionとは
「ロボットに対して自律移動の命令を送り、移動が終わったときに結果が失敗したか成功したかを即座に知りたい。」というようなシチュエーションに対応するための通信方式、らしい。
「Service」形式 では、呼び出し先(子側)の処理が終わるまで待つため、上記の用途として適切ではない。
「TOPIC」形式 では、単純な実装では相手側の処理結果を知る仕組みが無いため、結果を返すためだけの「TOPIC」を追加で作成する必要があり、あまりスマートではない。

##＊ 基本用語

• Action
  • 処理に時間がかかる場合、実行中に要求を取り消したり、要求の進行状況を定期的に調べることができる　
  • 「Service」形式 に非同期通信機能を付加した便利方式のイメージ
  • 非同期通信方式
• Action Message
  • Action通信方式で使用する伝達対象となるデータ
  • TOPIC の Message に該当
• Goal
  • 日本語で "ゴール"　Serverを呼び出す際の引数
  • Action Message に定義する項目の一つ
  • 初期化パラメータのようなもの
• Result
  • 日本語で "結果"　Server側処理の結果
  • Action Message に定義する項目の一つ
  • 処理が終わった時に１回だけ通知される
• Feedback
  • 日本語で "反応"　Server側処理の途中経過
  • Action Message に定義する項目の一つ
  • Resultとは異なり複数回通知される
• Action Server
  • 「Service」形式 の 「Server」 に該当　（呼び方が違うだけ）
• Action Client
  • 「Service」形式 の 「Client」 に該当　（呼び方が違うだけ）
  • Serviceを呼び出すときに内部的には、Message を TOPIC へ Publish しているだけらしい
• Actionlib
  • 「Action」を実装するときに Import する必要のあるライブラリ
• フィボナッチの数列（Fibonacci numbers）
  • 算数レベルの足し算を小難しい表現にしただけ
  • 最初の２つが "1" で、３つ目以降の数字はすべて直前の２つの数字の和になっている数列
  • 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, ... ということ
  • 1+1=2, 1+2=3, 2+3=5, 3+5=8, ...

##＊ Actionの概念図
自分なりに腹落ちさせたイメージは下図のとおり。

#◆ Actionのサンプル実装
今回は、 Action Server側 で フィボナッチ数列 を計算しつつ、計算の途中経過を Action Client側 へ Feedback する仕組みを構築してみることにする。
といっても、チュートリアル段階ではコーディングを極力せずにサンプルプログラムに頼れる部分は最大限頼ってしまう。

##＊ Actionファイルのサンプルダウンロード

actionサンプルのダウンロード

$ cd ~/catkin_ws
$ source devel/setup.bash
$ roscd tutorials
$ mkdir action;cd action
$ wget -O fibonacci.action https://raw.githubusercontent.com/ros/common_tutorials/indigo-devel/actionlib_tutorials/action/Fibonacci.action

**fibonacci.action の内容**

fibonacci.action

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

##＊ ActionServer / ActionClient のサンプルダウンロード

ActionServer,ActionClientサンプルのダウンロード

$ roscd tutorials/scripts
$ wget https://raw.githubusercontent.com/ros/common_tutorials/indigo-devel/actionlib_tutorials/scripts/fibonacci_client.py
$ wget https://raw.githubusercontent.com/ros/common_tutorials/indigo-devel/actionlib_tutorials/scripts/fibonacci_server.py

　
Action Server を呼び出し、フィボナッチ数列の生成を指示するプログラム「fibonacci_client.py」。
Action Server を呼び出すときに Goal のパラメータを order=20 とする。
20個分のフィボナッチ数列を生成するよう Server側 へ指示する。
なお、ダウンロードしたサンプルプログラムは自分が作成している環境のパッケージ名とは異なるパッケージ名を指定しているため、パッケージ名部分を自分の環境に合わせて修正する。

**編集前のfibonacci_client.py の内容**

fibonacci_client.py

#! /usr/bin/env python

from __future__ import print_function

import rospy
# Brings in the SimpleActionClient
import actionlib

# Brings in the messages used by the fibonacci action, including the
# goal message and the result message.
import actionlib_tutorials.msg

def fibonacci_client():
    # Creates the SimpleActionClient, passing the type of the action
    # (FibonacciAction) to the constructor.
    client = actionlib.SimpleActionClient('fibonacci', actionlib_tutorials.msg.FibonacciAction)

    # Waits until the action server has started up and started
    # listening for goals.
    client.wait_for_server()

    # Creates a goal to send to the action server.
    goal = actionlib_tutorials.msg.FibonacciGoal(order=20)

    # Sends the goal to the action server.
    client.send_goal(goal)

    # Waits for the server to finish performing the action.
    client.wait_for_result()

    # Prints out the result of executing the action
    return client.get_result()  # A FibonacciResult

if __name__ == '__main__':
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.
        rospy.init_node('fibonacci_client_py')
        result = fibonacci_client()
        print("Result:", ', '.join([str(n) for n in result.sequence]))
    except rospy.ROSInterruptException:
        print("program interrupted before completion", file=sys.stderr)

**編集後のfibonacci_client.py の内容**

fibonacci_client.py

#! /usr/bin/env python

from __future__ import print_function

import rospy
# Brings in the SimpleActionClient
import actionlib

# Brings in the messages used by the fibonacci action, including the
# goal message and the result message.
import tutorials.msg

def fibonacci_client():
    # Creates the SimpleActionClient, passing the type of the action
    # (FibonacciAction) to the constructor.
    client = actionlib.SimpleActionClient('fibonacci', tutorials.msg.fibonacciAction)

    # Waits until the action server has started up and started
    # listening for goals.
    client.wait_for_server()

    # Creates a goal to send to the action server.
    goal = tutorials.msg.fibonacciGoal(order=20)

    # Sends the goal to the action server.
    client.send_goal(goal)

    # Waits for the server to finish performing the action.
    client.wait_for_result()

    # Prints out the result of executing the action
    return client.get_result()  # A FibonacciResult

if __name__ == '__main__':
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.
        rospy.init_node('fibonacci_client_py')
        result = fibonacci_client()
        s1 = 'Result:'
        s2 = ', '.join([str(n) for n in result.sequence])
        print("Result:", ', '.join([str(n) for n in result.sequence]))
    except rospy.ROSInterruptException:
        print('program interrupted before completion', file=sys.stderr)

　 Goal に設定された要素数分のフィボナッチ数列を生成する Action Server プログラム「**fibonacci_server.py**」。 Action Client の Goal設定時のパラメータ指定が `order=20` となっているので、そのまま実行すると20個のフィボナッチ数列が生成される想定。 forループ内で数列を生成する過程で、作られた数列を Feedback を使用して TOPIC形式 の Message として Publish しまくるとともに、処理完了時に Result を使用して最終処理結果（成功／失敗）を通知する。 こちらも、Clientと同じくパッケージ名が自分の環境と合っていないため、パッケージ名部分のみ自分の環境に合わせて修正する。

**編集前のfibonacci_server.py の内容**

fibonacci_server.py

#! /usr/bin/env python

import rospy

import actionlib

import actionlib_tutorials.msg

class FibonacciAction(object):
    # create messages that are used to publish feedback/result
    _feedback = actionlib_tutorials.msg.FibonacciFeedback()
    _result = actionlib_tutorials.msg.FibonacciResult()

    def __init__(self, name):
        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name, actionlib_tutorials.msg.FibonacciAction, execute_cb=self.execute_cb, auto_start = False)
        self._as.start()
      
    def execute_cb(self, goal):
        # helper variables
        r = rospy.Rate(1)
        success = True
        
        # append the seeds for the fibonacci sequence
        self._feedback.sequence = []
        self._feedback.sequence.append(0)
        self._feedback.sequence.append(1)
        
        # publish info to the console for the user
        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]))
        
        # start executing the action
        for i in range(1, goal.order):
            # check that preempt has not been requested by the client
            if self._as.is_preempt_requested():
                rospy.loginfo('%s: Preempted' % self._action_name)
                self._as.set_preempted()
                success = False
                break
            self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
            # publish the feedback
            self._as.publish_feedback(self._feedback)
            # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
            r.sleep()
          
        if success:
            self._result.sequence = self._feedback.sequence
            rospy.loginfo('%s: Succeeded' % self._action_name)
            self._as.set_succeeded(self._result)
        
if __name__ == '__main__':
    rospy.init_node('fibonacci')
    server = FibonacciAction(rospy.get_name())
    rospy.spin()

**編集後のfibonacci_server.py の内容**

fibonacci_server.py

#! /usr/bin/env python

import rospy

import actionlib

import tutorials.msg

class fibonacciAction(object):
    # create messages that are used to publish feedback/result
    _feedback = tutorials.msg.fibonacciFeedback()
    _result = tutorials.msg.fibonacciResult()

    def __init__(self, name):
        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name, tutorials.msg.fibonacciAction, execute_cb=self.execute_cb, auto_start = False)
        self._as.start()
      
    def execute_cb(self, goal):
        # helper variables
        r = rospy.Rate(1)
        success = True
        
        # append the seeds for the fibonacci sequence
        self._feedback.sequence = []
        self._feedback.sequence.append(0)
        self._feedback.sequence.append(1)
        
        # publish info to the console for the user
        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]))
        
        # start executing the action
        for i in range(1, goal.order):
            # check that preempt has not been requested by the client
            if self._as.is_preempt_requested():
                rospy.loginfo('%s: Preempted' % self._action_name)
                self._as.set_preempted()
                success = False
                break
            self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
            # publish the feedback
            self._as.publish_feedback(self._feedback)
            # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
            r.sleep()
          
        if success:
            self._result.sequence = self._feedback.sequence
            rospy.loginfo('%s: Succeeded' % self._action_name)
            self._as.set_succeeded(self._result)
        
if __name__ == '__main__':
    rospy.init_node('fibonacci')
    server = fibonacciAction(rospy.get_name())
    rospy.spin()

##＊ CMakeLists.txt と package.xml の編集
恒例の編集作業。
下図の CMakeLists.txt と package.xml に対し Action を使用するうえで必要となるビルド指定を追加する。

###CMakeLists.txt の編集

２行追加

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  actionlib_msgs  #<--- 追加
  actionlib  #<--- 追加
)

全体のコメントアウトを除去して下記のとおり編集

add_action_files(
  FILES
  fibonacci.action
)

１行追加

generate_messages(
  DEPENDENCIES
  std_msgs
  actionlib_msgs  #<--- 追加
)

###package.xml の編集

下記４行を追加

  <build_depend>actionlib</build_depend>
  <build_depend>actionlib_msgs</build_depend>
  <exec_depend>actionlib</exec_depend>
  <exec_depend>actionlib_msgs</exec_depend>

##＊ Action のビルドと実行

ビルド

$ cd ~/catkin_ws
$ catkin_make

予想以上に長いビルドログが出力されてビックリした。
-- tutorials: 8 messages, 1 services
って、そんなにたくさん Message は自作していないんですけど。。。

**実行ログ**

実行ログ

Base path: /home/pi/catkin_ws
Source space: /home/pi/catkin_ws/src
Build space: /home/pi/catkin_ws/build
Devel space: /home/pi/catkin_ws/devel
Install space: /home/pi/catkin_ws/install
####
#### Running command: "make cmake_check_build_system" in "/home/pi/catkin_ws/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/pi/catkin_ws/devel
-- Using CMAKE_PREFIX_PATH: /home/pi/catkin_ws/devel;/opt/ros/kinetic
-- This workspace overlays: /home/pi/catkin_ws/devel;/opt/ros/kinetic
-- Using PYTHON_EXECUTABLE: /usr/bin/python
-- Using Debian Python package layout
-- Using empy: /usr/bin/empy
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/pi/catkin_ws/build/test_results
-- Found gtest sources under '/usr/src/gtest': gtests will be built
-- Using Python nosetests: /usr/bin/nosetests-2.7
-- catkin 0.7.8
-- BUILD_SHARED_LIBS is on
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- ~~  traversing 1 packages in topological order:
-- ~~  - tutorials
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- +++ processing catkin package: 'tutorials'
-- ==> add_subdirectory(tutorials)
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- Generating .msg files for action tutorials/fibonacci /home/pi/catkin_ws/src/tutorials/action/fibonacci.action
Generating for action fibonacci
-- tutorials: 8 messages, 1 services
-- Configuring done
-- Generating done
-- Build files have been written to: /home/pi/catkin_ws/build
####
#### Running command: "make -j4 -l4" in "/home/pi/catkin_ws/build"
####
Scanning dependencies of target _tutorials_generate_messages_check_deps_fibonacciGoal
Scanning dependencies of target actionlib_msgs_generate_messages_py
[  0%] Built target std_msgs_generate_messages_py
[  0%] Built target actionlib_msgs_generate_messages_py
Scanning dependencies of target _tutorials_generate_messages_check_deps_fibonacciActionGoal
[  0%] Built target _tutorials_generate_messages_check_deps_greet
[  0%] Built target _tutorials_generate_messages_check_deps_fibonacciGoal
Scanning dependencies of target _tutorials_generate_messages_check_deps_fibonacciAction
Scanning dependencies of target _tutorials_generate_messages_check_deps_fibonacciActionResult
[  0%] Built target _tutorials_generate_messages_check_deps_hello
[  0%] Built target _tutorials_generate_messages_check_deps_fibonacciActionGoal
Scanning dependencies of target _tutorials_generate_messages_check_deps_fibonacciActionFeedback
Scanning dependencies of target _tutorials_generate_messages_check_deps_fibonacciFeedback
[  0%] Built target _tutorials_generate_messages_check_deps_fibonacciAction
[  0%] Built target _tutorials_generate_messages_check_deps_fibonacciActionResult
Scanning dependencies of target _tutorials_generate_messages_check_deps_fibonacciResult
Scanning dependencies of target actionlib_msgs_generate_messages_lisp
[  0%] Built target _tutorials_generate_messages_check_deps_fibonacciActionFeedback
[  0%] Built target _tutorials_generate_messages_check_deps_fibonacciFeedback
[  0%] Built target actionlib_msgs_generate_messages_lisp
Scanning dependencies of target actionlib_msgs_generate_messages_cpp
[  0%] Built target std_msgs_generate_messages_lisp
[  0%] Built target std_msgs_generate_messages_cpp
[  0%] Built target actionlib_msgs_generate_messages_cpp
Scanning dependencies of target actionlib_msgs_generate_messages_eus
[  0%] Built target _tutorials_generate_messages_check_deps_fibonacciResult
Scanning dependencies of target actionlib_msgs_generate_messages_nodejs
[  0%] Built target actionlib_msgs_generate_messages_eus
[  0%] Built target std_msgs_generate_messages_eus
[  0%] Built target actionlib_msgs_generate_messages_nodejs
[  0%] Built target std_msgs_generate_messages_nodejs
[  2%] Generating Python from MSG tutorials/fibonacciActionFeedback
[  4%] Generating Lisp code from tutorials/fibonacciActionFeedback.msg
[  6%] Generating C++ code from tutorials/fibonacciActionFeedback.msg
[  8%] Generating EusLisp code from tutorials/fibonacciActionFeedback.msg
[ 10%] Generating Lisp code from tutorials/fibonacciFeedback.msg
[ 12%] Generating EusLisp code from tutorials/fibonacciFeedback.msg
[ 14%] Generating Lisp code from tutorials/greet.msg
[ 16%] Generating Python from MSG tutorials/fibonacciFeedback
[ 18%] Generating Lisp code from tutorials/fibonacciActionResult.msg
[ 20%] Generating EusLisp code from tutorials/greet.msg
[ 22%] Generating C++ code from tutorials/fibonacciFeedback.msg
[ 25%] Generating Python from MSG tutorials/greet
[ 27%] Generating EusLisp code from tutorials/fibonacciActionResult.msg
[ 29%] Generating Lisp code from tutorials/fibonacciGoal.msg
[ 31%] Generating Lisp code from tutorials/fibonacciResult.msg
[ 33%] Generating EusLisp code from tutorials/fibonacciGoal.msg
[ 35%] Generating Python from MSG tutorials/fibonacciActionResult
[ 37%] Generating Lisp code from tutorials/fibonacciActionGoal.msg
[ 39%] Generating Lisp code from tutorials/fibonacciAction.msg
[ 41%] Generating C++ code from tutorials/greet.msg
[ 43%] Generating EusLisp code from tutorials/fibonacciResult.msg
[ 45%] Generating Lisp code from tutorials/hello.srv
[ 47%] Generating Python from MSG tutorials/fibonacciGoal
[ 47%] Built target tutorials_generate_messages_lisp
[ 50%] Generating EusLisp code from tutorials/fibonacciActionGoal.msg
[ 52%] Generating Javascript code from tutorials/fibonacciActionFeedback.msg
[ 54%] Generating Python from MSG tutorials/fibonacciResult
[ 56%] Generating EusLisp code from tutorials/fibonacciAction.msg
[ 58%] Generating C++ code from tutorials/fibonacciActionResult.msg
[ 60%] Generating EusLisp code from tutorials/hello.srv
[ 62%] Generating Python from MSG tutorials/fibonacciActionGoal
[ 64%] Generating Javascript code from tutorials/fibonacciFeedback.msg
[ 66%] Generating EusLisp manifest code for tutorials
[ 68%] Generating Javascript code from tutorials/greet.msg
[ 70%] Generating Python from MSG tutorials/fibonacciAction
[ 72%] Generating C++ code from tutorials/fibonacciGoal.msg
[ 75%] Generating Javascript code from tutorials/fibonacciActionResult.msg
[ 77%] Generating Javascript code from tutorials/fibonacciGoal.msg
[ 79%] Generating Python code from SRV tutorials/hello
[ 81%] Generating Javascript code from tutorials/fibonacciResult.msg
[ 83%] Generating C++ code from tutorials/fibonacciResult.msg
[ 85%] Generating Javascript code from tutorials/fibonacciActionGoal.msg
[ 87%] Generating Python msg __init__.py for tutorials
[ 89%] Generating Javascript code from tutorials/fibonacciAction.msg
[ 91%] Generating Javascript code from tutorials/hello.srv
[ 91%] Built target tutorials_generate_messages_eus
[ 93%] Generating Python srv __init__.py for tutorials
[ 93%] Built target tutorials_generate_messages_nodejs
[ 95%] Generating C++ code from tutorials/fibonacciActionGoal.msg
[ 97%] Generating C++ code from tutorials/fibonacciAction.msg
[100%] Generating C++ code from tutorials/hello.srv
[100%] Built target tutorials_generate_messages_py
[100%] Built target tutorials_generate_messages_cpp
[100%] Built target tutorials_generate_messages

とにもかくにも一度実行してみる。

Masterの実行

$ roscore

別のターミナルでActionServerの実行

$ cd catkin_ws
$ source devel/setup.bash
$ rosrun tutorials fibonacci_server.py

何も起こらない。。。Client が起動していないから当然か。

ここで、Message の生成数が８個だったのがどういう理由なのか気になったので、別のターミナルを起動し、どのような TOPIC が生成されているのかを念の為確認しておく。
rostopic list というコマンドを使用すると確認できるそうな。

Messageの確認コマンド

$ cd ~/catkin_ws
$ source devel/setup.bash
$ rostopic list -v

あっ、５つも増えてる。。。
catkin_make コマンドを実行すると Action の実装に必要な Message/TOPIC を自動的に生成してくれるということかな。

• Published topics:

  • /fibonacci/feedback [tutorials/fibonacciActionFeedback] 1 publisher
  • /fibonacci/status [actionlib_msgs/GoalStatusArray] 1 publisher
  • /fibonacci/result [tutorials/fibonacciActionResult] 1 publisher

• Subscribed topics:

  • /fibonacci/goal [tutorials/fibonacciActionGoal] 1 subscriber
  • /fibonacci/cancel [actionlib_msgs/GoalID] 1 subscriber

では、Clientを起動してみる。

別のターミナルでActionClientの実行

$ cd catkin_ws
$ source devel/setup.bash
$ rosrun tutorials fibonacci_client.py

まず、Server側に「Executing, creating fibonacci ...」と表示され、しばらく待っていると、「Succeeded」と表示された。

Server側の「Succeeded」の表示とほぼ同時に、Client側にResult値であるフィボナッチ数列２０個分が一斉に表示された。

#◆ 本日のまとめ

• フィボナッチ、 フィボナッチ、 フィボナッチ... 20回噛まずに言えたら大したもんです
• Action形式、色々なシチュエーションで使える気がする
• むしろ、Service形式いらなくない？
• Feedback を拾っていないし、Cancel の動きは試していないけど、そこはご愛嬌で
• Python3系の構文がPython2系で使えるようになる__future__という超便利なmoduleがあることを知る

#◆ 次回予告
無謀にもわずか５日目にしてセンサーか何かの制御にチャレンジしたいと思う。
調べごとなどが増えたり、実装につまづく部分が多かったりと、記事をすぐに書けない可能性があるが、頑張る。
オジサン頑張る。
情熱だけで無理やり乗り切る。
乗りきれるといいな。。。

#◆ 次回記事
RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_005日目_Arduino+モーター制御　に続く