ROS TutorialのPubliserとSubscriberを作る
Simple Wiki Based Contents Management System
ソフトウェア関連 >> RTコンポーネント関連 >> eSEAT2 >> eSEATでROSノードを作る >> ROS TutorialのPubliserとSubscriberを作る

ROS TutorialのPubliserとSubscriberを作る

eSEATでまずは簡単なROSノードを作成を行う。
eSEATにおけるROS対応をわかりやすくするために、ROS初心者向けチュートリアルにあるシンプルなPublisherとSubscriberをseatmlで記述していく。

Publisherを書く

ROSチュートリアルと同じく、まずはPublisherを作成する。チュートリアルで紹介されたPublisherの例である talker.pyは下記のとおりである。
 #!/usr/bin/env python
 # license removed for brevity
 import rospy
 from std_msgs.msg import String
 
 def talker():
    pub = rospy.Publisher('chatter', String, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    r = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        str = "hello world %s"%rospy.get_time()
        rospy.loginfo(str)
        pub.publish(str)
        r.sleep()

 if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException: pass

このプログラムと同等の機能を持つSEATMLファイル(talker.seatml)は、下のようになります。
 <?xml version="1.0" encoding="UTF-8"?>
 <seatml>
  <general name="talker" anonymous="1" rate="10">
    <adaptor name="/chatter" type="ros_pub" datatype="std_msgs/String" size="10" />

    <onexec>
      <script>
       hello_str = "hello world %s" % rospy.get_time()
       seat.loginfo(hello_str)
       seat.sendto("/chatter", hello_str)
      </script>
    </onexec>

  </general>

  <state name="main_mode">
  </state>
 </seatml>
このファイルは、SEATMLのほぼ最低限の構成になっています。
まず最初に、SEATMLファイルであることを宣言します。
 <?xml version="1.0" encoding="UTF-8"?>
 <seatml>
これは、XMLの標準的な宣言とメインとなる seatmlタグを記載しています。
次に、eSEATの基本要素となる名称やアダプタの宣言をgeneralタグを用いて記載しています。
  <general name="talker" anonymous="1" rate="10">
    <adaptor name="/chatter" type="ros_pub" datatype="std_msgs/String" size="10" />

    <onexec>
      <script>
       hello_str = "hello world %s" % rospy.get_time()
       seat.loginfo(hello_str)
       seat.sendto("/chatter", hello_str)
      </script>
    </onexec>

  </general>
genealタグの開始タグ
  <general name="talker" anonymous="1" rate="10">
では、ROSのノードの名称と周期実行のためのrospy.Rateオブジェクト生成用の宣言をname属性、anonymous属性、rate属性で行います。name属性は必須ですが、anonymous属性、rate属性は省略可能です。省略された場合には、anonymous="0", rate="1"と同じになります。
generalタグで宣言する部分は、talker.pyのソースコードにおける下記の部分にあたります。
    rospy.init_node('talker', anonymous=True)
    r = rospy.Rate(10) # 10hz
generalタグの中では、 adaptorタグとonexecタグで、Publisheの作成とROSノードの周期実行部分の定義を行っています。
Publisherは、adaptorタグで生成します。
    <adaptor name="/chatter" type="ros_pub" datatype="std_msgs/String" size="10" />
name属性値は、topic名となり、datatypeでPublisherのデータ型を設定することができます。通常のROSプログラムでは、データ型のモジュールをインポートする必要がありますが、eSEATでは内部で自動行っています。
この例では、 std_msgs/String になっていますが、eSEATの内部では、
  import std_msgs.msg as std_msgs
  self.adaptors['/chatter']=Publisher('chatter', std_msgs.String, queue_size=10)
と同値のコードが実行されてモジュールのインポートも自動的にが実行されています。したがって、SEATMLのscriptタグでPythonのコードを書く場合には、メッセージ型には、モジュール名を付加する必要があります。
eSEATにおける周期処理は、onexecタグで設定することができます。generalタグ内で設定するとeSEATのどの状態であっても必ず実行されます。
talker.seatmlでは、周期実行部分は下記のように書くことができます
    <onexec>
      <script>
       hello_str = "hello world %s" % rospy.get_time()
       seat.loginfo(hello_str)
       seat.sendto("/chatter", hello_str)
      </script>
    </onexec>
eSEATでは、アダプタに対して出力を要求する場合には、sendtoメソッドで実行することができます。RosAdaptorでは、Publisherのみ publishメソッドの実行と同じです。
また、eSEATのロガーに出力する場合にが、loginfoメソッドが定義されていますので、上記のコードがほぼtalker.pyの
    while not rospy.is_shutdown():
        str = "hello world %s"%rospy.get_time()
        rospy.loginfo(str)
        pub.publish(str)
        r.sleep()
の部分と同値となります。
この例では、状態がmain_modeの一つしかありませんので、generalタグ内ではなく、stateタグ内でonexecを定義しても同じ振舞をします。その場合には、下記のように書くことができます。
 <?xml version="1.0" encoding="UTF-8"?>
 <seatml>
  <general name="talker" anonymous="1" rate="10">
    <adaptor name="/chatter" type="ros_pub" datatype="std_msgs/String" size="10" />
  </general>

  <state name="main_mode">
    <onexec>
      <script>
       hello_str = "hello world %s" % rospy.get_time()
       seat.loginfo(hello_str)
       seat.sendto("/chatter", hello_str)
      </script>
    </onexec>
  </state>
 </seatml>
seatmlファイルの実行は、eSEAT.py を使うのですが、OpenRTCのポートがない場合には、eSEAT_Node.py で実行することができます。したがって、talker.seatmlの実行は
  # python eSEAT_Node.py talker.seatml
で行います。

Subscriberを書く

次に、ROSチュートリアルにあるlistener.pyと同じ動作を行うSubscriberをSEATMLで記述していきます。
listener.pyは、下記のようになっていました。
   #!/usr/bin/env python
   import rospy
   from std_msgs.msg import String
   
   def callback(data):
       rospy.loginfo(rospy.get_caller_id()+"I heard %s",data.data)
       
   def listener():
   
       # in ROS, nodes are unique named. If two nodes with the same
       # node are launched, the previous one is kicked off. The 
       # anonymous=True flag means that rospy will choose a unique
       # name for our 'listener' node so that multiple listeners can
       # run simultaenously.
       rospy.init_node('listener', anonymous=True)
   
       rospy.Subscriber("chatter", String, callback)
   
       # spin() simply keeps python from exiting until this node is stopped
       rospy.spin()
           
   if __name__ == '__main__':
       listener()
このプログラムと同じ働きをするノードをSEATMLで作成していきます。
最初は、Publisherの時と同じように、SETAMLの宣言とノード情報です。ノード名は、listnerとしています。
 <?xml version="1.0" encoding="UTF-8"?>
 <seatml>
   <general name="listener" anonymous="1">
次にSubscriberのadaptorを設定します。adaptorは、generalタグ内に書く必要がありますので、上記とあわせると下のようになります。
 <?xml version="1.0" encoding="UTF-8"?>
 <seatml>
   <general name="listener" anonymous="1">
     <adaptor name="/chatter" type="ros_sub" datatype="std_msgs/String" />
   </general>
ここで、Subscriberには、topic名とデータ型のみを設定しています。ROSの通常の記述だとSubscriberを生成する場合には、callback関数を設定するのですが、eSEATでは、callbackの指定がない場合には、eSEAT_Code.onDataメソッドが呼び出されるようになります。
このonDataメソッドでは、stateタグ内のruleで記述されたスクリプトなどを実行することができます。
なお、adaptorタグでSubscriberを設定する場合には、callback属性やfile属性を設定することで、明示的にcallback関数を設定することもできます。
最後に、stateタグ内にSubscriberがデータを受信した場合に呼び出されるスクリプトを設定していきます。
   <state name="main_mode">
     <rule source="/chatter">
       <script>
         print(rospy.get_caller_id() + "I heard %s" % rtc_in_data)
         seat.loginfo( "I heard %s" % rtc_in_data)
       </script>
     </rule>
   </state>
eSEATでは、従来からruleタグでは、adaptorが受信したデータが文字列の場合には、内部のkeyタグで設定さたキーの文字列と比較して選択的な実行が可能なのですが、上記の例のようにruleタグにsource属性でadaptor名を指定することで、指定したadaptorがデータ受信した場合の振る舞いを指定することができます。
最終的に、listener.pyと同じ振舞を行うSuscriberノードは、下記のように書くことができます。
 <?xml version="1.0" encoding="UTF-8"?>
 <seatml>
   <general name="listener" anonymous="1">
     <adaptor name="/chatter" type="ros_sub" datatype="std_msgs/String" />
   </general>

   <state name="main_mode">
     <rule source="/chatter">
       <script>
         print(rospy.get_caller_id() + "I heard %s" % rtc_in_data)
         seat.loginfo( "I heard %s" % rtc_in_data)
       </script>
     </rule>
   </state>
 </seatml>

実行

eSEATでPublisherとSubscriberの機能をもつROSノードを実行してみます。
Publisherのところにも書きましたが、OpenRTM-aistを使わないのであれば、eSEAT_Node.pyで実行することができます。
Terminalウィンドウを3つ開き、それぞれroscore, talker, listenerを実行しましょう。
まずは、rocoreを実行します。
 # roscore
次に別のターミナルで、listenerを実行します。
 # python eSEAT_Node.py listener.seatml
最後に、talkerを実行します。
 # python eSEAT_Node.py talker.seatml
これで、listenerにtalkerから送信されているメッセージが出力されると思います。