Python3ではじめるROS2（ノード編）

#はじめに
ROS2とPython3を用いて、カスタムメッセージを使った簡単なノードを作ってみます。

**Python3ではじめるROS2 関連記事**

|回数|サブタイトル|内容|
|:--|:--|:--|
|[第1回](https://qiita.com/myasu/items/3a026697f535183e12e8)|カスタムメッセージ編|・環境の準備<br>・カスタムメッセージを作る<br>・（おまけ）.bashrcのカスタマイズ|
|[第2回](https://qiita.com/myasu/items/87ee6a208f4af615e653)（今回）|ノード編|・Pub/Subのノードを作る|
|[第3回](https://qiita.com/myasu/items/39362b03780cb773b1ed)|自動起動編|・roslaunchから起動する<br>・systemdからroslaunchを起動する|

#１．Pub / Sub ノードの作成

ROS1ではあまり気にしませんでしたが、ROS2では必ず「パッケージ」を作る必要があります。
ここでは、pub/subノードののパッケージを準備します。

##（１）準備

```sh:コマンドライン
~$ cd ~/ros2_ws/src/
#ワークスペースを作成
~/ros2_ws/src/$ ros2 pkg create pubsubpy
~/ros2_ws/src/$ cd pubsubpy
#オリジナルのバックアップ、不要なファイルの改名
~/ros2_ws/src/pubsubpy$ cp -p package.xml package.xml.org
~/ros2_ws/src/pubsubpy$ mv CMakeLists.txt CMakeLists.txt.org

#ファイルの生成
~/ros2_ws/src/pubsubpy$ touch __init__.py setup.py setup.cfg

#nodeディレクトリの作成
~/ros2_ws/src/pubsubpy$ mkdir node

#あとでビルドするときに下記のエラーがでるので、予め空ファイルを作っておく
#「error: can't copy 'resource/pubsubpy': doesn't exist or not a regular file」
~/ros2_ws/src/pubsubpy$ mkdir resource
~/ros2_ws/src/pubsubpy$ cd resource
~/ros2_ws/src/pubsubpy/resource$ touch pubsubpy
~/ros2_ws/src/pubsubpy/resource$ cd ../..
~/ros2_ws/src$
```

この時点で、下記のようなディレクトリ構成になっています。

```:ディレクトリ構成
~/ros2_ws
   + src
     + pubsubpy
     |  + include
     |  + node
     |  + resource
     |  + src
     |
     + pubsubpy_mes
        + msg
        + src
        + srv
```

VSCodeから見たディレクトリ構成・ファイル構成です。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/145581/73c2c5c3-cd03-6d56-5096-3abedafe7640.png)


##（２）Pythonパッケージの準備

###__init__.py

```python:__init__.py
```

忘れずに`__init__.py`を作成して下さい。中身は空で良いです。

###package.xml

次に、先に実行したros2 pkg create pubsubpyで作られたファイルを編集します。

```xml:package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">

  <!-- ### ここにパッケージ名を記述 ### -->
  <name>pubsubpy</name>
  <version>0.0.0</version>
  <description>pubsubpy sample</description>
  <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
  <license>TODO: License declaration</license>

  <!-- ### python使う時は不要なのでコメントアウト ### -->
  <!--<buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>-->

  <!-- ### ここにmsgのパッケージ名を記述 ### -->
  <exec_depend>pubsubpy_mes</exec_depend>

  <!-- ### その他、依存関係のあるパッケージ ### -->
  <exec_depend>rclpy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>launch_ros</exec_depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <export>
 　 <!-- ### python使う時は不要なのでコメントアウト ### -->
    <!--<build_type>ament_cmake</build_type>-->
    <build_type>ament_python</build_type>
  </export>
</package>
```

###setup.py

新規に作った`setup.py`ファイルを記述します。

```python:setup.py
from setuptools import find_packages
from setuptools import setup
import os
from glob import glob

### ここにパッケージ名を記述 ###
package_name = 'pubsubpy'

### 必要に応じてパッケージ情報を記述 ###
setup(
    name=package_name,
    version='0.7.1',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('*.launch.py'))
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    author='ubuntu',
    author_email='ubuntu@todo.todo',
    maintainer='ubuntu',
    maintainer_email='ubuntu@todo.todo',
    keywords=['ROS'],
    classifiers=[
        'Intended Audience :: Developers',
        'License :: MIT License',
        'Programming Language :: Python',
        'Topic :: Software Development',
    ],
    description=(
        'pubsubpy sample'
    ),
    license='MIT License',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'pub = node.Pub:main',
            'sub = node.Sub:main',
        ],
    },
)
### ↑ entry_pointsの部分に、パッケージに含まれるノード(pythonソース)を列挙 ###
```

上記のソースの終わりの方で、ノード(pythonソース)を記述しますが、`node.*:main`となっているのは、pythonのソースを`pubsubpy/node`ディレクトリに置いているためです。
ここの書き方が、地味にハマりどころなので、注意して下さい・・・
（間違えると、runするときにpythonノードが見つからないエラーが出ます）

###setup.cfg

最後に、新規に作った`setup.cfg`ファイルを記述します。

```:setup.cfg
[develop]
script-dir=$base/lib/pubsubpy
[install]
install-scripts=$base/lib/pubsubpy
```


##（３）ノードを作る

```sh:コマンドライン
~/ros2_ws/src/pubsubpy$ mkdir node 
~/ros2_ws/src/pubsubpy$ cd node
~/ros2_ws/src/pubsubpy/node$ touch Pub.py Sub.py　__init__.py

#nodeディレクトリにも__init__.pyを生成します
#作るのを忘れると ros2 run pubsubpy pub したときに
#「ModuleNotFoundError: No module named 'Pub'」エラーが出ます
```

ソースファイル名の先頭文字は、大文字にしてください。


```python:__init__.py
```

忘れずに`__init__.py`を作成して下さい。中身は空で良いです。


###送信側ノード

```python:Pub.py
#!/usr/bin/env /usr/bin/python3
# -*- coding: utf-8 -*-
# -----------------------------------------------
# ROS2 Node 送信側
#
# The MIT License (MIT)
# Copyright (C) 2019 myasu.
# -----------------------------------------------

import rclpy
from rclpy.node import Node
# カスタムメッセージ
from pubsubpy_mes.msg import GpioMsg


class MyPublisher(Node):
    """
    送信側
    """

    # ノード名
    SELFNODE = "mypub"
    # トピック名
    SELFTOPIC = "mes_" + SELFNODE

    def __init__(self):
        """
        コンストラクタ
        Parameters
        ----------
        """
        # ノードの初期化
        super().__init__(self.SELFNODE)
        # コンソールに表示
        self.get_logger().info("%s initializing..." % (self.SELFNODE))
        # publisherインスタンスを生成
        self.pub = self.create_publisher(GpioMsg, self.SELFTOPIC, 10)
        # タイマーのインスタンスを生成（1秒ごとに発生）
        self.create_timer(1.0, self.callback)
        # カウンタをリセット
        self.count = 0
        # コンソールに表示
        self.get_logger().info("%s do..." % self.SELFNODE)

    def __del__(self):
        """
        デストラクタ
        """
        # コンソールに表示
        self.get_logger().info("%s done." % self.SELFNODE)

    def callback(self):
        """
        タイマーの実行部
        """
        self.get_logger().info("Publish [%s]" % (self.count))
        # 送信するメッセージの作成
        msg = GpioMsg()
        msg.port = 0
        msg.value = self.count
        # 送信
        self.pub.publish(msg)
        # カウンタをインクリメント
        self.count += 1


def main(args=None):
    """
    メイン関数
    Parameters
    ----------
    """
    try:
        # rclpyの初期化
        rclpy.init(args=args)
        # インスタンスを生成
        node = MyPublisher()
        # プロセス終了までアイドリング
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        # 終了処理
        node.destroy_node()
        rclpy.shutdown()


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

###受信側ノード

```python:Sub.py
#!/usr/bin/env /usr/bin/python3
# -*- coding: utf-8 -*-
# -----------------------------------------------
# ROS2 Node 受信側
#
# The MIT License (MIT)
# Copyright (C) 2019 myasu.
# -----------------------------------------------

import rclpy
from rclpy.node import Node
# カスタムメッセージ
from pubsubpy_mes.msg import GpioMsg


class MySubscription(Node):
    """
    受信側
    """

    # ノード名
    SELFNODE = "mysub"
    # トピック名
    SELFTOPIC = "mes_" + SELFNODE

    def __init__(self):
        """
        コンストラクタ
        Parameters
        ----------
        """

        # ノードの初期化
        super().__init__(self.SELFNODE)
        # コンソールに表示
        self.get_logger().info("%s initializing..." % (self.SELFNODE))
        # subscriptionインスタンスを生成
        self.sub = self.create_subscription(
            GpioMsg, "mes_mypub", self.callback, 10)
        # コンソールに表示
        self.get_logger().info("%s do..." % self.SELFNODE)

    def __del__(self):
        """
        デストラクタ
        """
        self.get_logger().info("%s done." % self.SELFNODE)

    def callback(self, message):
        """
        コールバック関数
        Parameters
        ----------
        message : gpio_mes
            メッセージ
        """
        # 受け取ったメッセージの表示
        self.get_logger().info('Subscription > Port: %d Value: %d' %
                               (message.port, message.value))


def main(args=None):
    """
    メイン関数
    Parameters
    ----------
    """
    try:
        # rclpyの初期化
        rclpy.init(args=args)
        # インスタンスを生成
        node = MySubscription()
        # プロセス終了までアイドリング
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        # 終了処理
        node.destroy_node()
        rclpy.shutdown()


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

##（４）ビルド

```sh:コマンドライン
#ワーキングディレクトリに戻ってからビルド
~/ros2_ws/src/node$ de ../..
~/ros2_ws$ colcon build --symlink-install
Starting >>> pubsubpy_mes
Finished <<< pubsubpy_mes [8.57s]                        
Starting >>> pubsubpy
Finished <<< pubsubpy [15.1s]            

Summary: 2 packages finished [25.8s]
#環境変数の読み込み
~/ros2_ws$ source ./install/setup.bash
#パッケージがインストールできたか確認
~/ros2_ws$ ros2 pkg list | grep pubsubpy
pubsubpy
pubsubpy_mes
ubuntu@raspi3u:~/ros2_ws$ 
```

##（５）実行

[Ctrl-C]で終了できます。

```sh:コマンドライン１（送信側）
~/ros2_ws$ ros2 run pubsubpy pub
[INFO] [mypub]: mypub initializing...
[INFO] [mypub]: mypub do...
[INFO] [mypub]: Publish [0]
[INFO] [mypub]: Publish [1]
[INFO] [mypub]: Publish [2]
[INFO] [mypub]: Publish [3]
[INFO] [mypub]: Publish [4]
[INFO] [mypub]: Publish [5]
[INFO] [mypub]: Publish [6]
[INFO] [mypub]: Publish [7]
[INFO] [mypub]: Publish [8]
[INFO] [mypub]: Publish [9]
[INFO] [mypub]: Publish [10]
^C[INFO] [mypub]: mypub done.
```

```sh:コマンドライン２（受信側）
~/ros2_ws$ ros2 run pubsubpy sub
[INFO] [mysub]: mysub initializing...
[INFO] [mysub]: mysub do...
[INFO] [mysub]: Subscription > Port: 0 Value: 0
[INFO] [mysub]: Subscription > Port: 0 Value: 1
[INFO] [mysub]: Subscription > Port: 0 Value: 2
[INFO] [mysub]: Subscription > Port: 0 Value: 3
[INFO] [mysub]: Subscription > Port: 0 Value: 4
[INFO] [mysub]: Subscription > Port: 0 Value: 5
[INFO] [mysub]: Subscription > Port: 0 Value: 6
[INFO] [mysub]: Subscription > Port: 0 Value: 7
[INFO] [mysub]: Subscription > Port: 0 Value: 8
[INFO] [mysub]: Subscription > Port: 0 Value: 9
[INFO] [mysub]: Subscription > Port: 0 Value: 10
^C[INFO] [mysub]: mysub done.
~/ros2_ws$ 
```

##（６）デバッグ
（作成中）

#おわりに

ROS2の勉強を19年7月から始めたのですが、やはりPython3のまとまった作例が少なかったので、覚え書きを兼ねてまとめを作ってみました。
参考になりましたら幸いです。