GPIOをノード間通信で使う

ラズパイ２のI/Oポートを使うと、ROSの機能を、ボタンやLEDのような実際のハードを使って、とてもわかりやすく試すことが出来ます。これは案外ROS入門の良い教材になるのかもしれません。ということで、ちょっとこの辺を掘り下げておきたいと思います。

今回はノードを二つ作ります。デジタル入力の状態をbrinkcontというトピックでパブリッシュするdsensと、それをサブスクライブしてLEDをコントロールするbrinkerです。rqtでモニターするとこうなっています。

トピックbrinkcontのtypeはsensor_msgs/Joy、つまりゲームパッドで使われるトピックです。これは、ボタンの値を入れるint32と、アナログスティックの値を入れるfloat32の配列を受け渡しできるので、複数のデータを渡したいときに便利です。もっとも今回は１ビットだけの情報ですが。

dsensノードのpythonコードdsens.pyです。ポート9を入力に設定し、これが１か０かをJoy型のトピックbrinkcontのbuttons[0]フィールドに入れてパブリッシュします。ポートは数Kオームの抵抗で3.3Vにプルアップし、ボタンを押すとGNDに落ちるように配線します。

buttonsフィールドの要素はbuttons[0,0]とやって二つ定義していますが、うち一つしか使ってませんし、axesにいたっては定義していません。定義しないのはどうかなと思いましたが、基本、サブスクライブ側で使ってなければそれでいいようです。

#!/usr/bin/env python
import roslib
import rospy
import time
import wiringpi
import subprocess
from sensor_msgs.msg import Joy

#main
if __name__ == '__main__':
    ### init io port ###
    subprocess.check_call('gpio export 9 in',shell=True)
    ###
    rospy.init_node('dsens')
    rate = rospy.Rate(10.0)
    # create object from sensor_msgs/Joy
    s = Joy()
    s.buttons = [0,0] # Two button data
    s.header.frame_id = 'base_link' # not use
    # create publisher with topic 'brinkcont'
    pub = rospy.Publisher('brinkcont', Joy, queue_size=10)
    # io pin setting
    io = wiringpi.GPIO(wiringpi.GPIO.WPI_MODE_SYS)
    io.pinMode(9,io.INPUT)  # Setup pin 9 degital input

    while not rospy.is_shutdown():
        s.header.stamp = rospy.get_rostime()
        sens = io.digitalRead(9)
        if sens == 1:
            s.buttons[0] = 0
        else:
            s.buttons[0] = 1
        pub.publish(s)
        rate.sleep()
    print('\n\rStopped')

お次ぎはサブスクライブ側です。brinkerノードのpythonコードbrinker.pyです。これは前回のflash.pyに似ていますが、ポート11に赤、8に緑のLEDを接続しました。いずれも1で点灯するように配線します。このノードはサブスクライブしたトピックbrinkcontのbuttons[0]で赤、[1]で緑のLEDが点灯するようになっています。

#!/usr/bin/env python
import roslib
import rospy
import wiringpi
import subprocess
from sensor_msgs.msg import Joy

#callback
def joy_callback(new_joy):
    if new_joy.buttons[0] == 1:
        io.digitalWrite(11,1) # turn on red
    else:
        io.digitalWrite(11,0) # turn off red
    if new_joy.buttons[1] == 1:
        io.digitalWrite(8,1) # turn on green
    else:
        io.digitalWrite(8,0) # turn off green

#main
if __name__ == '__main__':
    ### init io port ###
    subprocess.check_call('gpio export 11 out',shell=True)
    subprocess.check_call('gpio export 8 out',shell=True)
    ###
    rospy.init_node('brinker')
    # io pin setting
    io = wiringpi.GPIO(wiringpi.GPIO.WPI_MODE_SYS)
    io.pinMode(11,io.OUTPUT)  # Setup pin 11 (red)
    io.pinMode(8,io.OUTPUT)  # Setup pin 8 (green)

    rospy.Subscriber('brinkcont' , Joy ,joy_callback) 

    rospy.spin()

テストするにはroscore、dsens.py、brinker.pyをそれぞれターミナルで起動した後、dsensがハンドルしているボタンを押すと、brinkerが応答して赤いLEDが点灯します。