ROS2の勉強　第3弾：topicを使う

# #プログラミング ROS2< topicを使う >

##はじめに
ROS2(バージョンアップしたROS)を難なく扱えるようになることが目的である．その第3弾として，「topicを使う」を扱う．

##環境
#####仮想環境
|||
|:-:|:--|
|ソフト|VMware Workstation 15|
|実装RAM|3 GB|
|OS|Ubuntu 64 ビット|
|isoファイル|ubuntu-20.04.3-desktop-amd64.iso|

#####コンピュータ
|||
|:-:|:--|
|デバイス|MSI|
|プロセッサ|Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz|
|実装RAM|8.00 GB (7.89 GB 使用可能)|
|OS|Windows (Windows 10 Home, バージョン：21H1)|

#####ROS2
|||
|:-:|:--|
|Distribution|foxy|

## topic
topicはメッセージをやり取りするパイプのようなもので，データを配信する配信者（Publisher）とデータを購読する購読者（Subscriber）が存在する．ここでは，デモプログラミングを理解するのと，前回扱ったturtlesimをある軌道で動かすようなプログラムを通して，topicを使うプログラムの作成について学ぶ．

## プログラム1: デモ
### デモプログラム
<details><summary>**デモプログラム再現：配信者**</summary><div>

```python:publisher_test.py
import rclpy    # rospy on ROS
from std_msgs.msg import String

# main function
def main(args=None):
    # initate rclpy for creating Node
    rclpy.init(args=args)   # if not need, you don't have to set args

    node = rclpy.create_node('publisher_node')  # create node with node name

    publisher = node.create_publisher(String, '/topic_lesson', 10)  # 10 is queue size for QoS: queue size is depended on communication rate but 10 is enough in most cases
                                                                    # http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers#Choosing_a_good_queue_size
    msg = String()
    i = 0

    def cb():
        nonlocal i                          # to use variable i adjacent here, declare nonlocal instead of global
                                            # https://qiita.com/domodomodomo/items/6df1419767e8acb99dd7
        msg.data = f'Hello World: {i}'

        i += 1
        node.get_logger().info(f'Publishing: {msg.data}')   # log to show information on the console
        publisher.publish(msg)              # publish data
    
    rate = 0.5  # seconds
    timer = node.create_timer(timer_period_sec=rate, callback=cb)

    rclpy.spin(node)            # loop; waiting for callback of timer

    # after break loop, stop anything to finish this node safely
    node.destroy_timer(timer)
    node.destroy_node()
    rclpy.shutdown()
```
</details></div>

<details><summary>**デモプログラム再現：購読者**</summary><div>

```python:subscriber_test.py
import rclpy    # rospy on ROS
from std_msgs.msg import String

# main function
def main(args=None):
    # initate rclpy for creating Node
    rclpy.init(args=args)   # if not need, you don't have to set args

    node = rclpy.create_node('subscriber_node')  # create node with node name

    node.create_subscription(
                            String,             # Data type of message it will subscribe
                            '/topic_lesson',    # topic name it will subscribe
                            lambda msg: node.get_logger().info(f'I heard: {msg.data}'), # callback function run when it subscribes data; here, 
                                                                                        # using lambda (non-declared function) insted of function
                            10
                            )

    rclpy.spin(node)            # loop; waiting for data from publisher

    # after break loop, stop anything to finish this node safely
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
```
</details></div>

今回，デモプログラムに触れることで，改めてqueue_sizeについて少し学んだ．以下のサイトを参考にしたが，queue_sizeはデータの扱う速度（Hz: rate）に依存するらしい．どのような値すればよいかはこれといったものはないが，10Hz程度であれば1,2,3あたりで十分らしい．大きすぎても0であってもよくはないようだ．**特に0は注意で，0は無限の大きさを表しているらしく，メモリが無限大に使用されようとして危険とある**．だから例ではよく10にしているみたいである．10であれば，よっぽどのことがない限り不具合などは出なさそうである．（推論だが．．．）

http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers#Choosing_a_good_queue_size

