トピック通信の実装

Python言語を用いてトピック通信を実装する方法について学びます．

目的

ここまでにトピック通信の仕組み，ROS2によるパッケージやプログラミング方法を学びました．それら学んだ内容を活用し，簡易シミュレータturtlesimと作成したノード間のトピック通信をプログラミングにより実装します．これにより，ロボットのローレベルな制御が可能となります．

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

パッケージの作成

前回の講義で作成したワークスペース practice_ws の中に新たに turtlesim_practice パッケージを作成します．パッケージ作成時に与えるノードの名前は turtlesim_practice_node とします．パッケージの作成方法が分からない人は前回の講義資料を参考にしてください．

プログラミング

以前の講義でコマンドラインによってシミュレータ上のロボットを操作しました．同じ内容をプログラムで実現します．パッケージを作成した際に生成されるturtlesim_practice_node.pyに下記のコードを入力しましょう．

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42


import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose

class TurtleSim(Node):
    def __init__(self):
        super().__init__('turtlesim_practice_node')
        self.twist_pub = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)
        self.pose_sub = self.create_subscription(Pose, 'turtle1/pose', self.pose_callback, 10)
        self.timer = self.create_timer(1.0, self.timer_callback)

        self.is_forward = True

    def timer_callback(self):
        if(self.is_forward == True):
            self.set_move_vector(0.2)
        else:
            self.set_move_vector(-0.2)

        self.is_forward = not self.is_forward

    def set_move_vector(self, linear_x):    
        twist = Twist()
        twist.linear.x = linear_x
        twist.angular.z = 0.0
    
        self.twist_pub.publish(twist)        

    def pose_callback(self, pose):
        self.get_logger().info(f'x:{pose.x}')

def main(args=None):
    rclpy.init(args=args)
    node = TurtleSim()
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

実行

プログラムの中身を理解する前に実行してみましょう．実行方法は前回と同じですが，turtlesimシミュレータを起動しないと正しく動作しないので注意してください．正しく実行できるとロボットが前後運動し，ターミナルにx座標が出力されます．

シミュレータ画像

プログラムの内容

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

1
2
3
4


import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose

まずはプログラム冒頭のimport文です． 1行目のrclpyは，ROS2のpythonモジュールです． ROS2の機能を使用する場合には必ず必要なモジュールとなります． 2行目は，プログラム中でノードを作成するためにrclpy.nodeモジュールからNodeクラスをimportします． 3，4行目はトピック通信に必要なメッセージファイルをimportしています．書き方が少し難解ですが，from パッケージ名.msg import メッセージ型と書くと覚えましょう．

 6
 7
 8
 9
10
11
12


class TurtleSim(Node):
    def __init__(self):
        super().__init__('turtlesim_practice_node')
        self.twist_pub = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)
        self.pose_sub = self.create_subscription(Pose, 'turtle1/pose', self.pose_callback, 10)
        self.timer = self.create_timer(1.0, self.timer_callback)
        self.is_forward = True

TurtleSimというクラスを定義してます． classという単語の後に任意のクラス名を定義できます．次にコンストラクタを定義しています． pythonではdefで関数を定義します． classから始まる構文の中に__init__という関数が定義されています．これがコンストラクタとなりインスタンスを作成されると最初に実行される関数となります． __init__の中では，ノードの作成，配信，購読，タイマーのオブジェクトを生成しています．重要な処理をしているので詳しく見ていきましょう．

8

        super().__init__('turtlesim_practice_node')

この1行でturtlesim_practice_nodeという名前のノードを作成しています． Pythonでは，あるクラスを元にして同じ構造を持ちつつ拡張したクラスを定義することができます．元のクラスを基底クラス，拡張することをクラスの継承，継承したクラスを派生クラスと言います．基底クラスのNodeのコンストラクタを呼び出してクラスを継承しています．

9

        self.twist_pub = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)

