Go to Qiita Advent Calendar 2024 Top
LoginSignup
19
18

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@myasuinfukuoka.ex：福岡Elixirコミュ

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

Last updated at Posted at 2019-10-15

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

Python3ではじめるROS2 関連記事

回数 サブタイトル 内容
第1回 カスタムメッセージ編 ・環境の準備
・カスタムメッセージを作る
・（おまけ）.bashrcのカスタマイズ
第2回（今回） ノード編 ・Pub/Subのノードを作る
第3回 自動起動編 ・roslaunchから起動する
・systemdからroslaunchを起動する

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

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

##（１）準備

コマンドライン
~$ 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

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

###init.py

__init__.py

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

###package.xml

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

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ファイルを記述します。

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

##（３）ノードを作る

コマンドライン
~/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'」エラーが出ます

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

__init__.py

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

###送信側ノード

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()

###受信側ノード

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()

##（４）ビルド

コマンドライン
#ワーキングディレクトリに戻ってからビルド
~/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]で終了できます。

コマンドライン１（送信側）
~/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.
コマンドライン２（受信側）
~/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のまとまった作例が少なかったので、覚え書きを兼ねてまとめを作ってみました。
参考になりましたら幸いです。

19
18
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
19
18

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?