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 <run_depend>行。 在本案例中，我們的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 <run_depend> line in your package’s package.xml file. In this case, our package.xml file has to include the line:
<run_depend>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.