プログラミングによるトピック通信の実装(Python版)

目的

プログラミングによる配信と購読の実装方法を学びます.さらに,シミュレータ上のロボットをPythonにより制御する方法を習得します.なお,C++言語による配信と購読の実装方法についてはこちらのページを参照してください.

プログラミングによる配信と購読

前回はノード,トピック,メッセージについて学びました.その過程でROSのコマンドを実行することで,配信及び購読ができることを確認しました.ここでは,プログラムにより配信及び購読する方法を習得します.これにより,ロボットを自律的に動かすことが可能となります.

1. パッケージの作成
新たにパッケージを作成しましょう.以前に作成したワークスペース「practice_ws」の中に新しくパッケージ「turtlesim_practice」を作成します.依存パッケージとして下記を指定してください.(必要ないパッケージもありますが,今後使用する可能性があるので全てを指定してください.)

geometry_msgs message_generation rosconsole rospy roslib rostime std_msgs

パッケージの作成方法がわからない人はこちらのページを参照してください.

2. プログラミング
前回,ROSのコマンドを使用して配信及び購読することでシミュレータ上のロボット(亀)を動かしました.全く同じ内容をプログラムによって実現しましょう.パッケージ「turtlesim_practice」の中の「scripts」という名前のディレクトリを作り,「move_turtlesim.py」というファイルを作成し,下記のプログラムを記述します.

#! /usr/bin/env python
# -*- coding: utf-8 -*-

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(0.1), 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.2, 10)
        self.setMoveVector(-0.2, 10)

    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, 10):
            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

次にcatkinmakeによりビルドし,sourceコマンドにより環境変数を設定します.pythonはスクリプト型プログラミング言語ですので,プログラムの記述が誤っていてもビルドによってエラーは発見されません.これらの処理により,作成したパッケージがパソコン上のどこに存在するかを記憶させ,rosrunコマンドによりノードを実行させることができるようになります.

$ cd ../../
$ catkin_make
$ source devel/setup.bash

最後に下記のようにノードを起動します.pythonの場合には,拡張子「.py」が必要ですので忘れないように注意しましょう.

$ rosrun turtlesim_practice move_turtlesim.py

4. プログラムの内容

それでは,作成したプログラムの挙動を確認した上で,プログラムの中身を順番に見ていきましょう.

#! /usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose

1行目はターミナルで「./move_turtlesim.py」の形式で実行できるようにするためのものです.2行目はUTF-8で書かれていることを示しています.pythonでプログラミングする場合にはこの2行が必要となりますので記述しましょう.
次の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(0.1), 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(0.1), self.timerCallback)

指定された間隔で呼び出すコールバック関数です.Timer関数の第一引数であるDurationは呼び出す間隔を表します.rospy.Duration(0.1)とした場合には,0.1分の1秒(100ミリ秒に1回)の間隔で指定した関数を呼び出します.

        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」というトピックを購読する度にROS_INFO関数によりロボットのx座標を表示しています.

    def timerCallback(self, event):
        self.setMoveVector(0.2, 10)
        self.setMoveVector(-0.2, 10)

タイマー関数で指定した間隔でtimerCallbackを実行します.今回は100ミリ秒に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, 10):
            self.twist_pub.publish(twist)
            r.sleep()

x軸の並進速度を設定し,トピックを配信します.ros::Rate loop_rate(10)は,1秒間に10回動作するタイマーを意味します.loop_rate.sleep()で100ミリ秒スリープすることを表します.つまり,この関数では与えられた引数の値(0.1)でx軸の並進速度を設定し,100ミリ秒ごとに配信する処理をcnt回(今回は10回)繰り返しています.

if __name__ == '__main__':

    try:
        ts = turtleSim()
        rospy.spin()
    except rospy.ROSInterruptException: pass

main文です.try-except文によって例外処理を行なっています.まずはtryの中でturtleSimクラスのインスタンスを生成し,ros::spin()でコールバックを待っています.コールバック関数を呼び出す際にはspin関数を実行する必要がありますので注意してください.もしこの2行が何らかの原因で正常に実行できなかった場合には,exceptの中が実行されます.今回はpass文が実行されますが,このpass文は何も行わない関数です.pythonでは何も行わない場合にも何らかの記述が必要ですのでpass文を実行しています.

以上でプログラミングにより配信と購読することができます.前回に学んだノードやトピック,メッセージの情報をROSのコマンドで取得する方法と今回の内容で,ロボットを自律制御することが可能となります.

課題

1. setMoveVector関数の引数に与える値を変え,その際の挙動を予想しなさい.そして結果と比較しなさい.
2. 購読している全ての情報(ヒント : pose->x以外に4つの情報があります)をROS_INFOにより出力しなさい.
3. ロボットに正四角形の軌跡を描かせるプログラムを作成しなさい.
4. ロボットを座標(5,5)に移動させなさい.座標は購読した情報を使用すること.
5. トピック「/turtle1/color_sensor」を購読してROS_INFOにより出力しなさい.
ヒント:色情報を購読するコールバック関数の引数の型は下記の通りです.
turtlesim::ColorConstPtr& color
6. ロボットが直進する際に,スムーズに加速,減速するような加減速制御を行いなさい.