【 rbx1翻译 第七章、控制移动基座】第六节、使用ROS节点发布Twist消息

7.6 Publishing Twist Messages from a ROS Node (使用ROS节点发布Twist消息)

到目前为止，我们一直在从命令行移动机器人，但是大多数时候，您将依靠ROS节点来发布适当的Twist消息。 举一个简单的例子，假设您要对机器人进行编程，使其向前移动一个1.0米，旋转180度，然后回到起点。 我们将尝试通过多种不同方式来完成此任务，这些方式将很好地说明ROS中运动控制的不同级别。
So far we have been moving our robot from the command line, but most of the time you will rely on a ROS node to publish the appropriate Twist messages. As a simple example, suppose you want to program your robot to move one 1.0 meters forward, turn around 180 degrees, then come back to the starting point. We will attempt to accomplish this task a number of different ways which will nicely illustrate the different levels of motion control in ROS.

7.6.1 Estimating Distance and Rotation Using Time and Speed (使用时间和速度估算距离和旋转)

我们的第一个尝试是使用定时的Twist命令将机器人向前移动一定距离，旋转180度，然后使用相同的时间和相同的速度再次向前移动，最终如愿的在开始的地方停止运动。 最后，我们再次将机器人再旋转180度以匹配原始方向。
Our first attempt will be to use timed Twist commands to move the robot forward a certain distance, rotate 180 degrees, then move forward again for the same time and at the same speed where it will hopefully end up where it started. Finally, we will rotate the robot 180 degrees one more time to match the original orientation.

可以在rbx1_nav / nodes子目录中的文件timed_out_and_back.py中找到该脚本。 在查看代码之前，让我们在ArbotiX模拟器中尝试一下。
The script can be found in the file timed_out_and_back.py in the rbx1_nav/nodes subdirectory. Before looking at the code, let’s try it in the ArbotiX simulator.

7.6.2 Timed Out-and-Back in the ArbotiX Simulator (在ArbotiX Simulator中定时往返)

为了确保将假的Turtlebot重新放置在起始位置，请使用Ctrl-C终止已经运行的假的Turtlebot启动文件，然后使用以下命令再次将其启动：
To make sure the fake Turtlebot is repositioned at the start location, use Ctrl-C to terminate the fake Turtlebot launch file if you already have it running, then bring it up again with the command:

roslaunch rbx1_bringup fake_turtlebot.launch

（如果需要，请用Pi Robot或您自己的机器人的那个文件替换fake_turtlebot.launch文件。这不会影响结果。）
(If desired, replace the fake_turtlebot.launch file with the one for Pi Robot or your own robot. It won’t make a difference to the results.)

如果RViz尚未运行，请立即启动它：
If RViz is not already running, fire it up now:

rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz

或单击“重置”按钮来清除以前教程中的所有里程箭头。 最后，运行timed_out_and_back.py节点：
or click the Reset button to clear any Odometry arrows from the previous section. Finally, run the timed_out_and_back.py node:

rosrun rbx1_nav timed_out_and_back.py

希望RViz将显示您的机器人执行一次往返动作，最终结果将类似于以下内容：
Hopefully RViz will show your robot executing an out-and-back maneuver and the end result will look something like the following:
大箭头表示机器人在其轨迹上各个点的位置和方向，如机器人（假的）内部里程计所报告的那样。 在接下来的几节中，我们将学习如何利用里程表信息。
The large arrows represent the position and orientation of the robot at various points along its trajectory as reported by the robot’s (fake) internal odometry. We will learn how to make use of this odometry information in the next few sections.

到目前为止，在理想的模拟器中一切看起来都不错。 但是，在实际的机器人上进行尝试之前，让我们看一下代码。
So far, things look pretty good in the ideal simulator. But before trying it out on a real robot, let’s look at the code.

7.6.3 The Timed Out-and-Back Script (定时往返的脚本)

