目的
プログラミングによる配信と購読の実装方法を学びます．さらに，シミュレータ上のロボットをPythonにより制御する方法を習得します．なお，C++言語による配信と購読の実装方法についてはこちらのページを参照してください．
プログラミングによる配信と購読
前回はノード，トピック，メッセージについて学びました．その過程でROSのコマンドを実行することで，配信及び購読ができることを確認しました．ここでは，プログラムにより配信及び購読する方法を習得します．これにより，ロボットを自律的に動かすことが可能となります．
1. パッケージの作成
新たにパッケージを作成しましょう．以前に作成したワークスペース「practice_ws/src」の中に新しくパッケージ「turtlesim_practice」を作成します．依存パッケージとして下記を指定してください．(必要ないパッケージもありますが，今後使用する可能性があるので全てを指定してください．)
geometry_msgs message_generation rosconsole rospy roslib rostime std_msgs
パッケージの作成方法がわからない人はこちらのページを参照してください．
2. プログラミング
前回，ROSのコマンドを使用して配信及び購読することでシミュレータ上のロボット(亀)を動かしました．全く同じ内容をプログラムによって実現しましょう．パッケージ「turtlesim_practice」の中の「scripts」という名前のディレクトリを作り，「move_turtlesim.py」というファイルを作成し，下記のプログラムを記述します．なお，ROS Kineticを利用している方は，1行目の「python3」を「python」に書き換えてください．
#! /usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
class turtleSim:
def __init__(self):
rospy.init_node('move_turtlesim', anonymous=True)
self.twist_pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=1000)
rospy.Subscriber('turtle1/pose',Pose, self.poseCallback)
rospy.Timer(rospy.Duration(1.0), self.timerCallback)
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
self.twist_pub.publish(twist)
def poseCallback(self, pose):
rospy.loginfo("x:%f", pose.x)
def timerCallback(self, event):
self.setMoveVector(0.3, 5)
self.setMoveVector(-0.3, 5)
def setMoveVector(self, linear_x, cnt):
twist = Twist()
r = rospy.Rate(10)
twist.linear.x = linear_x
twist.angular.z = 0.0
for i in range(0, cnt):
self.twist_pub.publish(twist)
r.sleep()
if __name__ == '__main__':
try:
ts = turtleSim()
rospy.spin()
except rospy.ROSInterruptException: pass
3. 実行
プログラムの中身を理解する前に実行してみましょう．実行方法はC++でプログラミングした際と少し異なりますので下記の流れで実行してください．まずは作成したファイルに対して実行権限を与えます．
$ chmod u+x move_turtlesim.py
次にcatkin_makeによりビルドし，sourceコマンドにより環境変数を設定します．pythonはスクリプト型プログラミング言語ですので，プログラムの記述が誤っていてもビルドによってエラーは発見されません．これらの処理により，作成したパッケージがパソコン上のどこに存在するかを記憶させ，rosrunコマンドによりノードを実行させることができるようになります．
$ cd ../../../ $ catkin_make $ source devel/setup.bash
最後に下記のようにノードを起動します．pythonの場合には，拡張子「.py」が必要ですので忘れないように注意しましょう．
$ rosrun turtlesim_practice move_turtlesim.py
4. プログラムの内容
それでは，作成したプログラムの挙動を確認した上で，プログラムの中身を順番に見ていきましょう．
#! /usr/bin/env python3 import rospy from geometry_msgs.msg import Twist from turtlesim.msg import Pose
1行目はターミナルで「./move_turtlesim.py」の形式で実行できるようにするためのものです．
次の3行はimport文を使ってパッケージ等を読み込んでいます．pythonでプログラミングする場合には，rospyというパッケージを必ず読み込む必要があります．メッセージファイルを読み込む際には，「from パッケージ名.msg import メッセージ型」としてください．
class turtleSim:
def __init__(self):
rospy.init_node('move_turtlesim', anonymous=True)
self.twist_pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=1000)
rospy.Subscriber('turtle1/pose',Pose, self.poseCallback)
rospy.Timer(rospy.Duration(1.0), self.timerCallback)
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
self.twist_pub.publish(twist)
turtleSimというクラスを定義ています．「class」という単語の後に好きなクラス名を指定します．今回はturtleSimというクラス名としています．
次にコンストラクタを定義しています．pythonでは「def」で関数を定義します．「def」という単語の後に好きな関数名を指定します．classから始まる構文の中に「__init__」という関数が定義されていますが，これがコンストラクタとなりインスタンスを作成されると始めに実行される関数となります．「__init__」関数の中では，配信，購読，タイマーのオブジェクトを生成しています．重要な処理をしているので，もう少し詳細に見ていきましょう．
rospy.init_node('move_turtlesim', anonymous=True)
「move_turtlesim」という名前のノードを登録しています．pythonでプログラミングする際には始めに書く必要があります．
self.twist_pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=1000)
ノードがマスターに対して「Twist」型のメッセージでトピック「turtle1/cmd_vel」を配信するよう宣言しています．’ ‘の中にはトピック名を入力します．advertise関数の第三引数の1000は，配信するキューの大きさを表し，データを最大で1000個まで蓄積できることを意味します．
rospy.Subscriber('turtle1/pose',Pose, self.poseCallback)
指定したトピックを購読した際に呼び出すコールバック関数を指定しています．’ ‘の中にはトピック名を入力し，そのトピックに対応したメッセージの型を第二引数で与えます．サンプルプログラムでは，トピック「turtle1/pose」を購読するとposeCallback関数を実行します．
rospy.Timer(rospy.Duration(1.0), self.timerCallback)
指定された間隔で呼び出すコールバック関数です．Timer関数の第一引数であるDurationは呼び出す間隔を表します．rospy.Duration(1.0)とした場合には，1/1.0秒の間隔でtimerCallback()を呼び出します．
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
self.twist_pub.publish(twist)
メッセージの型「Twist」の各要素を初期化し，トピックを配信しています．初期化しない場合には想定していない値を配信する可能性があるため値を初期化しましょう．
def poseCallback(self, pose):
rospy.loginfo("x:%f", pose.x)
指定したトピックを購読する度にposeCallbackを実行します．今回は「turtle1/pose」というトピックを購読する度にloginfo()によりロボットのx座標を表示しています．
def timerCallback(self, event):
self.setMoveVector(0.3, 5)
self.setMoveVector(-0.3, 5)
タイマー関数で指定した間隔でtimerCallbackを実行します．今回は1秒に1回の間隔でtimerCallbackを実行します．
def setMoveVector(self, linear_x, cnt):
twist = Twist()
r = rospy.Rate(10)
twist.linear.x = linear_x
twist.angular.z = 0.0
for i in range(0, cnt):
self.twist_pub.publish(twist)
r.sleep()
x軸の並進速度を設定し，トピックを配信します．rospy.Rate(10)は，1秒間に10回動作するタイマーを意味します．r.sleep()で100ミリ秒スリープすることを表します．つまり，この関数では与えられた引数の値(0.3)でx軸の並進速度を設定し，100ミリ秒ごとに配信する処理をcnt回(今回は5回)繰り返しています．
if __name__ == '__main__':
try:
ts = turtleSim()
rospy.spin()
except rospy.ROSInterruptException: pass
main文です．try-except文によって例外処理を行なっています．まずはtryの中でturtleSimクラスのインスタンスを生成し，spin()でコールバック関数を読み込みます．コールバック関数を呼び出す際にはspin()を実行する必要がありますので注意してください．もしこの2行が何らかの原因で正常に実行できなかった場合には，exceptの中が実行されます．今回はpass文が実行されますが，このpass文は何も行わない関数です．pythonでは何も行わない場合にも何らかの記述が必要ですのでpass文を実行しています．
以上でプログラミングにより配信と購読することができます．前回に学んだノードやトピック，メッセージの情報をROSのコマンドで取得する方法と今回の内容で，ロボットを自律制御することが可能となります．
課題
1. setMoveVector関数の引数に与える値を変え，その際の挙動を予想しなさい．そして結果と比較しなさい．
2. 購読している全ての情報(ヒント : pose.x以外に4つの情報があります)をrospy.loginfo()により出力しなさい．
3. ロボットに正四角形の軌跡を描かせるプログラムを作成しなさい．
4. ロボットを座標(5,5)に移動させなさい．座標は購読した情報を使用すること．
5. トピック「/turtle1/color_sensor」を購読してrospy.loginfo()により出力しなさい．
6. ロボットが直進する際に，スムーズに加速，減速するような加減速制御を行いなさい．