LoginSignup
8
6

More than 1 year has passed since last update.

@nasu_onigiri

ROS1 の SimpleActionServer を ROS2 で書き換える

Last updated at Posted at 2020-04-25

前提

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

はじめに

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

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

今回作成したプログラムは、こちら。

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 というパッケージ中に作りました。

action/Move.action
# goal
string command
uint8 count
---
# result
string result
---
# feedback
string fb  # do not use here
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()
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がありませんでした。

公式でも少し議論はあるようです。

わざわざSimpleActionServerのようなものを別途作らなくてもよくない？
既存のActionServerをsingle goal policy用に少し改良できるんじゃない？

のようなノリです。

実際、single goal policy用に書かれたサンプルプログラムがありました。
上記の議論をしていたJacobさんによるプログラムです。
これを参考に、先ほどのROS1のプログラムを書き換えてみます。

サンプルプログラム

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

独自Actionの作り方は、こちら。
ROS2における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()
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の頃よりも柔軟な処理ができそうです。

8
6
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
8
6