HARD2021:ルンバの位置をPythonプログラムで知ろう！

このページではルンバの位置を知るためのROSを使ったPythonプログラムを学びます。ここでいう位置とはROSのcreate_autonomyパッケージで計算したodometry(オドメトリ)情報です。オドメトリはロボットの車輪回転速度から移動量を求め自分の位置を計算する方法のことで、デッドレコニングともいいます。

Pythonプログラムを読む前にROSの基本を簡単に説明します。これらの知識はプログラムを読むうえで頭に入れておく必要があります。まず、ROSではノード(実行中のプログラム)間でいろいろなデータを通信することで、ロボットを動作させています。

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

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

オドメトリのトピックは/odomが一般的ですが、HARDワークショップで使っている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, _odom_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, _odom_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パッケージを作っていきましょう！
    • まず、odometryパッケージを格納するhard2021ディレクトリを作成。
      • $ mkdir -p ~/catkin_ws/src/hard2021
    • hard2021ディレクトリに移動。
      • $ cd ~/catkin_ws/src/hard2021
    • odometryパッケージの作成。依存パッケージとしてroscpp、rospy、std_msgsを指定します。
      • $ catkin_create_pkg odometry roscpp rospy std_msgs
  • 上手く作成できたか確認しましょう。
    • $ cd ~/catkin_ws/src/hard2021/odometry
    • $ ls
    • 下図のようにCMakeLIsts.txt, include, package.xml, srcができていれば成功。
      
• ソースコードの作成
  • エディタを開き、上の説明のついていない方のソースコードをコピペして保存する。以下のコマンドはエディタにgeditを使う場合。
    • $ cd ~/catkin_ws/src/hard2021/odometry/src
    • $ gedit odometry.py
• ビルド
  • Pythonはビルドする必要はないが、ROSで使用する場合は必要になる場合があるので、以下のコマンドを実行する。
    • $ cd ~/catkin_ws
    • $ catkin build odometry
  •  作成したpythonプログラムに実行権を与える。実行権を与えないとrosrunコマンドでノードを実行できない。
    • $ source ~/.bashrc
    • $ roscd odometry/src
    • chmodはファイルのアクセス権を変更するコマンド。ここでは、このファイルをユーザが実行できるように変更している。
      • $ 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
    • キーボードでロボットを動かすと、以下のようにオドメトリの値が変化していることがわかる。最上図で、赤線がx軸、緑線がy軸、青線がz軸となっており、碁盤の目の１目盛りが1mとなっている。オドメトリのthetaはロボットの姿勢で単位はradian。

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

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

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

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

ハンズオン

1. サンプルの実行
   • 上の説明に従って、サンプルプログラムを実行してみよう。
2. 走行距離の算出
   • サンプルプログラムではcallback_odom関数でRoombaのodometry情報を端末に表示しています。その関数を改造して、スタートしてからの走行距離を端末に表示するプログラムを作ってみよう。

終わり