HARD2020：ルンバの位置をPythonで知る

Pythonプログラムでルンバの位置を知ろう！ここでの位置はROSで計算したodometry（オドメトリ情報です。オドメトリはロボットの車輪回転速度から移動量を求め自分の位置を計算する方法のことです。ROSでは各ノード(実行中のプログラム)間でいろいろなデータをやりとりをすることで、ロボットを動作させています。

ROSの通信方式にはトピック通信とサービス通信があり、トピック通信は１対多の非同期通信で、メッセージ通信は１体１の同期通信です。ここではトピック通信を学びます。

トピック通信では送り手と受け手がTopic(トピック)とよばれる名前付きのバス(伝送路)を介してデータをやり取りしています。送り手のことをPublisher(配信者）、受け手のことをSubscriber(購読者)、データのことをメッセージとよびます。

オドメトリのトピックは/odomが一般的ですが、このワークショップで使っているcreate_autonomyパッケージでは/create1/odomとなっています。

ソースコード

では、さっそくソースコードを紹介します。たったの３７行です。

#!/usr/bin/env python
# -*- coding: utf-8 -*- 
import rospy 
from nav_msgs.msg import Odometry   
import tf
from tf.transformations import euler_from_quaternion

_odom_x, _odom_y, _odom_theta = 0.0, 0.0, 0.0

def callback_odom(msg):
    global _odom_x, _odom_y, _odon_theta 
    _odom_x = msg.pose.pose.position.x  
    _odom_y = msg.pose.pose.position.y  
    qx = msg.pose.pose.orientation.x
    qy = msg.pose.pose.orientation.y
    qz = msg.pose.pose.orientation.z
    qw = msg.pose.pose.orientation.w
    q = (qx, qy, qz, qw)
    e = euler_from_quaternion(q)
    _odom_theta = e[2] 
    rospy.loginfo("Odomery: x=%s y=%s theta=%s", _odom_x, _odom_y, _odom_theta)

def odometry():
    rospy.init_node('odometry')
    odom_subscriber = rospy.Subscriber('/create1/odom', Odometry, callback_odom)
    rospy.spin()

if __name__ == '__main__':
    odometry()

説明付きソースコード

次に説明付きソースコードを読んでプログラムを理解しよう。なお、このプログラムは上で説明したSubscriber（購読者）のプログラム例となっています。

#!/usr/bin/env python
# -*- coding: utf-8 -*- # 日本語を使うためのおまじない。
import rospy  # ROSでpython使う場合に必要
from nav_msgs.msg import Odometry   # オドメトリを使う場合は必要
import tf
from tf.transformations import euler_from_quaternion

# グローバル変数。先頭のアンダーバーはグローバル変数の目印。
# 0.0で初期化している。
_odom_x, _odom_y, _odom_theta = 0.0, 0.0, 0.0


# callback_odom関数
def callback_odom(msg):
    global _odom_x, _odom_y, _odon_theta # グローバル変数
    _odom_x = msg.pose.pose.position.x  # x座標 [m]
    _odom_y = msg.pose.pose.position.y  # y座標 [m]
    # クォーターニオン(4次元数) 姿勢を表す手法の一つ
    qx = msg.pose.pose.orientation.x
    qy = msg.pose.pose.orientation.y
    qz = msg.pose.pose.orientation.z
    qw = msg.pose.pose.orientation.w
    q = (qx, qy, qz, qw)
    # クォータニオンからオイラー角を取得
    e = euler_from_quaternion(q)
    _odom_theta = e[2] # e[0]:ロール角, e[1]：ピッチ角, e[2]：ヨー角 [rad]

    rospy.loginfo("Odomery: x=%s y=%s theta=%s", _odom_x, _odom_y, _odom_theta)


# odometry関数
def odometry():
    #  ノードの初期化。引数はノード名。ROSでは同じノード名のノードを複数作ることはできない。
    rospy.init_node('odometry')

    # サブスクライバー（購読者)の生成。一番目の引数はトピック名、２番目の引数はメッセージの型、
    # オドメトリのメッセージはOdometry型。３番目の引数はコールバック関数。
    # 新しいメッセージが来るたびにコールバック関数が自動的に呼び出される。
    # これは以下のrospy.spin()とは別に並列で実行される。
    odom_subscriber = rospy.Subscriber('/create1/odom', Odometry, callback_odom)

    # spin()はノードが終了するまで、このプロプグラムを終わらせないようにしている。
    # これがないとプログラムはすぐ終了しノードも死んでしまう。
    rospy.spin()



# このプログラムをモジュールとしてimportできるようにするおまじない。
# なお、モジュールとは他のプログラムから再利用できるようにしたファイルのこと。
if __name__ == '__main__':
    odometry()

実　行

パッケージの作成
- パッケージは次のcatkin_create_pkgコマンドで作ります。依存パッケージはそのパッケージを作るために必要なパッケージです。
  - catkin_create_pkg <パッケージ名> [依存パッケージ1] [依存パッケージ2]　[依存パッケージ3]
- では、次のコマンドでodometryパッケージを作りましょう！
  - $ cd ~/catkin_ws/src
  - $ mkdir hard2020
  - $ cd ~/catkin_ws/src/hard2020
  - $ catkin_create_pkg odometry roscpp rospy std_msgs
- 上手く作成できたか確認します。
  - $ cd ~/catkin_ws/src/hard2020/odometry
  - $ ls
- CMakeLIsts.txt, include, package.xml, srcができていれば成功。
ソースコードの作成
- エディタを開き、上のソースコードをコピペして保存する。以下のコマンドはエディタにgeditを使う場合。
  - $ cd ~/catkin_ws/src/hard2020/odometry/src
  - $ gedit odometry.py
ビルド
- Pythonはビルドする必要はないが、ROSで使用する場合は必要になる場合があるので、以下のコマンドを実行する。
  - $ cd ~/catkin_ws
  - $ catkin build odometry
- 作成したpythonプログラムに実行権を与える。実行権を与えないとrosrunコマンドでノードを実行できない。
  - - $ source ~/.bashrc
    - $ roscd odometry/src
    - $ chmod u+x odometry.py
実行
- シミュレータの起動
  - 端末を開き、以下のコマンドを実行してシミュレータGazeboを起動する。一番上図のルンバをベースにしたロボットが現れる。
    - $ roslaunch ca_gazebo create_empty_world.launch
- tutlebot_teleop_keyboardノードの実行
  - 端末をもう一つ開き、以下のコマンドを実行してGazebo上のルンバをキー入力により遠隔制御する。
    - $ roslaunch ca__tools keyboard_teleop.launch
- odometryノードの実行
  - 端末をさらに一つ開き、以下のrosrunコマンドによりodometryノードを実行する。rosrunコマンドの１番目の引数はノード名でディレクトリ名にもなっている。２番目の引数は実行するPythonプログラム。
    - $ rosrun odometry odometry.py
  - キーボードでロボットを動かすと、以下のようにオドメトリの値が変化していることがわかる。

ノードとトピックの可視化

では、以下のコマンドでノードやトピックを見てみよう。ノードは楕円で囲まれ中の文字列がノード名、矢印はデータの流れる方向で、その上の文字列がノード名です。
ここでは、/create1と書かれた四角枠の外の、楕円で囲まれたteleop_twist_keyboardノードとgazeboノードとodometryノードだけに注目します。gazeboノードはシミュレータで、odemetryノードへ/create1/odomトピックを送り、teleop_twist_keyboardノードから/create1/cmd_velトピックを受け取っていることがわかります。

トピックをコマンドで調べる

次のトピックを調べるコマンドを紹介します。
- $ rostopic
  - rostopicコマンドを実行すると使い方がわかります。その中で良く使うのは次のrostopic listです。使われているトピックがわかります。
- トピックの表示
  - $ rostopic list
- メッセージの表示
  - $ rostopic echo トピック名
  - 例として gazeboノードからは/create1/joint_statesトピックも出ているので、そのメッセージを見てみましょう。
  - /create1/joint_statesトピックは左右のロボット車輪の回転量[rad]がわかります。これを使うとロボットのオドメトリを計算できそうですね。なお、各トピックには連番と時間情報がついています。