这是定时往返节点的完整脚本。 整体列出后，我们将其细分为较小的部分。
Here is the full script for the timed out and back node. After the listing, we will break it down into smaller pieces.

源连接： timed_out_and_back.py
Link to source: timed_out_and_back.py

#!/usr/bin/env python

""" timed_out_and_back.py - Version 1.2 2014-12-14

    A basic demo of the using odometry data to move the robot along
    and out-and-back trajectory.

    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
from math import 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=1)
        
        # How fast will we update the robot's movement?
        rate = 50
        
        # Set the equivalent ROS rate variable
        r = rospy.Rate(rate)
        
        # Set the forward linear speed to 0.2 meters per second 
        linear_speed = 0.2
        
        # Set the travel distance to 1.0 meters
        goal_distance = 1.0
        
        # How long should it take us to get there?
        linear_duration = goal_distance / linear_speed
        
        # Set the rotation speed to 1.0 radians per second
        angular_speed = 1.0
        
        # Set the rotation angle to Pi radians (180 degrees)
        goal_angle = pi
        
        # How long should it take to rotate?
        angular_duration = goal_angle / angular_speed
     
        # Loop through the two legs of the trip  
        for i in range(2):
            # Initialize the movement command
            move_cmd = Twist()
            
            # Set the forward speed
            move_cmd.linear.x = linear_speed
            
            # Move forward for a time to go the desired distance
            ticks = int(linear_duration * rate)
            
            for t in range(ticks):
                self.cmd_vel.publish(move_cmd)
                r.sleep()
            
            # Stop the robot before the rotation
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
            
            # Now rotate left roughly 180 degrees  
            
            # Set the angular speed
            move_cmd.angular.z = angular_speed

            # Rotate for a time to go 180 degrees
            ticks = int(goal_angle * rate)
            
            for t in range(ticks):           
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                
            # Stop the robot before the next leg
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)    
            
        # Stop the robot
        self.cmd_vel.publish(Twist())
        
    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.")

由于这是我们的第一个脚本，因此让我们从顶部开始逐行解释：
Since this is our first script, let’s take it line-by-line starting from the top:

#!/usr/bin/env python
import rospy

如果您使用Python做过ROS入门教程，您将已经知道我们所有的ROS节点都以这两行开头。 第一行确保程序将作为Python脚本运行，而第二行则导入Python的主ROS库。
If you did the ROS Beginner Tutorials in Python, you’ll already know that all of our ROS nodes begin with these two lines. The first line ensures that the program will run as a Python script while the second imports the main ROS library for Python.

from geometry_msgs.msg import Twist
from math import pi

在这里，我们会处理本脚本所需的任何其他需要导入的软件包。 在本案例中，我们将需要ROS geometry_msgs包中的Twist消息类型和Python数学模块中的常数PI。 请注意，导入错误的常见原因是忘记在包的package.xml文件中包括必要的ROS 行。 在本案例中，我们的package.xml文件必须包含以下行：
Here we take care of any other imports we need for the script. In this case, we will need the Twist message type from the ROS geometry_msgs package and the constant pi from the Python math module. Note that a common source of import errors is to forget to include the necessary ROS line in your package’s package.xml file. In this case, our package.xml file has to include the line:

geometry_msgs</run_depend>

这样我们就可以从geometry_msgs.msg导入Twist了。
so that we can import Twist from geometry_msgs.msg .

class OutAndBack():
	def __init__(self):

在这里，我们将ROS节点的主体定义为Python类并且使用标准类的初始化行进行初始化，以此开始。
Here we begin the main body of our ROS node by defining it as a Python class along with the standard class initialization line.

# 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)

每个ROS节点都需要调用rospy.init_node()函数，并且我们还为on_shutdown()函数设置了一个回调，以便在脚本终止时可以执行任何必要的清除操作，例如 当用户点击Ctrl-C时。 对于移动机器人，最重要的清理任务是停止机器人！ 我们稍后将在脚本中看到如何执行此操作。
Every ROS node requires a call to rospy.init_node() and we also set a callback for the on_shutdown() function so that we can perform any necessary cleanup when the script is terminated—e.g. when the user hits Ctrl-C . In the case of a mobile robot, the most important cleanup task is to stop the robot! We’ll see how to do this later in the script.

# 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 = 50
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)

在这里，我们定义了ROS发布器，用于将Twist命令发送到/ cmd_vel话题。 我们还设置了要更新机器人运动的速率（在这种情况下，每秒更新50次），并将queue_size参数设置为5。
Here we define our ROS publisher for sending Twist commands to the /cmd_vel topic. We also set the rate at which we want to update the robot’s movement, 50 times per second in this case, and the queue_size parameter to 5.

queue_size参数是在ROS Hydro中引入的，在ROS Jade中是必需的。 在ROS Indigo中，如果不使用此参数初始化发布者，则您的代码将发出警告。 您可以在ROS Wiki上了解有关queue_size参数的更多信息。 要记住的最重要的事实是，如果省略此参数或将其设置为None，则发布者将表现为同步运行。 这意味着，如果发布者的主题有多个订阅者，并且其中一个订阅者被挂起（例如，它位于不稳定的无线连接的另一端），那么发布将阻止该主题的所有订阅者，直到挂起的订阅者 复活。 通常这不是理想的结果。 将queue_size参数设置为数值会导致发布者异步运行，因此每个订阅者都会在单独的线程上接收消息，而没有一个订阅者会锁住整个系统。（发布者同步，导致订阅者只能合用一个线程。发布者异步，订阅者可以用多个线程）
The queue_size parameter was introduced in ROS Hydro and will be mandatory in ROS Jade. In ROS Indigo, your code will issue a warning if a publisher is initialized without this parameter. You can learn more about the queue_size parameter on the ROS Wiki. The most important fact to keep in mind is that if this parameter is omitted or set to None , the publisher will behave synchronously. This means that if there are multiple subscribers to the publisher’s topic and one of the subscribers gets hung up—for example, it sits at the other end of a flaky wireless connection—then publishing will block on all subscribers to that topic until the hung subscriber comes back to life. This is generally not a desirable outcome. Setting the queue_size parameter to a numeric value causes the publisher to behave asynchronously so that each subscriber receives messages on a separate thread an no one subscriber and lock up the whole system.

# Set the forward linear speed to 0.2 meters per second
linear_speed = 0.2
# Set the travel distance to 1.0 meters
goal_distance = 1.0# Set the forward linear speed to 0.2 meters per second
linear_speed = 0.2
# Set the travel distance to 1.0 meters
goal_distance = 1.0
# How long should it take us to get there?
linear_duration = linear_distance / linear_speed

我们将前进速度初始化为相对安全的每秒0.2米，并将目标距离初始化为1.0米。 然后，我们计算这需要多长时间。
We initialize the forward speed to a relatively safe 0.2 meters per second and the target distance to 1.0 meters. We then compute how long this should take.

# Set the rotation speed to 1.0 radians per second
angular_speed = 1.0
# Set the rotation angle to Pi radians (180 degrees)
goal_angle = pi
# How long should it take to rotate?
angular_duration = angular_distance / angular_speed

同样，我们将旋转速度设置为每秒1.0弧度，并将目标角度设置为180度或Pi弧度。
Similarly, we set the rotation speed to 1.0 radians per second and the target angle to 180 degrees or Pi radians.

# Loop through the two legs of the trip
for i in range(2):
	# Initialize the movement command
	move_cmd = Twist()
	# Set the forward speed
	move_cmd.linear.x = linear_speed
	# Move forward for a time to go the desired distance
	ticks = int(linear_duration * rate)#总发布次数=发布持续时间(s) * 每秒发布次数
	for t in range(ticks):#定义了发布次数ticks，到达这个次数之后，就不再发布消息，以前beginner_tutorials的之所以会无限运动，因为用的死循环
		self.cmd_vel.publish(move_cmd)
		r.sleep()

这是实际移动机器人的循环-两条腿中的每条腿都有一个循环。 回想一下，一些机器人需要不断发布Twist消息以保持其移动。 因此，为了使机器人以每秒linear_speed米的速度向前移动linear_distance米，我们在适当的持续时间内每1 / rate秒发布一次move_cmd消息。 表达式r.sleep（）是rospy.sleep（1 / rate）的缩写，因为我们定义了变量r = rospy.Rate（rate）。
This is the loop that actually moves the robot—one cycle for each of the two legs. Recall that some robots require a Twist message to be continually published to keep it moving. So to move the robot forward linear_distance meters at a speed of linear_speed meters per second, we publish the move_cmd message every 1/rate seconds for the appropriate duration. The expression r.sleep() is a shorthand for rospy.sleep(1/rate) since we defined the variable r = rospy.Rate(rate) .

	# Now rotate left roughly 180 degrees
	# Set the angular speed
	move_cmd.angular.z = angular_speed
	# Rotate for a time to go 180 degrees
	ticks = int(goal_angle * rate)#总发布次数=发布持续时间(s) * 每秒发布次数
	for t in range(ticks):
		self.cmd_vel.publish(move_cmd)
		r.sleep()

在循环的第二部分中，我们以适当的持续时间（在这种情况下为Pi秒）以angular_speed弧度/秒的速度旋转机器人，这应产生180度。
In the second part of the loop, we rotate the robot at a rate of angular_speed radians per second for the appropriate duration (Pi seconds in this case) which should yield 180 degrees.

# Stop the robot.
self.cmd_vel.publish(Twist())

当机器人完成往返行程后，我们通过发布空的Twist消息（所有字段均为0）来停止它。
When the robot has completed the out-and-back journey, we stop it by publishing an empty Twist message (all fields are 0).

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)

这是我们的shutdown()回调函数。 如果脚本以任何原因终止，我们将通过发布空的Twist消息来停止机器人。
This is our shutdown callback function. If the script is terminated for any reason, we stop the robot by publishing an empty Twist message.

if __name__ == '__main__':
	try:
		OutAndBack()
	except rospy.ROSInterruptException:
		rospy.loginfo("Out-and-Back node terminated.")

最后，这是用于运行脚本的标准Python模块。 我们只需简单的实例化OutAndBack类即可使脚本（和机器人）运动。
Finally, this is the standard Python block for running the script. We simply instantiate the OutAndBack class which sets the script (and robot) in motion.

7.6.4 Timed Out and Back using a Real Robot (使用真实机器人进行定时往返)

如果您拥有类似TurtleBot的机器人，则可以在现实世界中尝试timed_out_and_back.py脚本。 请记住，我们仅使用时间和速度来估计距离和旋转角度。 因此，我们希望与理想的ArbotiX仿真相比，机器人的惯性会有所帮助（您会记得，该仿真不会对任何物理模型进行建模。）
If you have a robot like the TurtleBot, you can try the timed_out_and_back.py script in the real world. Remember that we are only using time and speed to estimate distance and rotation angles. So we expect that the robot’s inertia will throw things off compared to the ideal ArbotiX simulation (which, as you will recall, does not model any physics.)

首先，终止所有正在运行的仿真。 接下来，请确保您的机器人有足够的工作空间-距离机器人至少1.5米，并且两边各有一米。 然后调出机器人的启动文件。 如果您有原始的TurtleBot（iRobot创建基础），使用SSH连接机器人的电脑并运行：
First, terminate any running simulations. Next, 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. Then bring up your robot’s startup launch file(s). If you have an original TurtleBot (iRobot Create base), ssh into the robot’s laptop and run:

roslaunch rbx1_bringup turtlebot_minimal_create.launch

或者，如果您已经创建了一个启动文件来存储校准参数，则可以使用自己的启动文件。
Or use your own launch file if you have created one to store your calibration parameters.

我们还将使用辅助脚本，以便可以在RViz中看到TurtleBot的组合里程表框架。 （这将在下一部分中变得更加清楚。）如果您不使用TurtleBot，则可以跳过此步骤。 此启动文件应使用另一个ssh终端在TurtleBot的电脑上运行：
We will also use an auxiliary script so that we can see the TurtleBot’s combined odometry frame in RViz . (This will become clearer in the next section.) You can skip this if you are not using a TurtleBot. This launch file should be run on the TurtleBot’s laptop using another ssh terminal:

roslaunch rbx1_bringup odom_ekf.launch

接下来，我们将配置RViz以显示组合的里程表数据（编码器+陀螺仪），而不是仅显示编码器数据的 /odom。 如果您已经从上一个测试运行了RViz，则只需取消选中Odometry显示并选中Odometry EKF显示，然后跳过以下步骤。
Next we’re going to configure RViz to display the combined odometry data (encoders + gyro) rather than /odom which only shows the encoder data. 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

此配置与上一个配置之间的唯一区别是，我们现在在/ odom_ekf话题上显示组合的里程表数据，而不仅仅是在/ odom话题上发布的车轮编码器数据。 如果要比较两个显示，可以选中两个显示。
The only difference between this configuration and the previous one is that we are now displaying the combined odometry data on the /odom_ekf topic rather than just the wheel encoder data published on the /odom topic. You can check both displays if you want to compare the two.

最后，我们像之前一样运行out和back脚本。 请注意，脚本本身并不关心我们是在运行模拟还是在控制真实的机器人。 它只是在/ cmd_vel话题上发布Twist消息，供任何想听的人使用。 这是ROS如何使我们抽象出运动控制层次结构较低级别的一个示例。 您可以在你的工作站 或 使用ssh登录后的机器人的电脑上运行以下命令：
Finally, we run the out and back script just like we did before. Note that the script itself does not care if we are running a simulation or controlling a real robot. It is simply publishing Twist messages on the /cmd_vel topic for anyone who cares to listen. This is one example of how ROS allows us to abstract away the lower levels of the motion control hierarchy. You can run the following command either on your workstation or on the robot’s laptop after logging in with ssh :

rosrun rbx1_nav timed_out_and_back.py

这是在地毯上操作我自己的TurtleBot的结果：
Here is the result for my own TurtleBot when operating on a low-ply carpet:

从图片中可以看出，机器人最终并没有非常接近起始位置。 首先，它在转身之前走得还不够远（网格正方形相距0.5米）。 然后它在回头之前没有旋转完整的180度。 结果是机器人距离起点左侧约0.5m，并且方向错误。
As you can tell from the picture, the robot did not end up very close to the starting position. First of all, it did not go far enough before turning around (the grid squares are 0.5 meters apart). Then it did not rotate a full 180 degrees before heading back. The result is that the robot is about 0.5m to the left of the starting point, and it is oriented in the wrong direction.

幸运的是，我们需要纠正问题的数据直面我们。 上方图像中的大里程计箭头指示了机器人的内部里程计报告的机器人的位置和方向。 换句话说，机器人“知道”了它的混乱，但是我们在脚本中通过不使用里程表数据，从而不公平地使它变成了残障机器人。 虽然里程表数据不会完全匹配实际运动，但如果使用它，它应该可以为我们提供更好的结果。
Fortunately, the data we need to correct the problem is staring us right in the face. The large odometry arrows in the image above indicate the robot’s position and orientation as reported by its internal odometry. In other words, the robot “knows” it messed up but we have unfairly handicapped it by not using the odometry data in our script. While the odometry data will not match the real motion exactly, it should give us a better result if we use it.