ROS – rospy でトピックの publisher/subscriber を作成する

概要

ROS の Python API を使って、ノード間でトピックをやり取りする方法について解説します。

チュートリアル

パッケージの作成

ROS の Python API である rospy と標準的なメッセージを提供する std_msgs を依存ライブラリに指定し、パッケージを作成します。 そして、Python スクリプトを配置する scripts ディレクトリを追加します。

cd ~/catkin_ws/src
catkin_create_pkg python_pubsub rospy std_msgs
cd python_pubsub
mkdir scripts

publisher を作成する

トピックを送信するノード publisher.py を作成します。

touch scripts/publisher.py
chmod +x scripts/publisher.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import String


def main():
    # ノードを初期化する。
    rospy.init_node("publisher")

    # 送信者を作成する。
    pub = rospy.Publisher("chatter", String, queue_size=10)

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        # トピックを送信する。
        msg = "hello world {}".format(rospy.get_time())
        pub.publish(msg)

        rospy.loginfo("Message '{}' published".format(msg))

        # 0.1 秒スリープする。
        rate.sleep()


if __name__ == "__main__":
    main()

上記コードを解説します。

1 publisher という名前でノードを初期化します。

rospy.init_node("publisher")

2 型が std_msgs.msg.String で名前が chatter であるトピックの Publisher オブジェクトを作成します。

pub = rospy.Publisher("chatter", String, queue_size=10)

queue_size は publisher と subscriber 間でやり取りするトピックを記録しているキューのサイズです。キューは subscriber が受信する際にキューの一番古いものから取得し、削除されていきます。トピックが到着した際にキューがいっぱいの場合は一番古いトピックが削除して、新しいトピックがキューに追加されます。

3 0.1 秒起きにトピックを送信します。

rate = rospy.Rate(10) で 10Hz (1秒あたり10回) の Rate オブジェクトを作成し、ループの最後で Rate.sleep() を呼び出すことで、ループを 10Hz で回すことができます。sleep() は、ループ内のそれより前の処理を考慮して、必要な分だけスリープします。 ループの中では pub.publish(msg) でトピックを送信しています。

rate = rospy.Rate(10)
while not rospy.is_shutdown():
    # トピックを送信する。
    msg = "hello world {}".format(rospy.get_time())
    pub.publish(msg)

    rospy.loginfo("Message '{}' published".format(msg))

    # 0.1 秒スリープする。
    rate.sleep()

subscriber を作成する

トピックを送信するノード subscriber.py を作成します。

touch scripts/subscriber.py
chmod +x scripts/subscriber.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import String


def callback(msg):
    rospy.loginfo("Message '{}' recieved".format(msg.data))


def subscriber():
    # ノードを初期化する。
    rospy.init_node("subscriber")

    # 受信者を作成する。
    rospy.Subscriber("chatter", String, callback)

    # ノードが終了するまで待機する。
    rospy.spin()


if __name__ == "__main__":
    subscriber()

上記コードを解説します。

1 publisher という名前でノードを初期化します。

rospy.init_node("subscriber")

2 トピックを受信したときに実行する関数を作成します

引数にメッセージが渡されるので、トピックを受信した際に行う処理をここに書きます。

def callback(msg):
    rospy.loginfo("Message '{}' recieved".format(msg.data))

3 型が std_msgs.msg.String で名前が chatter であるトピックの Subscriber オブジェクトを作成します。第3引数はトピックを受信したときに呼び出されるコールバック関数を指定します。

rospy.Subscriber("chatter", String, callback)

3 プログラムが終了するまで待機します

このままではプログラムがそのまま終了してしまうので、rospy.spin() でプログラムが終了しないようにします。

launch ファイルを作成する

publisher 及び subscriber を起動する launch ファイルを作成します。

mkdir launch
touch launch/run.launch

<launch>
    <node name="publisher" pkg="python_pubsub" type="publisher.py" output="screen" />
    <node name="subscriber" pkg="python_pubsub" type="subscriber.py" output="screen" />
</launch>

