7.8 オドメトリを使ったOut and Backスクリプト

7.8.1 シミュレータによるオドメトリを使った Out and Back
7.8.2 実機によるオドメトリを使った Out and Back
7.8.3 オドメトリを使った Out-and-Back のPythonスクリプト

Out and Back スクリプトは7.6.2 Pythonスクリプトでロボットを動かしてみる（シミュレーション編）で使ったスクリプトです。

Out and Backはロボットに一定時間だけ速度を与え続けることで、目的の距離だけ移動します。

ここでは、ロボットのオドメトリ（自己位置）を利用してロボットを動かすことを考えます。オドメトリは必ずしも正確ではありませんが、速度を一定時間だけ決め打ち（？）で与えるよりは正確に移動できます。

ここでは、オドメトリを使ったodom_out_and_back.pyスクリプトを見ていきます。

1　#!/usr/bin/env python
2
3　import rospy
4　from geometry_msgs.msg import Twist, Point, Quaternion
5　import tf
6　from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
7　from math import radians, copysign, sqrt, pow, pi

geometry_msgsパッケージに含まれる、Twist,Point,Quaternionメッセージタイプを使えるようにする。

/odom と /base_link (あるいは /base_footprint) を変換するための tf メッセージタイプを使えるようにする。

quat_to_angleはquaternion を オイラー角 (yaw) に変換する関数、

normalize_angle はremoves the ambiguity between 180 and -180 degrees as well as 0 and 360 degreesする関数で、いずれもROS by Exampleのサンプルコードに含まれる。

9　　class OutAndBack():
10 　　def __init__(self):
11 　　　# Give the node a name
12 　　　rospy.init_node('out_and_back', anonymous=False)
13
14 　　　# Set rospy to execute a shutdown function when exiting
15 　　　rospy.on_shutdown(self.shutdown)
16
17 　　　# Publisher to control the robot's speed
18 　　　self.cmd_vel = rospy.Publisher('/cmd_vel', Twist)
19
20 　　　# How fast will we update the robot's movement?
21 　　　rate = 20
22
23 　　　# Set the equivalent ROS rate variable
24 　　　r = rospy.Rate(rate)

/cmd_velトピックにTwist型の型のメッセージ（コマンド？）を送る宣言をします。 一秒間に20回のタイミングでコマンドをアップデートします。

26 　　　# Set the forward linear speed to 0.2 meters per second
27 　　　linear_speed = 0.2
28
29 　　　# Set the travel distance in meters
30 　　　goal_distance = 1.0
31
32 　　　# Set the rotation speed in radians per second
33 　　　angular_speed = 1.0

直進速度は秒速0.2メートルでゴールまでの距離は1メートル。

回転角度は1秒間に1ラジアン。

35 　　　# Set the angular tolerance in degrees converted to radians
36 　　　angular_tolerance = radians(2.5)

angular_tolerance は角度の許容誤差をラジアンで表した変数です。

実機のロボットは目的角度にピッタリ止めるのは難しく、目的角度周辺で振動してしまいます（行き過ぎて、逆回転して、戻りすぎて、逆回転して…）。

そこで、angular_toleranceに入ったらOKにしましょう、という値です。

38 　　　# Set the rotation angle to Pi radians (180 degrees)
39 　　　goal_angle = pi

回転角度の目標値は180度です。

41 　　　# Initialize the tf listener
42 　　　self.tf_listener = tf.TransformListener()

TransformListenerオブジェクトを生成する。

44 　　　# Give tf some time to fill its buffer
45 　　　rospy.sleep(2)

tfはバッファにデータが溜まるのに時間がかかるので、2秒スリープする。

47 　　　# Set the odom frame
48 　　　self.odom_frame = '/odom'
49

ロボットの位置・姿勢を取得するために、 /base_footprint あるいは /base_link を /odm フレームの変換する。

TurtleBot の場合は/base_footprint フレームを使う。

Pi Robot や Maxwell の場合は /base_link フレームを使う。

50 　　　# Find out if the robot uses /base_link or /base_footprint51　　　 try:
52 　　　　self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
53 　　　　self.base_frame = '/base_footprint'
54 　　　except (tf.Exception, tf.ConnectivityException, tf.LookupException):
55 　　　　try:
56 　　　　　self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
57 　　　　　self.base_frame = '/base_link'
58 　　　　except (tf.Exception, tf.ConnectivityException, tf.LookupException):
59 　　　　　rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
60 　　　　　rospy.signal_shutdown("tf Exception")

try:

   /base_footprint が見つかるか？

except:

  try:

    /base_link が見つかるか？

  except:

    どちらも見つからないので、エラーを出力して終了

self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))

waitForTransform()には４つの引数がある:

　取得したい変換のfromにあたるframe（ここでは /odm）