また，プログラムの中で私自身知らなかったところがあったため，記述しておく．以下の記事を参考にしたのだが，``global``とよく似た``nonlocal``というものがあるようだ．ざっくりいうと，globalが一番外側にある変数を扱うようにするのに対して，nonlocalは1つ外側にある変数を扱えるようにするようだ．詳しくは以下の記事を参照されたい．

https://qiita.com/domodomodomo/items/6df1419767e8acb99dd7

### ビルドの準備
ROS1のときには，pythonファイルで記述している場合，基本的には実行権限さえ与えていれば，C++のようなコンパイル（ビルド）は必要ではなかった．しかしながら，ROS2ではpythonで作成したROSプログラムをros2で動かすためにはビルドしてあげる必要があるようだ．そのための手順を以下に示す．

#### 実行権限の付与はいらない！？
ROS1でpythonを扱う際には，``rosrun``で実行するものはpythonファイルそのものであったために実行権限の付与をしていた．しかしながら，ROS2ではpythonファイルでも変更時にはビルドを毎回必要とし，``ros2 run``で実行するものはpythonファイルそのものではないため，実行権限の付与は特に必要ではなさそうである．実際に実行権限の有無に関係なくビルドさえしていれば実行が可能であった．ただし，前回学んだlaunchはpythonファイルそのものを``ros2 launch``で実行するため実行権限の付与が必要であると思われる．

#### ファイルの配置
基本的にはpythonファイルは以下に示す構造となるように配置すればよい．

```bash:tree
topic_lesson/
├── package.xml
├── resource
│   └── topic_lesson
├── setup.cfg
├── setup.py
├── test
│   ├── test_copyright.py
│   ├── test_flake8.py
│   └── test_pep257.py
└── topic_lesson
    ├── __init__.py
    ├── publisher_test.py
    └── subscriber_test.py

3 directories, 11 files
```

#### setup.pyの編集
基本的にビルドに関する情報はsetup.pyの中に記述されている．

<details><summary>**setup.pyの編集**</summary><div>

```python:setup.py
from setuptools import setup

package_name = 'topic_lesson'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Yuya Shimizu',
    maintainer_email='yuya@example.com',
    description="a package for practice 'topic'",
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'publisher_test = topic_lesson.publisher_test:main',    # make file to run with "ros2 run", whose name is "publisher_test"
            'subscriber_test = topic_lesson.subscriber_test:main',   # make file to run with "ros2 run", whose name is "subscriber_test"
        ],
    },
)
```
</details></div>

今回は前回説明したlaunchファイルの作成はしないため，特に前回のような設定はしていない．
今回設定しているのは，下の方にある**entry_points**である．ここで，実行ファイルの名前とその実行内容を記載することで，``ros2 run``で実行できるファイルの生成ができる．ソースファイルとは別であるため，変更時には毎回ビルドが必要となる．ただし，setup.pyにさえ書き込んでいれば，``colcon build``するだけでよい．

#### ビルド
```bash:ビルド
cd ~/ros2_ws
colcon build
```

#### 実行コマンド
```bash:terminal_1
ros2 run topic_lesson publisher_test
```

```bash:terminal_2
ros2 run topic_lesson subscriber_test
```


#### 実行の様子
![topic_pub_sub.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/40f0ba93-e0f9-b92f-d13a-a0e3bd5ece0f.gif)

## プログラム2: turtlesim
### turtlesimを動かす
ここでは，topicを使うプログラムの作成について学んでいくために，前回触れたturtlesimのカメを円を描くような軌跡で動くようなプログラムを作成する．

<details><summary>**カメを動かすプログラム：配信者&購読者**</summary><div>