これまでの作業で以下のディレクトリ構成になっているはずです。

python_pubsub/
├── CMakeLists.txt
├── launch
│   └── run.launch
├── package.xml
└── scripts
    ├── publisher.py
    └── subscriber.py

実行する

以下のコマンドを実行します。

roslaunch python_pubsub run.launch

publisher が送信したトピックを subscriber が受信している様子がわかります。

[INFO] [1629892976.352340]: Message 'hello world 1629892976.35' published
[INFO] [1629892976.452512]: Message 'hello world 1629892976.45' published
[INFO] [1629892976.453263]: Message 'hello world 1629892976.45' recieved
[INFO] [1629892976.552771]: Message 'hello world 1629892976.55' published
[INFO] [1629892976.553216]: Message 'hello world 1629892976.55' recieved
[INFO] [1629892976.652551]: Message 'hello world 1629892976.65' published
[INFO] [1629892976.653484]: Message 'hello world 1629892976.65' recieved
[INFO] [1629892976.752837]: Message 'hello world 1629892976.75' published
[INFO] [1629892976.752930]: Message 'hello world 1629892976.75' recieved

キューのサイズ

• queue_size=0: キューのサイズは無制限です。メモリが溢れてしまう恐れがあるため、非推奨です。
• queue_size=1: 常に最新のメッセージのみ保持します。
• queue_size=2, 3: 10hz ぐらいで送受信できる場合はこのぐらいの値が最適です。
• queue_size=10: キューが溢れてトピックが捨てられてしまうことを防ぎたい場合は大きめの値を設定します。

FIFO で古いものから取得する仕組みのため、キューのサイズが大きいと、トピックを送信する速度より受信する速度が遅い場合に、受信したメッセージが最新のものよりだいぶ古いものになってしまいます。そのようなケースでは、キューのサイズは 1~3 の小さい値に設定するのがよいでしょう。

送信するメッセージの作成

Publisher.publish() で送信しているトピックは std_msgs/String 型で、data という str 型の属性1つ持ちます。

string data

このオブジェクトに値を設定する方法として、次の3種類の方法があります。

1. オブジェクトを作成して、後から値を設定する

   msg = String("Hello World")

2. 位置引数で値を指定して、初期化する

   msg = String("Hello World")

3. キーワード引数で値を指定して、初期化する

   msg = String(data="Hello World")

2、3 のやり方の場合 publish の引数に直接指定することもでき、publish() 内でその引数で String オブジェクトを作成して送信されます。チュートリアルのコードでは、2のやり方で String 型のトピックを送信しています。

msg = "hello world {}".format(rospy.get_time())
pub.publish(msg)

latch を有効にする

キューは publisher と subscriber のコネクションが確立されて初めて作成されるため、subscriber が立ち上がるより前に publisher によって送信されたトピックは失われます。Publisher() のコンストラクタ引数に latch=True を指定しておくと、送信した最新のトピックを記録しておき、コネクションが確立した際に subscriber に送信されます。

pub = rospy.Publisher("chatter", String, queue_size=10, latch=True)

トピックを1つ受信するまで待機する

rospy.wait_for_message() はトピックを1回受信するまで待機する関数です。トピックを受信した場合、返り値で返します。 例えば、カメラ画像を送信するノードからとりあえず1枚画像を受信して、画像の大きさを知りたい場合などに利用できます。

受信したトピック = rospy.wait_for_message(トピック名, トピックの型)

そのトピックの subscriber が最低1つ現れるまで待機する

そのトピックの subscriber が最低1つ現れるまで待機したい場合、Publisher.get_num_connections() で接続数を取得できるので、その値が1以上になるまで rospy.sleep() することで実現できます。

# コネクションが1つ以上になるまで待機する。
while not pub.get_num_connections():
    rospy.sleep(0.1)

参考

• rospy_tutorials/Tutorials/WritingPublisherSubscriber – ROS Wiki
• rospy/Overview/Publishers and Subscribers – ROS Wiki
• ROSのトピック通信におけるPublisherとSubscriberの内部動作（概要） – Qiita