ROS机器人仿真（四）- 控制一个移动基座
这一章要学习的是怎么控制一个差分轮机器人
1 单位和坐标系统
ROS使用的坐标系统是右手坐标系。
右手坐标系
旋转依据的是右手法则。
move
接下来我们就要学习，如何利用这些东西去做运动控制。在这之前，我们需要了解一些基本的东西。
3 Twisting 和 Turning
ROS使用Twist消息类型推送移动指令控制运动基座。我们将message推送到一个任意名字的topic，通常是/cmd_vel（也就是command velocities的缩写）。base_controller node订阅这个topic并且将Twist消息转换为电机信号使轮子转动。
可以查看一下Twist消息类型：
rosmsg show geometry_msgs/Twist
可以得到以下信息：
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
linear就是指示米每秒，angular就是指示rad/s，1rad大概57度。
3.1 Twist message示例
如果我们想要让机器人以0.1m/s的速度移动。这样的话消息的样子应该是linear：x=0.1,y=0,z=0，然后angular的值应该是：x=0,y=0,z=0.
如果想要执行这个命令，参数部分应该是如下的形式：
{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}
如果我们要这么控制机器人运行，手打的要累死了，实际上我们也很少这样去控制机器人。我们会使用别的node来控制。
3.2 使用RViz监视机器人
首先启动机器人：
roslaunch rbx1_bringup fake_turtlebot.launch
再打开一个命令行，启动rviz:
rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz
再打开一个命令窗口，向Twist推送消息使他运动：
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: -0.5}}'
使用-r是让推送自动以后面10Hz的频率推送到Twist，停止推送机器人就会停止，这实际上是一个安全设置，有些机器人不需要这样。
然后我们会看到下面的效果：
rotate
可以点击reset按键清除掉箭头。
让机器人停止运动，要先Ctrl+C停止刚才的指令输出，然后发布新的指令：
rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'
现在我们尝试一下第二个例子。下面的命令，会先让机器人向前移动3s（-1就是只发布一次的意思，我也不知道为什么是3秒），然后转圈：
$ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'; rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'
4 校准量程
这一节是给真正的机器人的，所以我们先跳过。
5 使用node推送消息到Twist
我们要实现机器人按照一定路径去运行
5.1 运行out-and-back仿真
停止刚才的指令，启动机器人：
$ roslaunch rbx1_bringup fake_turtlebot.launch
如果已经关了rviz，可以重新启动：
$ rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz
运行node：
rosrun rbx1_nav timed_out_and_back.py
运行效果是下面这样：
效果
5.2 源码分析
源码
#!/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.")
分析
#!/usr/bin/env python
确保程序执行为python脚本。
import rospy
将ROS的Python库包含进来。
from geometry_msgs.msg import Twist
from math import pi
这里包含了我们要用到的两个东西，需要注意的是，要把这个message的依赖计入到package.xml文件里：
<run_depend>geometry_msgs</run_depend>
这样我们才能把Twist包含进来。
class OutAndBack():
def __init__(self):
建立Python类，还有初始化函数。
# 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 node都需要这样一个初始化函数，并且我们还定义了一个回调函数，当node需要被清除的时候调用，做善后工作，比如让机器人停下来。后面会讲到。
# 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)
这里定义了要发送主题，消息和队列大小。以及发送频率。
这个队列大小可以去官网查看详细内容，如果我们忘了设置它，或者设置成了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 = 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)
在旋转之前，又发了一次空指令，让机器人停止前进。
# 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()
这里就是对转圈的处理。
这两个里面，都要保持持续的命令推送，推送的次数就是（1/rate）。
# 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.")
这就是主程序。
6 量程测量
ROS提供了message nav_msgs/Odometry保存了有关路程的数据。
关于它简短的描述是：
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist
详细的信息使用指令rosmsg show nav_msgs/Odometry查看：
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[36] covariance
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance
我们看到 PoseWithCovariance 保存的是机器人现在的位置和姿态，TwistWithCovariance 保存的是其运行速度和旋转速度。
pose和twist都可以使用covariance 来修正偏差。
