前提
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 というパッケージ中に作りました。
# goal
string command
uint8 count
---
# result
string result
---
# feedback
string fb # do not use here
#! /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()
#! /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パッケージの作り方は、こちら。
# 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()
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を指定しています。
これにより
- goal を受信
-
goal_callbackが呼ばれる -
goal_callback内でreturn GoalResponse.ACCEPT -
handle_accepted_callbackが呼ばれる -
handle_accepted_callback内でgoal_handle.execute() -
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の頃よりも柔軟な処理ができそうです。