　取得したい変換のtoにあたるframe（ここでは /base_footprint）

　指定する時間

　時間切れ: この最大の期間よりは長く待たない（ここでは1.0秒）

/odmとbase_footprint'との間のtransformが利用可能になるか、transformが利用可能でない場合、時間切れが来るまでブロックします。

self.base_frame = '/base_footprint'

以降はbase_frame を使います。

62 　　　# Initialize the position variable as a Point type
63 　　　position = Point()

ロボットのポジジョンの為の変数を初期化します

66 　　　for i in range(2):

スクリプトでは同じことを2回繰り返します（1st legと2nd leg） それぞれのlegでは、ロボットは1メートル進んだあとに180度回転します。

67 　　　　# Initialize the movement command
68 　　　　move_cmd = Twist()

ロボットに移動速度を指示する変数を初期化します

70 　　　　# Set the movement command to forward motion
71 　　　　move_cmd.linear.x = linear_speed

ロボットに移動速を指示する変数に直進速度を代入します

73 　　　　# Get the starting position values
74 　　　　(position, rotation) = self.get_odom()
75
76 　　　　x_start = position.x
77 　　　　y_start = position.y

現在のオドメトリの値を取得して、初期位置として設定します。

self.get_odom() 関数を先に説明します。（113行目から定義されています）

130 def get_odom(self):
131 　# Get the current transform between the odom and base frames
132 　try:
133 　　(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
134 　except (tf.Exception, tf.ConnectivityException, tf.LookupException):
135 　　　rospy.loginfo("TF Exception")
136 　　　return
137
138 　return (Point(*trans), quat_to_angle(Quaternion(*rot)))

オドメトリとベースフレームの現在の変換を見るためにtf_listenerを使います。
　(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
tf_listener.lookupTrasform 関数によりtransとrot（位置と姿勢）が求められたら、Point型の位置情報とQuaternion型の姿勢情報を返します。

transとrotの前に付いている*記号はPythonスクリプトの記法で、transではx座標, y座標, z座標 、rotではx,y,z,wのQuaternion型のリストデータを返します。

詳しくはPython言語の解説書を御覧ください。

79 　　　　# Keep track of the distance traveled
80 　　　　distance = 0
81
82 　　　　# Enter the loop to move along a side
83 　　　　while distance < goal_distance and not rospy.is_shutdown():
84 　　　　　# Publish the Twist message and sleep 1 cycle
85 　　　　　self.cmd_vel.publish(move_cmd)
86
87 　　　　　r.sleep()
88
89 　　　　　# Get the current position
90　　　　　 (position, rotation) = self.get_odom()
91
92 　　　　　# Compute the Euclidean distance from the start
93 　　　　　distance = sqrt(pow((position.x - x_start), 2) +
94 　　　　　　　　　　　　　　 pow((position.y - y_start), 2))

83行目から94行目までのループでロボットは1メートル進みます。

Twistメッセージ（速度の指示）をパブリッシュしたら、必ず一定時間（ここでは1サイクル）スリープを入れるのを忘れないで下さい。

96 　　　　# Stop the robot before the rotation
97 　　　　move_cmd = Twist()
98 　　　　self.cmd_vel.publish(move_cmd)
99 　　　　rospy.sleep(1)

直進移動が終わったあと、回転する前にロボットを一時停止します。

停止した後に、1秒間スリープします。

101 　　　 # Set the movement command to a rotation
102 　　　 move_cmd.angular.z = angular_speed
104   # Track the last angle measured
105   last_angle = rotation

回転速度を設定して、現在の姿勢を設定します。

107   # Track how far we have turned
108   turn_angle = 0
109
110 　while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
111　　　# Publish the Twist message and sleep 1 cycle
112 　　self.cmd_vel.publish(move_cmd)
113 　　r.sleep()
114
115 　　# Get the current rotation
116 　　(position, rotation) = self.get_odom()
117
118 　　# Compute the amount of rotation since the last loop
119 　　delta_angle = normalize_angle(rotation - last_angle)
120
121 　　# Add to the running total
122 　　turn_angle += delta_angle
123 　　last_angle = rotation

オドメトリを監視しながら、ロボットが指定の姿勢（角度）になるまで回転させます。

125 　# Stop the robot before the next leg
126 　move_cmd = Twist()
127 　self.cmd_vel.publish(move_cmd)
128 　rospy.sleep(1)
129
130 # Stop the robot for good
131 self.cmd_vel.publish(Twist())

ROS by Example本では64ページの’プログラムのリストの125行目から131行目までのインデントが間違っているので注意。

付録のサンプルプログラムは正しいのでちゃんと動きます。

ここでは回転が終わったら次のlegの準備の為にロボットを静止させて、1秒間スリープします。

さらに1st、2nd のleg が終わったらロボットを停止させます。