```python:publisher_test.py
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose

# Class for pub and sub
class MoveTurtle(Node): # self = Node
    def __init__(self):
        super().__init__('turtle_move') # node name is 'turtle_move'
        
        # Publisher
        self.pub = self.create_publisher(Twist, 'turtlesim/turtle1/cmd_vel', 10) # make a publisher for turtle1/cmd_vel whose type is Twist treating vel information
        self.timer = self.create_timer(1.0, self.timer_callback)        # make a timer for interrupt handling

        # Subscriber
        self.sub = self.create_subscription(Pose, 'turtlesim/turtle1/pose', self.pose_callback, 10)   # make a subscriber for turtle1/Pose hose type is Pose treating pose information

    # a callback function for subscription
    def pose_callback(self, msg):
        self.get_logger().info(f"(x, y, theta): [{msg.x:.3f} {msg.y:.3f} {msg.theta:.3f}]") # just notify the information of subscribed data, pose
    
    # a callback function for interrupt handling
    def timer_callback(self):
        msg = Twist()   # have Twist class in order to treat Twist data

        msg.linear.x = 1.0  # 1.0 [m/s] is set to x linear velocity
        msg.angular.z = 0.5 # 0.5 [rad/s] is set to z angular velocity

        self.pub.publish(msg)

# main function to launch everything in this code
def main(args=None):
    rclpy.init(args=args)   # initiate rclpy to use rclpy for the first time here
    move = MoveTurtle()     # MoveTurtle to prepare for publisher and subscriber to communicate eith turtle
    rclpy.spin(move)        # loop


if __name__ == '__main__':
    main()
```
</details></div>

ここでは，classを用いて作成している．これを通して，pythonのclassについての理解も深まった．ソースコードにはコメントで記載しているが，class宣言時に引数を渡すとき，その引数がselfになるため，``Node``を引数としてやれば，``Node.~``をclass全体で``self.~``と書くことができるようになる．

### ビルドの準備
#### setup.pyの編集
基本的にビルドに関する情報はsetup.pyの中に記述されている．

<details><summary>**setup.pyの編集**</summary><div>

```python:setup.py
from setuptools import setup

package_name = 'topic_lesson'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Yuya Shimizu',
    maintainer_email='yuya@example.com',
    description="a package for practice 'topic'",
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'publisher_test = topic_lesson.publisher_test:ppp',    # make file to run with "ros2 run", whose name is "publisher_test"
            'subscriber_test = topic_lesson.subscriber_test:main',   # make file to run with "ros2 run", whose name is "subscriber_test"
            'move = topic_lesson.moveTurtle:main'                   # make file to run with "ros2 run", whose name is "move"
        ],
    },
)
```
</details></div>

先ほどは記述しておらず，調べていないため，まだ経験でしかいえないが，どうやらpythonでROSプログラムを作成するときには，明示的にmain関数を用意してあげるようだ．ビルドの際の``:main``はmain関数を表している．つまりは以下のような構成だと今のところ理解している．

```python:entry_pointsでの構成
'<実行ファイル名> = <パッケージ名>.<pythonファイル名（.pyを省く）>:<main関数>'
```

このことから，おそらくmain関数となるものであればよいので，名前は任意であると思い，試しに先ほど示したpublisher_test.pyの``main``を``ppp``に変えて，setup.pyの部分も変更してビルドしてみたところ特に問題なくできた．つまり，main関数を指定するのは確かだが，ソースファイルとsetup.pyで一致していれば，名前は何でもよい．

#### ビルド
```bash:ビルド
cd ~/ros2_ws
colcon build
```

#### 実行コマンド
```bash:terminal_1
ros2 launch turtlesim_test myTurtlesim.launch.py
```

```bash:terminal_2
ros2 run topic_lesson move
```


#### 実行の様子
メディアの容量が大きいため，動画で示す．

<iframe width="560" height="315" src="https://www.youtube.com/embed/6w6-kvJdKB4" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

##感想
今回，ようやくROSのプログラム作成について触れた．非常に基本となるtopicを使うプログラムであったが，ROS1でその概念などは学んでおり，そこは変わっていないため，そこまで苦しくはなかった．ただ，実行までの手順が少し異なるため慣れる必要がある．また，今回プログラムに触れる中で，pythonの理解が深められたのは個人的には大きい収獲の一つとなった．以降，サービスやパラメータ，アクションを使うプログラムについて学ぶ．この参考にしている教材は早くも終盤となってきているため，この勢いで詰めていきたい．

##参考
- ロボットプログラミングROS2入門　玉川大学　岡田浩之　著，科学情報出版株式会社