ROS　Pub＆Sub通信　Python編

概要

本投稿では、ROSの基本であるPub＆Sub通信についての解説と、Pythonによる簡単なPub＆Sub通信プログラムの実装を行います。

Pub＆Sub通信のスクリプトを記述する前に、ROSインストールと、catkin_wsの作成が必要です。今回、ソースはcatkin_ws/src/package_test/srcの中に保存する前提で記述します。

実行環境

本投稿は、以下の環境で実行しています。

Pub＆Sub通信とは

ROS（Robot Operating System）では、Pub&Sub通信という手法で、各プログラム間のメッセージのやり取りを行います。

下図のように、ROSではroscoreを介して、各プログラム間（ROSではノードと呼びます）で、変数（ROSではトピックと呼びます）のやり取りをしています。

これにより、例えばノードAはPython、ノードBはC++で記述したり、ノードBはGithubからクローンしたコード、ノードCは自作のスクリプト、というように様々な種類のプログラムでROSを実行することができます。

PythonによるPublisherの記述

まず、ROSのPublisherをPythonスクリプトで記述する場合は、下記のように記述します。

#!/usr/bin/env python
# coding: UTF-8

import rospy
import numpy as np
from geometry_msgs.msg import PoseWithCovarianceStamped

if __name__ == '__main__':
    rospy.init_node('publisher_test')

    pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size = 10)

    pose_test = PoseWithCovarianceStamped()
    pose_test.header.frame_id = 'map'
    pose_test.pose.pose.position.x = 10.0
    pose_test.pose.pose.position.y = 20.0
    pose_test.pose.pose.orientation.z = np.pi

    while not rospy.is_shutdown():

        pose_pub.publish(pose_test)

        rospy.sleep(0.2)

Publisherの解説

上記のスクリプトについて、詳細を１行ずつ解説します。

1・2行目で、スクリプトがPythonで記載されていることと、スクリプト内で日本語の記述を可能にしています。

また、import rospy によって、rosのPythonライブラリをインポートします。加えて、今回のPublisherの例では、数値計算ライブラリのnumpyと、rosのメッセージ型であるPoseWithCovarianceStampedをインポートしています。

#!/usr/bin/env python
# coding: UTF-8

import rospy
import numpy as np
from geometry_msgs.msg import PoseWithCovarianceStamped

メイン関数・ノード名の定義

メイン関数では、初めにノード名を定義しています。今回の例では、publisher_testというノード名としています。

if __name__ == '__main__':
    rospy.init_node('publisher_test')

Publisher・変数の定義

下の一番上の行が、Publisherの定義です。

rospy.Publisher(トピック名, 変数の型, バッファサイズ)の形で定義します。ここで重要なのは、ROSのPub＆Sub通信では、PythonでもC++でもソースを記述できる代わりに、各プログラム間でやり取りする変数の型は、あらかじめROSで決められた型を使用しなければならないということです。

ROSで使用できるメッセージの型は、公式HPに記載があります。ここでは、座標情報を格納できるPoseWithCovarianceStampedという型で、新たにpose_testという変数を宣言して、値を格納しています。

    pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size = 10)

    pose_test = PoseWithCovarianceStamped()
    pose_test.header.frame_id = 'map'
    pose_test.pose.pose.position.x = 10.0
    pose_test.pose.pose.position.y = 20.0
    pose_test.pose.pose.orientation.z = np.pi

whileループ・トピックのPublish

以下の記述で、rosが停止するまでwhileループを繰り返します。rospy.sleep(時間)で、指定した時間だけスリープして、whileループを繰り返します。

トピックのpublishは、定義したPublisher.publish(変数名)で行います。

    while not rospy.is_shutdown():

        pose_pub.publish(pose_test)

        rospy.sleep(0.2)

PythonによるSubscriberの記述

次に、Subscriber（受け取り側）の記載方法を解説します。

#!/usr/bin/env python
# coding: UTF-8

import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped

def pose_callback(msg_pose):
    global subscribed_pose
    subscribed_pose = msg_pose

if __name__ == '__main__':
    rospy.init_node('subscriber_test')

    pose_sub = rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, pose_callback)

    subscribed_pose = PoseWithCovarianceStamped()

    while not rospy.is_shutdown():

        rospy.loginfo(subscribed_pose)

        rospy.sleep(0.2)

Subscriberの解説

続いて、Subscriberの解説です。上から4行はPublisherと同じです。

Publisherと異なるのは、Subscriberではcallback関数を定義している点です。Subscribeしたいトピックが別のノードからPublishされる毎に、ここではmsg_poseという名前でcallback関数に入ります。

#!/usr/bin/env python
# coding: UTF-8

import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped

def pose_callback(msg_pose):
    global subscribed_pose
    subscribed_pose = msg_pose

Subscriberの定義

下記では、メイン関数内でSubscriberをpose_subという名前で宣言しています。Subscriberの書き方は、（トピック名, 変数名, callback関数名）の順に記述します。今回の例では、/initial_poseというトピックが別のノードからPublishされると、上で定義したcallback関数へと飛びます。

また、ここではsubscribed_poseという名前で、PoseWithCovarianceStamped型の変数を定義しています。

if __name__ == '__main__':
    rospy.init_node('subscriber_test')

    pose_sub = rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, pose_callback)

    subscribed_pose = PoseWithCovarianceStamped()

whileループ

このwhileループでは、rospy.loginfo(変数名)の記述で、Subscribeしたトピックを端末に表示させています。

    while not rospy.is_shutdown():

        rospy.loginfo(subscribed_pose)

        rospy.sleep(0.2)

作成したスクリプトを実行する

Pythonスクリプトに実行権限を付与する

Pythonで記述したコードを実行するには、まず実行権限を付与する必要があります。

試しにTerminalを開き、下記のコマンドを入力すると、作成したPythonスクリプトの色が白くなっています。これは、実行権限が無い状態となります。（下の写真では、ワークスペースはcatkin_ws12となっています。）

cd ~/catkin_ws/src/package_test/src

ls

ここで、下記コマンドを順番に実行して、chmodで実行権限を付与します。実行権限の違いは、こちらが参考になります。今回は、srcフォルダ内のスクリプト全てに読み書き実行権限を付与しています。

cd ..

sudo chmod 777 -R src

もう一度フォルダの中身を確認すると、スクリプトの色が緑色に変化して、実行権限が付与されていることを確認できます。

Terminalで実行する

実行権限が付与できたら、下のように端末を３つ立ち上げ、roscore、rosrun package_test publisher_test.py、rosrun package_test subscriber_test.pyをそれぞれ実行します。

rosrunの記述の仕方は、このように rosrun パッケージ名 スクリプト名の順で記述をします。

エラーが無く実行できれば、publisher_testでPublishしたトピックを、subscriber_testで受け取って表示することを確認できます。

rqt_graphによる確認

この状態で新たに端末を立ち上げ、下のようにrqt_graphを実行することで、各ノードとトピックのやり取りを可視化することができます。

このように、publisher_testからsubscriber_testにトピックの受け渡しができていることが確認できます。

まとめ

今回は、PythonによるROSのPub＆Sub通信について解説を行いました。

自作で様々な制御を作成する場合も、基本的には、このPub＆Sub通信をしながら変数を追加・編集していくこととなります。

次回はC++での記述方法について解説を行う予定です。