ROSでの双方向通信を可能とするプログラム作成

はじめに

この記事では2つノード双方にPublisher/Subscriberを持たせた際の、双方向通信のプログラム作成を行います。

研究を行うにあたり双方向通信の記事を調べたのですが、
海外の記事だといくつかあるものの日本語ではなかったので多少価値があると思います。

環境

ROS : indigo
OS : Ubuntu14.04
言語 : python2.7

その他

今回私が紹介するROSにおける双方向通信ですが当然機能はします。
しかし明らかにプログラムが汚いです。
絶対改良可能ですので後輩に託します。

メッセージ作成

前回の記事を読んで頂いた方はメッセージファイルにcount変数を追加して、catkin_makeしていただくだけで結構です。

メッセージファイル作成

まずはプロジェクト内にmsgディレクトリの作成をします。

$ cd ~/ros_ws/src/ros_start
$ mkdir msg
$ vim Msg.msg

メッセージファイルに下記を記します。

int32 x
int32 y
int32 count

CMakeListsテキストファイルの変更

メッセージファイルを元にメッセージ型を定義するヘッダファイルの生成

$ cd ~/ros_ws/src/ros_start
$ vim CMakeLists.txt

もう1度言いますが前回の記事を読んでいただいた方で、メッセージファイル名も同じであれば必要ありません。

#該当意部分がはコメント解除して適宜修正
#7行目あたり
find_package(catkin REQUIRED COMPONENTS
    roscpp
    rospy
    std_msgs
+ message_generation
)

#45行目あたり
## Generate messages in the 'msg' folder
add_message_files(
     FILES
- #   Message1.msg
- #   Message2.msg
+    Msg.msg
 )

#66行目あたり
generate_messages(
     DEPENDENCIES
     std_msgs
)

#104行目あたり
catkin_package(
#　 INCLUDE_DIRS include
    LIBRARIES ros_start
    CATKIN_DEPENDS roscpp rospy std_msgs
    DEPENDS system_lib
)

終わったらcatkin_make

$ cd ~/ros_ws
$ catkin_make

プログラム作成

今回は解説のために変数がPublishされる度に、
変数xはインクリメント、変数yはディクリメントされるプログラムの作成を行います。

双方向通信なのでどちらのプログラムもほとんど同じものを利用することでシステム自体は動きます。

Server

初めにPublish開始するのはこのプログラムです。

Server.py

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

import rospy
from practice.msg import Msg
from time import sleep

def callback(msg):
    print('* Callback *')

    if(msg.count == 1): 
        print('-- Matching Count')
        x = msg.x
        y = msg.y
        x += 1
        y -= 1
        sleep(1)
        serverSend(x, y)

def serverRecv():
    print('* Server Subscriber *')
    rospy.Subscriber('yyy', Msg, callback)

def serverSend(x, y):
    print('* Server Publisher *')
    pub = rospy.Publisher('xxx', Msg, queue_size=100)
    r = rospy.Rate(1)

    msg = Msg()
    msg.x = x
    msg.y = y
    print('-- X : %d'%msg.x)
    print('-- Y : %d'%msg.y)

    for i in range(2):
        msg.count = i
        pub.publish(msg)
        r.sleep()
    print('-- Publish to Client')

def main():
    rospy.init_node('server', anonymous=True)

    x = 0
    y = 0

    serverSend(x, y)

    while not rospy.is_shutdown():
        try:
            serverRecv()
            rospy.spin()
        except rospy.ROSInterruptException:
            pass

if __name__ == '__main__':
    main()

解説

今回は関数ごとに解説を行いたいと思います。

初めにmainからです。
基本的なところは当然同じです。

簡単な流れとしては、
ノード宣言⇒変数代入⇒Publish⇒Subscribe⇒Publish...
です。

def main():
    rospy.init_node('server', anonymous=True)

    x = 0
    y = 0

    serverSend(x, y)

    while not rospy.is_shutdown():
        try:
            serverRecv()
            rospy.spin()
        except rospy.ROSInterruptException:
            pass

if __name__ == '__main__':
    main()

Publish⇒Subscribe⇒Publish⇒...
の部分を実現するためにwhileを用いて常Subscribeできる状態にします。

次にPublisherの関数です。

ServerからはxxxというTopicにメッセージをPublishします。

def serverSend(x, y):
    print('* Server Publisher *')
    pub = rospy.Publisher('xxx', Msg, queue_size=100)
    r = rospy.Rate(1)

    msg = Msg()
    msg.x = x
    msg.y = y
    print('-- X : %d'%msg.x)
    print('-- Y : %d'%msg.y)

    for i in range(2):
        msg.count = i
        pub.publish(msg)
        r.sleep()
    print('-- Publish to Client')

Publishする際、for文の中で実行します。
これが前述したプログラムの汚い部分です。

for i in range(2):
        msg.count = i
        pub.publish(msg)
        r.sleep()

双方向通信をするだけなので前回と違い1度Publishしたいので何故かfor文をなくしてpub.publishのみでいいのですが何故かそれだけではSubscribeしてくれません...

必ず2回以上Publishしないとダメなんです
したがって私はfor文で2回Publishすることで対処しました。
海外の方の記事を参考にすれば解決できると思います。

最後にSubscriberの関数です。
ServerのSubscriberはTopic名yyyを監視します。

def callback(msg):
    print('* Callback *')

    if(msg.count == 1): 
        print('-- Matching Count')
        x = msg.x
        y = msg.y
        x += 1
        y -= 1
        sleep(1)
        serverSend(x, y)

def serverRecv():
    print('* Server Subscriber *')
    rospy.Subscriber('yyy', Msg, callback)

Publisherのところで解説した通り、
for文で2回Publishしているため2回目にPublishされたメッセージのみSubscribe処理を入れます。

if(msg.count == 1): 
        print('-- Matching Count')
        x = msg.x
        y = msg.y
        x += 1
        y -= 1
        sleep(1)
        serverSend(x, y)

メッセージを変数に代入し、
xはインクリメント、yはディクリメント、
そしてクライアントに送り返します。

sleep(1)があるのは双方PublishされてSubscriberが起動されるまでに時間が生じます。
したがってPublishするのを1秒だけ遅らせて待ち時間を入れました。

Client

ほとんどServerとプログラム構成は同じなので解説は省きます。
関数名とmain関数が変わっているので注意してください。

client.py

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

import rospy
from practice.msg import Msg
from time import sleep

def callback(msg):
    print('* Callback *')

    if(msg.count == 1): 
        print('-- Matching Count')
        x = msg.x
        y = msg.y
        x += 1
        y -= 1
        sleep(1)
        clientSend(x, y)

def clientRecv():
    print('* Client Subscriber *')
    rospy.Subscriber('xxx', Msg, callback)

def clientSend(x, y):
    print('* Client Publisher *')
    pub = rospy.Publisher('yyy', Msg, queue_size=100)
    r = rospy.Rate(1)

    msg = Msg()
    msg.x = x
    msg.y = y
    print('-- X = %d'%msg.x)
    print('-- Y = %d'%msg.y)

    for i in range(2):
        msg.count = i
        pub.publish(msg)
        r.sleep()
    print('- Publish to Server')

def main():
    rospy.init_node('client', anonymous=True)

    while not rospy.is_shutdown():
        try:
            serverRecv()
            rospy.spin()
        except rospy.ROSInterruptException:
            pass

if __name__ == '__main__':
    main()

プログラム実行

roscore⇒client⇒server
の順に実行していただければ問題ないと思います。

おわりに

改善の余地しかないためまだまだ性能向上をできると考えられます。
英文読みまくって後輩頑張ってくれ