ROS2 publisher/subscriber node(+topic)の作成

ROS2でプログラミングするために勉強したメモです．
ここではのpublisher/subscriber node(+topic)作成方法を書きます．


## 準備

### workspaceを作成する．

workspaceの作成に関する詳細は[ココ](https://qiita.com/Dorebom/items/dbcefa143b8bc7f2a9c0)．

#### 1. メインのROS2をunderlayに指定して，ターミナルに反映(source)する．

```
source /opt/ros/dashing/setup.bash
```

#### 2. `dev_ws`という名前のworkspaceを作成する．

```
mkdir dev_ws
mkdir dev_ws/src
cd dev_ws
```

#### 3. 環境の依存関係を解決する．

今回は何も`git clone`してないのでやる必要はないけど，忘れないように手順に組み込んどく．

```
# (注意)workspaceのルートである，dev_wsフォルダで実行
sudo rosdep install -i --from-path src --rosdistro dashing -y
```

### packageを作成する．

packageの作成に関する詳細は[ココ](https://qiita.com/Dorebom/items/2f99fdb16106ee96f066)．

#### 1. `src`フォルダに移動する

```
cd src
```

#### 2. `py_pubsub`という名前のpython-packageを作成

```
ros2 pkg create --build-type ament_python py_pubsub
```
ここで，`ament_python`はビルドの形式である．

## publisher nodeの作成

#### 1. publisher nodeのpythonファイルを作成．

`dev_ws/src/py_pubsub/`に移動し，以下を実行．

```
touch publisher_member_function.py
```

#### 2. 以下を内容として記述．

```publisher_member_function.py
#
# public，privateの書き方の作法は反映していない．
#

# rclpyからnodeクラスをインポート．
import rclpy
from rclpy.node import Node
# topicに渡すデータ構造化のために文字列メッセージタイプをインポート
from std_msgs.msg import String

# nodeクラスを継承して，MinimalPublisherという名前のnodeクラスを作成．
class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        # キューサイズが10の'topic'という名前のtopicを介してpublishするpublisherを作成．
        self.publisher_ = self.create_publisher(String, 'topic', 10)
        # コールバック時間周期に使用する変数を0.5秒に設定．
        timer_period = 0.5  # seconds
        # タイマーを作成
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        # timer_callbackは，カウンター値が追加されたメッセージを作成する．
        msg = String()
        msg.data = 'Hello World: %d' % self.i
        self.publisher_.publish(msg)
        # get_logger().infoを使用してコンソールにpublishします。
        self.get_logger().info('Publishing: "%s"' % msg.data)
        self.i += 1


def main(args=None):
    # rclpyライブラリを初期化
    rclpy.init(args=args)
    # nodeを作成
    minimal_publisher = MinimalPublisher()
    # nodeが「スピン」してコールバックが呼び出される．
    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


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

#### 3. 依存関係にpackageを構成するファイルたちに追加． 

package作成時に自動生成された`setup.py`，`setup.cfg`および`package.xml`ファイルを編集する．

##### `package.xml`を編集

以下を編集．

```package.xml
<description>Examples of minimal publisher/subscriber using rclpy</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>
```

`<buildtool_depend>ament_python</buildtool_depend>`の後に以下を追加．

```package.xml
<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>
```
これは，コードが実行されるときに`rclpy`と`std_msgs`がこのpackageにとって必要であることを記述している．

##### `setup.py`を編集 

`package.xml`を編集したときと全く同じ内容で`setup.py`を編集

```setup.py
maintainer='YourName',
maintainer_email='you@email.com',
description='Examples of minimal publisher/subscriber using rclpy',
license='Apache License 2.0',
```

後ろの方にエントリーポイントの記述を追加．

```setup.py
entry_points={
        'console_scripts': [
                'talker = py_pubsub.publisher_member_function:main',
        ],
},
```

##### `setup.cfg`を編集

以下が自動的に生成されるので，特に編集することはない．

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

## subscriber nodeの作成

#### 1. subscriber nodeのpythonファイルを作成．

```
touch subscriber_member_function.py
```

#### 2. 以下を内容として記述．

```subscriber_member_function.py

# subscriber_nodeのコードは，publisher nodeのコードとほぼ同じです．

import rclpy
from rclpy.node import Node

from std_msgs.msg import String


class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('minimal_subscriber')
        # コンストラクターは，publisherと同じ引数でsubscriberを作成します．
        # publisherとsubscriberが使用するtopicの名前とメッセージタイプが一致する必要があるから．
        self.subscription = self.create_subscription(
            String,
            'topic',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning

    # コールバック定義は，受信したデータとともに，情報メッセージをコンソールに出力するだけ．
    def listener_callback(self, msg):
        self.get_logger().info('I heard: "%s"' % msg.data)


def main(args=None):
    rclpy.init(args=args)

    minimal_subscriber = MinimalSubscriber()

    rclpy.spin(minimal_subscriber)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()


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

[Note]
- subscriberのコンストラクターとコールバックにはタイマー定義が必要ないため、タイマー定義は書かない．
- コールバックは、メッセージを受信するとすぐに呼び出される．


#### 3. 依存関係をpackageを構成するファイルたちに追加．

subscriber nodeは，publisherと同じ依存関係があるため，`package.xml`と`setup.cfg`に新しく追加する項目はない．

##### `setup.py`の編集

エントリーポイントに`listener`の記述を追加．

```setup.py
entry_points={
        'console_scripts': [
                'talker = py_pubsub.publisher_member_function:main',
                'listener = py_pubsub.subscriber_member_function:main',
        ],
},
```

ここで，`talker`はpublisher nodeを作成する際に記述済み．

## ビルドと実行

### ビルド

#### 1. `dev_ws`フォルダに移動し，以下を実行．

```
sudo rosdep install -i --from-path src --rosdistro dashing -y
```

#### 2. `py_pubsub`packageをcolconでビルド．

```
colcon build --packages-select py_pubsub
```

### 実行

#### 1. 新しいターミナルを開き，`dev_ws`に移動して以下を実行．

```
. install/setup.bash
```

#### 2. talker nodeを実行．

```
ros2 run py_pubsub talker
```

#### 3. listener nodeを実行．

同様に，別ターミナルで`. install/setup.bash`を行い，nodeを実行．

```
ros2 run py_pubsub listener
```

##### 4. `Ctrl+C`で終了．




# 参考サイト
- https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Py-Publisher-And-Subscriber/