この記事はTurtlebot2 (Kobuki)を使ったROS新人プログラム用の記事です。今回はROSの通信方式であるトピックを学びます。
1. Publisher
キーボードからロボットを操縦するmy_teleopパッケージを作ろう!
はじめてのROSプログラミング(python)と同じ要領でmy_teleopパッケージを作ります。今回の例ではhelloをmy_teleopに置き換えます。このプログラムはメッセージの送リ手であるpublisher（配信者)プログラムの簡単な例にもなっています。プログラムの説明はソースコード内のコメントを参照してください。テキストエディタ(gedit)にコピペして名前を付けて保存します。
保存ディレクトリは~/catkin_ws/src/my_teleop/srcの中です。
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy # rosで必要はモジュール
from geometry_msgs.msg import Twist
from std_msgs.msg import String
def teleop():
rospy.init_node('my_teleop') # ノードの初期化
pub = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=1)
rate = rospy.Rate(30) # ループの周期。この場合は30Hz、1秒間に30回。
vel = Twist()
print("f: forward, b: backward, r: right, l:left")
while not rospy.is_shutdown():
key = input() # 標準入力からキーを読み込む
print(key) # 読み込んだキーの値を標準出力へ出力
if key == 'f': # fキーが押されていたら
vel.linear.x = 0.5
elif key == 'b':
vel.linear.x = -0.5
elif key == 'l':
vel.angular.z = 1.0
elif key == 'r':
vel.angular.z = -1.0
# linear.xは前後方向の並進速度(m/s)。前方向が正。
# angular.zは回転速度(rad/s)。反時計回りが正。
else:
print("Input f, b, l, r")
pub.publish(vel) # 速度指令メッセージをパブリッシュ
vel.linear.x = 0.0 # 並進速度の初期化
vel.angular.z = 0.0 # 回転速度の初期化
rate.sleep() # 指定した周期でループするよう寝て待つ
if __name__ == '__main__':
teleop()
rospy.spin()
以下のコマンドでビルドして実行しよう。
$ cd ~/catkin_ws
$ catkin build
$ roscore
$ rosrun my_teleop my_teleop.py
2. Subscriber
次に、メッセージの受け手であるsubscriber（購読者)の簡単なプログラムを示します。このプログラムはmy_teleop_nodeがパブリッシュするトピック/cmd_vel_mux/input/teleopをサブスクライブして、並進速度(Linear Velocity)と角速度(Angular Velocity)を標準出力に出力する簡単なプログラムです。
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy # rosで必要はモジュール
from geometry_msgs.msg import Twist
def callback(vel):
rospy.loginfo("Liner:%f",vel.linear.x)
rospy.loginfo("Angular:%f",vel.angular.z)
def subscriber():
rospy.init_node('my_subscriber', anonymous=True) # ノードの初期化
# subscriberの作成。トピック/cmd_vel_mux/input/teleopを購読する。
rospy.Subscriber("/cmd_vel_mux/input/teleop", Twist, callback)
# コールバック関数を繰り返し呼び出す。
rospy.spin()
if __name__ == '__main__':
subscriber()
ここで、馴染みのないコールバック関数callbackが登場しています。コールバック関数はIT用語辞典によると「コールバック関数とは、プログラム中で、呼び出し先の関数の実行中に実行されるように、あらかじめ指定しておく関数。」と定義されています。
通常はマウスで線を描くなどの処理を実装するような、あるイベントが起きた時に処理をするプログラムを実装する場合に使われます。通常は、マウスのイベント処理などの場合のようにマウスのボタンが押されたり、移動したときに自動的にコールバック関数が呼び出されますが、ROSではrospy.spin()で明示的に呼び出さなければいけません。rospy.spin()はノードが動いている間、コールバック関数を呼び続けます。ROSではメッセージの受け渡しにコールバック関数を使います。
では、今までパッケージを作った要領で、my_subscriberパッケージを作りましょう。
~/catkin_ws/src/my_subscriberディレクトリの下にCMakeLists.txt, package.xml, src
などを作ることになります。上のプログラムのファイル名はmy_subscriber.pyとなります。
以下のコマンドでビルドして実行しよう。
$ cd ~/catkin_ws
$ catkin build
$ rosrun my_subscriber my_subscriber.py
3. シミュレータでの確認
turtlebot2シミュレータを起動し、my_teleop_nodeとmy_subscriber_nodeの動作を確認しましょう。シミュレータを次のコマンドで起動します
$ roslaunch turtlebot_gazebo turtlebot_world.launch
次にmy_teleopノードを起動した端末にマウスカーソルを持って行いきます。
f, b, l, rキーでロボットが移動したら成功。キーを押した後はエンターキーを押す。
このシミュレータの場合は、/cmd_vel_mux/input/teleop_velをPublishするだけでロボットがその速度で移動します。
次にSubscriberのプログラムが動作しているか確認するために、my_subscriberノードを起動した端末を見ましょう。速度が表示されていたら成功です。
お疲れ様！
終わり
4. ホームワーク
- キーsを押すと、turtlebot2が停止するようにmy_teleop.pyのソースコードを変更しよう！
- my_teleop.pyではキーボードのf, b, r, lを押した後にエンターキーを押すと、一定の速度でturtlebot2が動き出します。キーを押すたびに並進速度が0.01 [m/s]ずつ増減、回転角度が0.1 [rad/s]ずつ変化するようにソースコードを変更しよう！
- turtlebot2の移動する軌跡が正方形になるようプログラムを変更しよう！