create_publisherを用いて配信者(Publisher)を作成しています．第1引数はトピック通信で使用するメッセージの型，第2引数は文字列型で表すトピック名，第3引数は通信品質(QoS : Quality of Service)です． QoSプロファイルにより通信の状態に合わせて配信することもできます． QoSプロファイルは使用せず，デフォルトの10という値にしています．配信する際に1度バッファに格納し，バッファに格納された古いデータから順番に配信され，配信されたデータは破棄されます．問題なく通信できる環境であれば常に新しいデータが配信されることになりますが，ネットワークに遅延等の問題が発生している場合には，バッファにデータが溜まり続けます．

今回の場合は，Twistというメッセージの型を用いてturtle1/cmd_velという名前のトピックを配信する準備をしています．

10

        self.pose_sub = self.create_subscription(Pose, 'turtle1/pose', self.pose_callback, 10)

create_subscriptionを用いて購読者(Subscriber)を作成しています．第1引数はトピック通信で使用するメッセージの型，第2引数は文字列型で表すトピック名，第3引数はコールバック関数，第4引数は通信品質(QoS : Quality of Service)です．通信品質については先ほどと同じです．

上記の場合には，Poseというメッセージの型を用いたturtle1/poseという名前のトピックを購読した場合，pose_callback関数を実行することになります．

11

        self.timer = self.create_timer(1.0, self.timer_callback)

指定した間隔で動作するタイマーを作成しています． create_timerの第1引数は繰り返し間隔，第2引数は実行するコールバック関数です．上記の場合，1.0秒間隔でtimer_callback関数を実行することになります．

12

        self.is_forward = True

後ほど使用するフラグをTrueで初期化しています．このフラグは，ロボットが前進するか後退するかを表すフラグとなります．

15
16
17
18
19
20
21


    def timer_callback(self):
        if(self.is_forward == True):
            self.set_move_vector(0.2)
        else:
            self.set_move_vector(-0.2)

        self.is_forward = not self.is_forward

タイマーで指定した間隔でtimerCallback関数を実行します．先ほど初期化したis_forwardというフラグがTrueであれば，引数0.2を伴いset_move_vector関数を実行し， is_forwardがTrue以外であれば，引数-0.2を伴いset_move_vector関数を実行します． 21行目では，フラグis_forwardを反転させてます． TrueであったならばFalseに，FalseであったならばTrueになります．

23
24
25
26
27
28


    def set_move_vector(self, linear_x):    
        twist = Twist()
        twist.linear.x = linear_x
        twist.angular.z = 0.0
    
        self.twist_pub.publish(twist)  

並進速度と角速度を0としてトピックを配信します． linear_xには0.2もしくは-0.2が代入されているため，このトピックをロボットが購読するとロボットは前進もしくは後退します．

30
31


    def pose_callback(self, pose):
        self.get_logger().info(f'x:{pose.x}')

指定したトピックを購読する度にposeCallbackを実行します． pythonのprint関数のように，ROSにはターミナルに表示するためのメソッドが用意されています．少しわかりにくいですが，基底クラスNodeにてget_logger().info()が定義されています． infoの括弧内は，print関数と同じ書式で記述できます．今回はturtle1/poseというトピックを購読する度にロボットのx座標を表示しています．

print関数はターミナルに表示するだけですが，上記の書き方によりログファイルにも出力されます．規模が大きなシステムを構築する場合に，ログファイルからシステムの挙動を確認したいことがあります．そのような場合にはprint関数ではなく，上記の方法により記述した方が便利です． get_loggerでは，infoの他にwarningやerror，debug等が用意されています．

33
34
35
36
37
38
39
40
41
42


def main(args=None):
    rclpy.init(args=args)
    node = TurtleSim()
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

34行目で初期化し，35行目でturtleSimクラスのインスタンスを生成しています． rclpyはROS2のpythonモジュールですので，ROS2の機能を使う場合には必ず初期化が必要です． 36行目のspin関数により待ち状態(終了しない限り実行され続ける)が発生し，必要に応じでコールバック関数が呼び出されます． 38行目はノードを破壊し，39行目で終了処理を行います．

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

課題

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

トピック通信の実装

Tags:

Categories:

目的

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

パッケージの作成

プログラミング

実行

プログラムの内容

課題