7.8 Out and Back Using Odometry (使用裡程計進行往返運動)
現在，我們了解了裡程表資訊是如何在ROS中表示的，我們可以更精确地在往返過程中移動機器人。 下一個腳本将監視/ odom和/ base_link坐标系之間的轉換報中的機器人的位置和方向，而不是根據時間和速度來猜測距離和角度。
Now that we understand how odometry information is represented in ROS, we can be more precise about moving our robot on an out-and-back course. Rather than guessing distances and angles based on time and speed, our next script will monitor the robot’s position and orientation as reported by the transform between the /odom and /base_link frames.
在rbx1_nav / nodes目錄中，新檔案為odom_out_and_back.py。 在檢視代碼之前，讓我們比較一下模拟器和真實機器人之間的結果。
The new file is called odom_out_and_back.py in the rbx1_nav/nodes directory. Before looking at the code, let’s compare the results between the simulator and a real robot.
7.8.1 Odometry-Based Out and Back in the ArbotiX Simulator （在ArbotiX Simulator中基于裡程表的往返運動）
如果您已經在運作模拟機器人，請先用Ctrl-C終止模拟，以便我們從頭開始測量裡程。 然後再次啟動模拟的機器人，運作RViz，然後運作odom_out_and_back.py腳本，如下所示：
If you already have a simulated robot running, first Ctrl-C out of the simulation so we can start the odometry readings from scratch. Then bring up the simulated robot again, run RViz , then run the odom_out_and_back.py script as follows:
roslaunch rbx1_bringup fake_turtlebot.launch
rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz
rosrun rbx1_nav odom_out_and_back.py
一個經典的結果如下所示：
A typical result is shown below:
如您所見，在沒有實體環境影響的理想模拟器中使用裡程計可以産生基本上完美的結果。 這應該不足為奇。 那麼，當我們在真實的機器人上嘗試時會發生什麼呢？
As you can see, using odometry in an ideal simulator without physics produces basically perfect results. This should not be terribly surprising. So what happens when we try it on a real robot?
7.8.2 Odometry-Based Out and Back Using a Real Robot (使用真正的機器人進行基于裡程表的往返運動)
如果您有TurtleBot或其他與ROS相容的機器人，則可以在現實世界中嘗試基于裡程表的往返腳本。
If you have a TurtleBot or other ROS-compatible robot, you can try the odometry based out-and-back script in the real world.
首先，請確定您終止所有正在運作的仿真。 然後啟動機器人的啟動檔案。 對于TurtleBot，您可以運作：
First make sure you terminate any running simulations. Then bring up your robot’s startup launch file(s). For a TurtleBot you would run:
roslaunch rbx1_bringup turtlebot_minimal_create.launch
（或者，如果您已經建立了一個啟動檔案來存儲校準參數，則可以使用自己的啟動檔案。）
(Or use your own launch file if you have created one to store your calibration parameters.)
確定您的機器人有足夠的工作空間-距離機器人至少1.5米，兩側至少一米。
Make sure your robot has plenty of room to work in—at least 1.5 meters ahead of it and a meter on either side.
如果您使用的是TurtleBot，我們還将運作odom_ekf.py腳本（包含在rbx1_bringup軟體包中），以便我們可以在RViz中看到TurtleBot的組合裡程表坐标系。 如果您不使用TurtleBot，則可以跳過此步驟。 此啟動檔案應在TurtleBot的筆記本電腦上運作：
If you are using a TurtleBot, we will also run the odom_ekf.py script (included in the rbx1_bringup package) so we can see the TurtleBot’s combined odometry frame in RViz . You can skip this if you are not using a TurtleBot. This launch file should be run on the TurtleBot’s laptop:
roslaunch rbx1_bringup odom_ekf.launch
如果您已經從上一個測試運作了RViz，則隻需取消選中Odometry顯示并選中Odometry EKF顯示，然後跳過以下步驟。
If you already have RViz running from the previous test, you can simply un-check the Odometry display and check the Odometry EKF display, then skip the following step.
如果尚未啟動RViz，請立即使用nav_ekf配置檔案在你的工作站上運作它。 該檔案隻是預選擇了 /odom_ekf話題，以顯示組合的裡程表資料：
If RViz is not already up, run it now on your workstation with the nav_ekf config file. This file simply pre-selects the / odom_ekf topic for displaying the combined odometry data:
rosrun rviz rviz -d `rospack find rbx1_nav`/nav_ekf.rviz
最後，像在模拟中一樣，啟動基于裡程表的往返腳本。您可以在工作站 或 使用SSH登陸後的機器人計算機上運作以下指令：
Finally, launch the odometry-based out-and-back script just like we did in simulation. You can run the following command either on your workstation or on the robot’s laptop after logging in with ssh :
rosrun rbx1_nav odom_out_and_back.py
這是在地毯上操作時我自己的TurtleBot的結果：
Here is the result for my own TurtleBot when operating on a low-ply carpet:
從圖檔中可以看到，結果比定時往返的情況要好得多。 實際上，在現實世界中的結果甚至比RViz中顯示的還要好。 （請記住，RViz中的裡程計箭頭不會與機器人在現實世界中的實際位置和方向完全對齊。）在此特定運作中，機器人最終距起始位置不到1厘米，隻有幾厘米偏離正确方向的角度。 當然，要獲得甚至如此好的結果，您将需要花費一些時間，如前所述，仔細校準機器人的裡程計。
As you can see from the picture, the result is much better than the timed out-and-back case. In fact, in the real world, the result was even better than shown in RViz . (Remember that the odometry arrows in RViz won’t line up exactly with the actual position and orientation of the robot in the real world.) In this particular run, the robot ended up less than 1 cm from the starting position and only a few degrees away from the correct orientation. Of course, to get results even this good, you will need to spend some time carefully calibrating your robot’s odometry as described earlier.
7.8.3 The Odometry-Based Out-and-Back Script (基于Odometry的往返腳本)
現在，這是完整的基于裡程表的往返腳本。 腳本中的注釋應該使腳本的意思表達已經很明确了。在列出所有代碼後，我們将對關鍵代碼行進行更詳細地描述。
Here now is the full odometry-based out-and-back script. The embedded comments should make the script fairly self-explanatory but we will describe the key lines in greater detail following the listing.
源連結： odom_out_and_back.py
Link to source: odom_out_and_back.py
#!/usr/bin/env python
""" odom_out_and_back.py - Version 1.1 2013-12-20
A basic demo of using the /odom topic to move a robot a given distance
or rotate through a given angle.
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.5
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
from math import radians, copysign, sqrt, pow, pi
class OutAndBack():
def __init__(self):
# Give the node a name
rospy.init_node('out_and_back', anonymous=False)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# How fast will we update the robot's movement?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.15 meters per second
linear_speed = 0.15
# Set the travel distance in meters
goal_distance = 1.0
# Set the rotation speed in radians per second
angular_speed = 0.5
# Set the angular tolerance in degrees converted to radians [tolerance:公差]
angular_tolerance = radians(1.0)
# Set the rotation angle to Pi radians (180 degrees)
goal_angle = pi
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
# Loop once for each leg of the trip
for i in range(2):
# Initialize the movement command
move_cmd = Twist()
# Set the movement command to forward motion
move_cmd.linear.x = linear_speed
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Keep track of the distance traveled
distance = 0
# Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current position
(position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distance = sqrt(pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
# Stop the robot before the rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Set the movement command to a rotation
move_cmd.angular.z = angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Stop the robot for good
self.cmd_vel.publish(Twist())
def get_odom(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
def shutdown(self):
# Always stop the robot when shutting down the node.
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
rospy.sleep(1)
if __name__ == '__main__':
try:
OutAndBack()
except:
rospy.loginfo("Out-and-Back node terminated.")
現在讓我們看一下腳本中的關鍵行。
Let’s now look at the key lines in the script.
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
我們将需要geometry_msgs包中的Twist，Point和Quaternion資料類型。 我們還将需要tf庫來監視/ odom和/ base_link（或/ base_footprint）坐标系之間的轉換。 transform_utils庫是一個小子產品，您可以在rbx1_nav / src / rbx1_nav目錄中找到，并且包含從TurtleBot軟體包中借來的一些便捷功能。 函數quat_to_angle将四元數轉換為歐拉角（偏航角），而normalize_angle函數則消除180度和-180度之間以及0度和360度之間的歧義。
We will need the Twist , Point and Quaternion data types from the geometry_msgs package. We will also need the tf library to monitor the transformation between the /odom and /base_link (or /base_footprint ) frames. The transform_utils library is a small module that you can find in the rbx1_nav/src/rbx1_nav directory and contains a couple of handy functions borrowed from the TurtleBot package. The function quat_to_angle converts a quaternion to an Euler angle (yaw) while the normalize_angle function removes the ambiguity between 180 and -180 degrees as well as 0 and 360 degrees.
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(2.5)
在這裡，我們定義了旋轉的angular_tolerance。 原因是使用真正的機器人很容易使旋轉過度，甚至微小的角度誤差也可能使機器人遠離下一個位置。 根據經驗，發現約2.5度的公差是可以接受的結果。
Here we define an angular_tolerance for rotations. The reason is that it is very easy to overshoot the rotation with a real robot and even a slight angular error can send the robot far away from the next location. Empirically it was found that a tolerance of about 2.5 degrees gives acceptable results.
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame,'/base_footprint', rospy.Time(),rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame,'/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
接下來，我們建立一個TransformListener對象來監視坐标系轉換。 請注意，tf需要一點時間來填充監聽器的緩沖區，是以我們添加了一個rospy.sleep(2)調用。 要擷取機器人的位置和方向，我們需要在/ odom架構與TurtleBot使用的/ base_footprint架構或Pi Robot和Maxwell使用的/ base_link架構之間進行轉換。 首先，我們測試/ base_footprint架構，如果找不到，我們測試/ base_link架構。 結果存儲在self.base_frame變量中，以便稍後在腳本中使用。
Next we create a TransformListener object to monitor frame transforms. Note that tf needs a little time to fill up the listener’s buffer so we add a call to rospy.sleep(2) . To obtain the robot’s position and orientation, we need the transform between the /odom frame and either the /base_footprint frame as used by the TurtleBot or the /base_link frame as used by Pi Robot and Maxwell. First we test for the /base_footprint frame and if we don’t find it, we test for the / base_link frame. The result is stored in the self.base_frame variable to be used later in the script.
for i in range(2):
# Initialize the movement command
move_cmd = Twist()
與定時往返腳本一樣，我們将行程分成兩部分執行，每一部分都是：首先将機器人向前移動1米，然後将其旋轉180度。
As with the timed out-and-back script, we loop through the two legs of the trip: first moving the robot 1 meter forward, then rotating it 180 degrees.
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
在每部分的起點，我們使用get_odom()函數記錄起點和方向。 接下來讓我們看一下，以便我們了解其工作原理。
At the start of each leg, we record the starting position and orientation using the get_odom() function. Let’s look at that next so we understand how it works.
def get_odom(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame,self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
get_odom()函數首先使用tf_listener對象查找裡程表和基礎坐标系之間的目前轉換。 如果查找有問題，我們将抛出異常。 否則，傳回平移的Point表示和旋轉的四元數表示。 trans和rot變量前面的
*
是Python的用于将數字清單傳遞給函數的符号，我們在這裡使用它是因為trans是x，y和z坐标的清單，而rot是x，y， z和w四元數分量。
The get_odom() function first uses the tf_listener object to look up the current transform between the odometry and base frames. If there is a problem with the lookup, we throw an exception. Otherwise, we return a Point representation of the translation and a Quaternion representation of the rotation. The * in front of the trans and rot variables is Python’s notation for passing a list of numbers to a function and we use it here since trans is a list of x, y, and z coordinates while rot is a list of x, y, z and w quaternion components.
現在回到腳本的主要部分：
Now back to the main part of the script:
# Keep track of the distance traveled
distance = 0
# Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current position
(position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2))
這隻是我們向前移動機器人直到機器人走過1.0米距離的循環。
This is just our loop to move the robot forward until we have gone 1.0 meters.
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep for 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
這是我們在腳本開頭附近設定的在允許的角度公差内旋轉180度的循環。
And this is our loop for rotating through 180 degrees within the angular tolerance we set near the beginning of the script.
7.8.4 The /odom Topic versus the /odom Frame ( /odom話題與 /odom坐标系)
讀者可能想知道為什麼我們在之前的腳本中使用TransformListener來通路裡程表資訊，而不是僅訂閱/ odom話題。 原因是/ odom主題上釋出的資料并不總是完整的。 例如，TurtleBot使用單軸陀螺儀來提供對機器人旋轉的額外估計。 通過robot_pose_ekf節點（在TurtleBot啟動檔案中啟動）将其與車輪編碼器的資料相結合，以獲得比單獨使用任何一個源更好的旋轉估計。
The reader may be wondering why we used a TransformListener in the previous script to access odometry information rather than just subscribing to the /odom topic. The reason is that the data published on the /odom topic is not always the full story. For example, the TurtleBot uses a single-axis gyro to provide an additional estimate of the robot’s rotation. This is combined with the data from the wheel encoders by the robot_pose_ekf node (which is started in the TurtleBot launch file) to get a better estimate of rotation than either source alone.
但是，robot_pose_ekf節點不會将其資料釋出回 /odom話題（/odom話題是為車輪編碼器資料保留的。） 反而，将其釋出在/ odom_combined話題上。 此外，資料不是作為Odometry消息釋出，而是作為PoseWithCovarianceStamped消息釋出。 但是，它确實釋出了我們所需的從/ odom坐标系到/ base_link（或/ base_footprint）坐标系轉換的資訊。 是以，使用tf監視/ odom和/ base_link（或/ base_footprint）坐标系之間的轉換通常比依賴/ odom消息話題更加安全。
However, the robot_pose_ekf node does not publish its data back on the /odom topic which is reserved for the wheel encoder data. Instead, it publishes it on the /odom_combined topic. Furthermore, the data is published not as an Odometry message but as a PoseWithCovarianceStamped message. It does however, publish a transform from the /odom frame to the /base_link (or /base_footprint ) frame which provides the information we need. As a result, it is generally safer to use tf to monitor the transformation between the /odom and /base_link (or /base_footprint ) frames than to rely on the /odom message topic.
在rbx1_bringup / nodes目錄中，您将找到一個名為odom_ekf.py的節點，該節點将/ odom_combined話題上的PoseWithCovarianceStamped消息以Odometry類型消息重新釋出在/ odom_ekf話題上。 這樣做是為了使我們可以在RViz中同時檢視/ odom和/ odom_ekf主題，以将TurtleBot的基于車輪的裡程表與包含陀螺儀資料的組合裡程表進行比較。
In the rbx1_bringup/nodes directory you will find a node called odom_ekf.py that republishes the PoseWithCovarianceStamped message found on the /odom_combined topic as an Odometry type message on the topic called /odom_ekf . This is done only so we can view both the /odom and /odom_ekf topics in RViz to compare the TurtleBot’s wheel-based odometry with the combined odometry that includes the gyro data.