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 が終わったらロボットを停止させます。
