近藤豊さんの著書「ROS2ではじめよう次世代ロボットプログラミング」を輪講形式で読み進めていくROS2勉強会@別府の資料です． 本資料はそのうち第7章Pythonクライアントライブラリrclpyについて紹介しています． 公開されているソースコードのうち本章の部分のみソースコード等を一部変更して公開しています． 本家リポジトリ： https://github.com/youtalk/get-started-ros2 7章部分のみ抜き出して変更を加えたもの： https://github.com/AtsukiYokota/ros2_examples

1. 2
7

2. @atsukiyokota
@AtsukiYokota

3. • R 2 O
7
• R
• R
S S

4. • 2 1 1
• 3
• 2
• 4 7
• 25

5. 2
1

6. • RT PIO
RI 2 T A O SI
• t mo i mn m
• c t n eA c p
• S O T PTC 1
I
m
p ct
l
6 2
5 5 11 2
26 5
fm
R
r
g OSa nP

7. l m
• m w r 2 A c
C R
• :i i B t
• Op ro f i e
K
OR S
A
PP I
2 2
y
SR a
5 I f cn A

8. •
• ::
• :
• C
•
• . . . .
• 2 2
• 2 2
•
• 3 3
• + #-

9. .
.
2

10. 3 2
• 3 ) Ot h r c
$ ros2 pkg create –build-type ament_python YOUR_AWESOME_PACKAGE
u lnkc / 2 ) / g ws_rt c
• 2 ) .32uo_ k c nkkx t mP u
• r a u
• ( yu .32 t r / le snk
(SRt cs s s .32 rgp (

11. from setuptools import setup
package_name = 'examples_rclpy_topics'
setup(
name=package_name, # パッケージ名
version='0.6.3', # バージョン番号
packages=[package_name], # ソースコードのディレクトリ
data_files=[ # ソースコード以外のファイル
('share/ament_index/resource_index/packages', ['resource/’ +
package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'], # 依存Python3モジュール
zip_safe=True,
author='Mikael Arguedas',
author_email='mikael@osrfoundation.org',
maintainer='Atsuki Yokota',
maintainer_email='atsuki.yokota@gmail.com',

12. keywords=['ROS'],
classifiers=[ # PyPIの分類情報
'Intended Audience :: Developers',
'License :: OSI Approved :: Apache Software License',
'Programming Language :: Python',
'Topic :: Software Development',
],
description='Examples of publishers/subscribers using rclpy.',
license='Apache License, Version 2.0',
tests_require=['pytest'], # テストフレームワーク名
entry_points={ # 実行コマンド名とその呼び出し先
'console_scripts': [
'publisher = ' + package_name + '.publisher:main',
'subscriber = ' + package_name + '.subscriber:main',
],
},
)

13. ; =fR
• 2 2 m si a 2 . S _
R S Rc a
• 2 S d_ . . o ld O
• . 3 2 P b a R3 3 prl ba
$ ros2 run examples_rclpy_topics publisher
• O
• c e u“ fys m =;2 c _b a
• c ; p s e “ lays m ai t m _a
=;2 c . . / . ; ; 2 S
• ha ys fr mP S ai
1 ld

14. ,
• h . 2 v x i , egkm 2h
O rs PChy o n o
• _h . ,. , e h x O rs a oLR y h
o n v xo n e MSe
find_package(std_msgs REQUIRED)
find_package(action_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/SetMessage.srv"
"action/Fibonacci.action"
DEPENDENCIES std_msgs action_msgs
)
ament_export_dependencies(rosidl_default_runtime)
ament_package()
,

15. 3

16. import rclpy
from rclpy.node import Node
from std_msgs.msg import String
class MinimalPublisher(Node):
def __init__(self):
super().__init__('minimal_publisher')
self.publisher = self.create_publisher(String, 'chatter')
self.timer = self.create_timer(0.5, self.timer_callback)
def timer_callback(self):
msg = String()
msg.data = 'Hello World!'
self.publisher.publish(msg)
self.get_logger().info(msg.data)
_e
.
c
. dea
R . .
R N
. R
.
R _e
R

17. def main(args=None):
rclpy.init(args=args)
minimal_publisher = MinimalPublisher()
rclpy.spin(minimal_publisher)
rclpy.shutdown()
if __name__ == '__main__':
main()
. 1
.
= 2

18. import rclpy
from rclpy.node import Node
from std_msgs.msg import String
class MinimalSubscriber(Node):
def __init__(self):
super().__init__('minimal_subscriber')
self.subscription = self.create_subscription(
String, 'chatter', self.listener_callback)
def listener_callback(self, msg):
self.get_logger().info(msg.data)

19. def main(args=None):
rclpy.init(args=args)
minimal_subscriber = MinimalSubscriber()
rclpy.spin(minimal_subscriber)
rclpy.shutdown()
if __name__ == '__main__':
main()

20. 4

21. from example_interfaces.srv import AddTwoInts
import rclpy
from rclpy.node import Node
class MinimalService(Node):
def __init__(self):
super().__init__('minimal_service')
self.srv = self.create_service(
AddTwoInts, 'add_two_ints', self.add_two_ints_callback)
def add_two_ints_callback(self, request, response):
response.sum = request.a + request.b
self.get_logger().info('{} + {} = {}'.format(request.a, request.b,
response.sum))
return response
H
,
1

22. def main(args=None):
rclpy.init(args=args)
minimal_service = MinimalService()
rclpy.spin(minimal_service)
rclpy.shutdown()
if __name__ == '__main__':
main()

23. from example_interfaces.srv import AddTwoInts
import rclpy
from rclpy.node import Node
class MinimalClient(Node):
def __init__(self):
super().__init__('minimal_client')
self.client = self.create_client(AddTwoInts, 'add_two_ints')
while not self.client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('waiting...')
self.request = AddTwoInts.Request()
def call_async(self):
self.request.a = 1
self.request.b = 2
return self.client.call_async(self.request)
1 1

24. def main(args=None):
rclpy.init(args=args)
minimal_client = MinimalClient()
future = minimal_client.call_async()
rclpy.spin_until_future_complete(minimal_client, future)
if future.done() and future.result() is not None:
response = future.result()
minimal_client.get_logger().info('{} + {} = {}'.format(
minimal_client.request.a, minimal_client.request.b,
response.sum))
rclpy.shutdown()
F
F

25. 5

26. import time
from example_interfaces.action import Fibonacci
import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
class MinimalActionServer(Node):
def __init__(self):
super().__init__('minimal_action_server')
self._action_server = ActionServer(
self, Fibonacci, 'fibonacci',
execute_callback=self.execute_callback,
callback_group=ReentrantCallbackGroup())
_e _
b ec
b aec

27. def destroy(self):
self._action_server.destroy()
super().destroy_node()
async def execute_callback(self, goal_handle):
self.get_logger().info('executing...')
msg = Fibonacci.Feedback()
msg.sequence = [0, 1]
for i in range(1, goal_handle.request.order):
if goal_handle.is_cancel_requested:
goal_handle.canceled()
self.get_logger().info('goal_canceled')
return Fibonacci.Result()
msg.sequence.append(msg.sequence[i] + msg.sequence[i-1])
self.get_logger().info('feedback:{}'.format(msg.sequence))
goal_handle.publish_feedback(msg)
time.sleep(1) # dummy job
. /
/ PdI
c
w sn i
f a h f
. i
f f
t t Oe
5 3
Pd
f f
t
f f

28. goal_handle.set_succeeded()
result = Fibonacci.Result()
result.sequence = msg.sequence
self.get_logger().info('result:{}'.format(result.sequence))
return result
def main(args=None):
rclpy.init(args=args)
minimal_action_server = MinimalActionServer()
executor = MultiThreadedExecutor()
rclpy.spin(minimal_action_server, executor=executor)
minimal_action_server.destroy()
rclpy.shutdown()
if __name__ == '__main__':
main()
E
E

29. from action_msgs.msg import GoalStatus
from example_interfaces.action import Fibonacci
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
class MinimalActionClient(Node):
def __init__(self):
super().__init__('minimal_action_client')
self._action_client = ActionClient(self, Fibonacci, 'fibonacci')

30. def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('goal rejected')
return
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def feedback_callback(self, feedback):
self.get_logger().info('feedback:{}'.format(
feedback.feedback.sequence))

31. def get_result_callback(self, future):
status = future.result().action_status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('result:{}'.format(
future.result().requence))
rclpy.shutdown()
def send_goal(self):
self.get_logger().info('waiting...')
self._action_client.wait_for_server()
goal_msg = Fibonacci.Goal()
goal_msg.order = 10
self._send_goal_future = self._action_client.send_goal_async(
goal_msg, feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(
self.goal_response_callback)
ca _
b
b 3
b c
b

32. def main(args=None):
rclpy.init(args=args)
action_client = MinimalActionClient()
action_client.send_goal()
rclpy.spin(action_client)
action_client.destroy_node()
if __name__ == '__main__':
main()
D