ラズパイ２の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が点灯します。
