ROS1 の SimpleActionServer を ROS2 で書き換える

# 前提
ROS1のActionはなんとなく理解している。


# はじめに
ROS1のTopic通信は、サクッとROS2用に書き換えられます。
一方、Action通信は一手間かかりそうなので、学んだことの備忘録として投稿します。

feedbackなどに関しては、この記事では触れません。
goalを受け取って処理するまでの最低限の流れに絞っています。
また、この記事ではPythonを使いますが、C++版もそのうち投稿予定です。

今回作成したプログラムは、[こちら](https://github.com/namiky73/ros1-ros2-action)。

# ROS1 の SimpleActionServer
基本的に、SimpleActionClientとSimpleActionServerという2つのWrapperを利用して、Action通信を行います。
（これ以外の方法を見たことがない...。）

SimpleActionServerの特徴は、**同時に一つのgoalだけがアクティブな状態になれる**ことです。
**single goal policy** と呼ぶそう。

例えば、「ダンスをする」というgoalの処理中に「走る」というgoalが来た場合、
ダンスをやめて走る（もしくはダンス終了まで待ってから走る）必要があります。
ダンスをしながら走ることはできないのです。

この処理は、SimpleActionServerの`is_preempt_requested()`を通して実現します。
新たなgoalを受理すると`is_preempt_requested()`がTrueになるので
それを検知して 処理中のgoalの終了→新たなgoalの処理 を行います。

## サンプルプログラム
では、ROS1のSimepleActionClient/ServerによるAction通信のサンプルです。
1秒おきに、`count`の数だけ、`command`を出力するだけのプログラムです。
ros1_action_sample というパッケージ中に作りました。


```text:action/Move.action
# goal
string command
uint8 count
---
# result
string result
---
# feedback
string fb  # do not use here
```

```Python:scripts/action_server.py
#! /usr/bin/env python

import rospy
import actionlib
from ros1_action_sample.msg import MoveAction, MoveResult, MoveFeedback
 

class SampleActionServer(object):

    def __init__(self):
        self._as = actionlib.SimpleActionServer('move', MoveAction, execute_cb=self.execute_cb, auto_start=False)
        rospy.loginfo('server start!')
        self._as.start()
    

    def execute_cb(self, goal):
        move_name = goal.command
        move_count = goal.count
        rospy.loginfo('receive goal: %s' % move_name)
        r = rospy.Rate(1) # 1Hz
        for i in range(move_count):
            if self._as.is_preempt_requested():
                rospy.loginfo('%s end... (preempted)' % move_name)
                self._as.set_preempted()
                return
            # do some moves
            rospy.loginfo('%s now! (count: %d)' % (move_name, i))
            r.sleep()
        
        rospy.loginfo('%s: end!! (succeeded)' % move_name)
        result = MoveResult()
        result.result = 'success!'
        self._as.set_succeeded(result)


if __name__ == '__main__':
    rospy.init_node('action_server_node')
    SampleActionServer()
    rospy.spin()
```

```Python:scripts/action_clinet.py
#! /usr/bin/env python

import rospy
import actionlib
from ros1_action_sample.msg import MoveAction, MoveResult, MoveFeedback, MoveGoal


class SampleActionClient(object):
    def __init__(self):
        self._ac = actionlib.SimpleActionClient('move', MoveAction)
        self._ac.wait_for_server()
        self._send_move_goals()


    def _send_move_goals(self):
        goal = MoveGoal()
        
        goal.command = 'dance'
        goal.count = 5  # 5 secs
        self._ac.send_goal(goal)
        rospy.sleep(2.5)  # sleep 2.5 secs

        goal.command = 'run'
        goal.count = 5  # 5 sec
        self._ac.send_goal(goal)
        rospy.sleep(2.5)  # sleep 2.5 secs

        goal.command = 'walk'
        goal.count = 5  # 5 sec
        self._ac.send_goal(goal)


if __name__ == '__main__':
    rospy.init_node('action_client_node')
    SampleActionClient()
    rospy.spin()
```


この2つのプログラムを実行した時、action_server.pyの出力は以下のようになります。

```
[INFO] [1587789086.817839]: receive goal: dance
[INFO] [1587789086.821029]: dance now! (count: 0)
[INFO] [1587789087.821907]: dance now! (count: 1)
[INFO] [1587789088.822679]: dance now! (count: 2)
[INFO] [1587789089.822628]: dance end... (preempted)
[INFO] [1587789089.929216]: receive goal: run
[INFO] [1587789089.932516]: run now! (count: 0)
[INFO] [1587789090.933953]: run now! (count: 1)
[INFO] [1587789091.933875]: run end... (preempted)
[INFO] [1587789092.040326]: receive goal: walk
[INFO] [1587789092.043808]: walk now! (count: 0)
[INFO] [1587789093.045170]: walk now! (count: 1)
[INFO] [1587789094.045167]: walk now! (count: 2)
[INFO] [1587789095.045155]: walk now! (count: 3)
[INFO] [1587789096.045191]: walk now! (count: 4)
[INFO] [1587789097.045149]: walk: end!! (succeeded)
```
"dance"の実行中に"run"、"run"の実行中に"walk" のgoalが送られるため
`if self._as.is_preempt_requested()` が働いて処理が中断されます。
"walk"は途中で他のgoalが来なかったため、最後まで実行できました。

では、これをROS2で置き換えてみます。


# ROS2 の ActionServer
このプログラムをROS2に置き換えてみようと思ったところ...
ROS2にはSimpleActionServerのようなWrapperがありませんでした。

公式でも少し[議論](https://github.com/ros2/rclcpp/issues/759)はあるようです。
> わざわざSimpleActionServerのようなものを別途作らなくてもよくない？
既存のActionServerをsingle goal policy用に少し改良できるんじゃない？

のようなノリです。

実際、single goal policy用に書かれた[サンプルプログラム](https://github.com/ros2/examples/blob/master/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_single_goal.py)がありました。
上記の議論をしていたJacobさんによるプログラムです。
これを参考に、先ほどのROS1のプログラムを書き換えてみます。

## サンプルプログラム

my_msgsというパッケージ内で、先ほどと同じMove.actionを作成、
ros2_action_sample_pyというパッケージ内でプログラム作成しました。

独自Actionの作り方は、[こちら](https://index.ros.org/doc/ros2/Tutorials/Actions/Creating-an-Action/)。
ROS2におけるPythonパッケージの作り方は、[こちら](https://qiita.com/nasu_onigiri/items/dc10c3b0bfcd4754357f)。

```Python:action_server.py
# modified by @nasu_onigiri
# https://github.com/ros2/examples/blob/master/rclpy/actions/minimal_action_server/examples_rclpy_minimal_action_server/server_single_goal.py

# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import time
import threading

import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.node import Node
from rclpy.callback_groups import ReentrantCallbackGroup
from my_msgs.action import Move
from rclpy.executors import MultiThreadedExecutor


class SampleActionServer(Node):
    def __init__(self):
        super().__init__('action_server_node')

        self._goal_handle = None
        self._goal_lock = threading.Lock()
        self._execute_lock = threading.Lock()

        self._as = ActionServer(
            self,
            Move,
            'move', # action name
            execute_callback=self.execute_callback,
            goal_callback=self.goal_callback,
            handle_accepted_callback=self.handle_accepted_callback,
            cancel_callback=self.cancel_callback,
            callback_group=ReentrantCallbackGroup()
        )


    def execute_callback(self, goal_handle):
        with self._execute_lock:
            result = Move.Result()
            move_name = goal_handle.request.command
            move_count = goal_handle.request.count
            self.get_logger().info(f'receive goal: {move_name}')
            for i in range(move_count):
                if not goal_handle.is_active:
                    self.get_logger().info(f'{move_name} end... (preempted)')
                    result = Move.Result()
                    return result
                # do some moves here
                self.get_logger().info(f'{move_name} now! (count: {i})')
                # 私のROS2(rclpy)環境には、ROS1のRate相当のものがまだないのでこれで仮置き
                # 実際は、Node.create_rate()というものがあるらしい
                time.sleep(1.0)

            self.get_logger().info(f'{move_name} end!! (succeeded)')
            goal_handle.succeed()
            return result


    def goal_callback(self, goal_request):
        """Accepts or rejects a client request to begin an action."""
        return GoalResponse.ACCEPT


    def handle_accepted_callback(self, goal_handle):
        with self._goal_lock:
            # This server only allows one goal at a time
            if self._goal_handle is not None and self._goal_handle.is_active:
                # Abort the existing goal
                self._goal_handle.abort()
            self._goal_handle = goal_handle
        goal_handle.execute()


    def cancel_callback(self, goal):
        """Accepts or rejects a client request to cancel an action."""
        return CancelResponse.ACCEPT


def main(args=None):
    rclpy.init()
    node = SampleActionServer()

    executor = MultiThreadedExecutor()
    rclpy.spin(node, executor=executor)
    
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
```

```Python:action_client.py
import time

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from my_msgs.action import Move


class SampleActionClient(Node):
    def __init__(self):
        super().__init__('action_client_node')
        self._ac = ActionClient(self, Move, 'move')
        self._ac.wait_for_server()
        self._send_move_goals()


    def _send_move_goals(self):
        goal = Move.Goal()
        
        goal.command = 'dance'
        goal.count = 5
        self._ac.send_goal_async(goal)
        time.sleep(2.5)

        goal.command = 'run'
        goal.count = 5
        self._ac.send_goal_async(goal)
        time.sleep(2.5)

        goal.command = 'walk'
        goal.count = 5
        self._ac.send_goal_async(goal)


def main(args=None):
    rclpy.init(args=args)
    node = SampleActionClient()
    rclpy.spin(node)


if __name__ == '__main__':
    main()
```

2つのプログラムを実行したときの
action_server.pyの出力は以下のようになりました。
先ほどのROS1のプログラムと同様の結果です。

```
[INFO] [action_server_node]: receive goal: dance
[INFO] [action_server_node]: dance now! (count: 0)
[INFO] [action_server_node]: dance now! (count: 1)
[INFO] [action_server_node]: dance now! (count: 2)
[INFO] [action_server_node]: dance end... (preempted)
[INFO] [action_server_node]: receive goal: run
[INFO] [action_server_node]: run now! (count: 0)
[INFO] [action_server_node]: run now! (count: 1)
[INFO] [action_server_node]: run end... (preempted)
[INFO] [action_server_node]: receive goal: walk
[INFO] [action_server_node]: walk now! (count: 0)
[INFO] [action_server_node]: walk now! (count: 1)
[INFO] [action_server_node]: walk now! (count: 2)
[INFO] [action_server_node]: walk now! (count: 3)
[INFO] [action_server_node]: walk now! (count: 4)
[INFO] [action_server_node]: walk end!! (succeeded)
```

## サンプルプログラムのポイント

### MultiThreadedExecutor
これを指定しないと、goalの処理中に他のgoalを受信できない。

### goal_callback → handle_accepted_callback
ActionServerのインスタンス生成時に、
`goal_callback`と`handle_accepted_callback`を指定しています。

これにより

1. goal を受信 
2. `goal_callback` が呼ばれる
3. `goal_callback` 内で `return GoalResponse.ACCEPT`
4. `handle_accepted_callback` が呼ばれる
5. `handle_accepted_callback` 内で `goal_handle.execute()`
6. `execute_callback` が呼ばれる

という流れを作ることができます。

`goal_callback`を指定せず `handle_accepted_callback`のみを指定した場合、
上記 2. 3. がスキップされるようです。
そのため、今回のサンプルプログラムでは`goal_callback`はなくても正常動作します。

`handle_accepted_callback`を指定しなかった場合は、
上記 2.~5. がスキップされていきなり`execute_callback`が呼ばれるようです。

### self._goal_handle
処理中のgoal（のhandle）を格納しておきます。
`handle_accepted_callback` 内でこれを利用して
新たなgoalを受け取ったときに、処理中のgoalがあればそれを中断します。
これにより、single goal policyを実現しています。


### self.\_execute_lock
元のプログラムでは使われていませんでしたが、付け足しました。
これにより、複数のgoalが同時に`execute_callback`を使うことを防ぎます（ROS1のSimpleActionServerと同じです）。
もしサンプルプログラムで`self._execute_lock`を使わなければ、以下のような結果になります。
"dance"のgoalの終了処理前に、"run"のgoalを処理し始めています。

```
[INFO] [action_server_node]: receive goal: dance
[INFO] [action_server_node]: dance now! (count: 0)
[INFO] [action_server_node]: dance now! (count: 1)
[INFO] [action_server_node]: dance now! (count: 2)
[INFO] [action_server_node]: receive goal: run
[INFO] [action_server_node]: run now! (count: 0)
[INFO] [action_server_node]: dance end... (preempted)
[INFO] [action_server_node]: run now! (count: 1)
[INFO] [action_server_node]: run now! (count: 2)
...（略）
```

どちらがいいかは、ケースバイケースです。
前のgoalの終了処理を待たずに、すぐに新たなgoalの処理にかかりたい場合もあると思うので。

ちなみにこのサンプルプログラムで
`self._execute_lock` と 既存goalの中断処理 を無しにすると
複数のgoalを同時に処理するActionServerとなります。
"ダンスしながら走る"ができてしまいます。

```
[INFO] [action_server_node]: receive goal: dance
[INFO] [action_server_node]: dance now! (count: 0)
[INFO] [action_server_node]: dance now! (count: 1)
[INFO] [action_server_node]: dance now! (count: 2)
[INFO] [action_server_node]: receive goal: run
[INFO] [action_server_node]: run now! (count: 0)
[INFO] [action_server_node]: dance now! (count: 3)
[INFO] [action_server_node]: run now! (count: 1)
[INFO] [action_server_node]: dance now! (count: 4)
[INFO] [action_server_node]: run now! (count: 2)
...（略）
```


# おわりに
ROS1のSimpleActionServerを、ROS2で書き換えてみました。
一手間かかる分、ROS1の頃よりも柔軟な処理ができそうです。