【 rbx1翻译第七章、控制移动基座】第八节、使用里程计进行往返运动

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-20A basic demo of using the /odom topic to move a robot a given distanceor rotate through a given angle.Created for the Pi Robot Project: http://www.pirobot.orgCopyright (c) 2012 Patrick Goebel.  All rights reserved.This program is free software; you can redistribute it and/or modifyit under the terms of the GNU General Public License as published bythe Free Software Foundation; either version 2 of the License, or(at your option) any later version.5This program is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without even the implied warranty ofMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See theGNU 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, piclass OutAndBack():def __init__(self):# Give the node a namerospy.init_node('out_and_back', anonymous=False)# Set rospy to execute a shutdown function when exitingrospy.on_shutdown(self.shutdown)# Publisher to control the robot's speedself.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 variabler = rospy.Rate(rate)# Set the forward linear speed to 0.15 meters per secondlinear_speed = 0.15# Set the travel distance in metersgoal_distance = 1.0# Set the rotation speed in radians per secondangular_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 listenerself.tf_listener = tf.TransformListener()# Give tf some time to fill its bufferrospy.sleep(2)# Set the odom frameself.odom_frame = '/odom'# Find out if the robot uses /base_link or /base_footprinttry: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 typeposition = Point()# Loop once for each leg of the tripfor i in range(2):# Initialize the movement commandmove_cmd = Twist()# Set the movement command to forward motionmove_cmd.linear.x = linear_speed# Get the starting position values(position, rotation) = self.get_odom()x_start = position.xy_start = position.y# Keep track of the distance traveleddistance = 0# Enter the loop to move along a sidewhile distance < goal_distance and not rospy.is_shutdown():# Publish the Twist message and sleep 1 cycleself.cmd_vel.publish(move_cmd)r.sleep()# Get the current position(position, rotation) = self.get_odom()# Compute the Euclidean distance from the startdistance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2))# Stop the robot before the rotationmove_cmd = Twist()self.cmd_vel.publish(move_cmd)rospy.sleep(1)# Set the movement command to a rotationmove_cmd.angular.z = angular_speed# Track the last angle measuredlast_angle = rotation# Track how far we have turnedturn_angle = 0while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():# Publish the Twist message and sleep 1 cycleself.cmd_vel.publish(move_cmd)r.sleep()# Get the current rotation(position, rotation) = self.get_odom()# Compute the amount of rotation since the last loopdelta_angle = normalize_angle(rotation - last_angle)# Add to the running totalturn_angle += delta_anglelast_angle = rotation# Stop the robot before the next legmove_cmd = Twist()self.cmd_vel.publish(move_cmd)rospy.sleep(1)# Stop the robot for goodself.cmd_vel.publish(Twist())def get_odom(self):# Get the current transform between the odom and base framestry:(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")returnreturn (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 commandmove_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.xy_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 framestry:(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")returnreturn (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 cycleself.cmd_vel.publish(move_cmd)r.sleep()# Get the current position(position, rotation) = self.get_odom()# Compute the Euclidean distance from the startdistance = 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 cycleself.cmd_vel.publish(move_cmd)r.sleep()# Get the current rotation(position, rotation) = self.get_odom()# Compute the amount of rotation since the last loopdelta_angle = normalize_angle(rotation - last_angle)# Add to the running totalturn_angle += delta_anglelast_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.