ROS 提高篇 之 A Mobile Base-04 — 控制移动平台 — （Python编程）控制虚拟机器人的移动（不精确的制定目标位置）
我使用的虚拟机软件：VMware Workstation 11
使用的Ubuntu系统：Ubuntu 14.04.4 LTS
ROS 版本：ROS Indigo
注意：
1 . ROS 提高篇这个专栏的教学有门槛。
2 . 如果你没有学习前面的教程，请想学习前面的 beginner_Tutorials 和 learning_tf 的ROS 相关教程。
一 . 前言：
上一节，我们讲了Twist 的作用，并在终端的命令行中给机器人发布带有 Twist 消息的话题来控制机器人运动。这一讲，我们使用 python 语言来编写一个 ROS 节点程序，发布带有 Twist 消息的话题，让机器人运动。
到目前为止，我们已经使用命令行做到了控制我们的机器人移动，但是大多数时间你将依靠 ROS 节点来发布适当的 Twist 消息.一个简单的例子，假设你想控制你的机器人像前移动 1米，然后转180度，然后回到开始的位置。
我们将尝试使用不同的方式来完成上面说的这个任务，这将会非常好的演示出在ROS中不同抽象层级的运动控制。
二 . 先运行程序，先看看效果
使用Time 和 Speed 来估计距离和旋转 ：
我们的一个个尝试是使用定时Twist命令来移动机器人前进一段距离，转动180度，然后再次移动前进，以相同的速度回到开始的位置。最后，我们将再一次转动机器人180度回到原始的状态。
我们将上面的运行效果，编写在文件夹中： rbx1_nav/nodes中的 timed_out_and_back.py 程序文件。
在查看代码之前，我们先来启动这个节点，看看运行效果：
新开一个终端，执行下面的命令，启动一个虚拟的 TurtleBot 机器人：
$ roslaunch rbx1_bringup fake_turtlebot.launch再开一个终端，启动 RViz ：
$ rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz最后开一个终端，运行 timed_out_and_back.py 节点：
$ rosrun rbx1_nav timed_out_and_back.py当你将这句命令执行完，在 RVIz模拟器 中，你就可以看到上面图片里的运行效果。
三 . 下面看代码： timed_out_and_back.py
# -*- coding: utf-8 -*-
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from math import pi
class OutAndBack():
def __init__(self): #self:类似于C++中的this指针
# 给出节点的名字
rospy.init_node('out_and_back', anonymous=False)
# 设置rospy在程序退出时执行的关机函数
rospy.on_shutdown(self.shutdown)
# Publisher（发布器）：发布机器人运动的速度
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# 我们将用多快的速度更新控制机器人运动的命令?
rate = 50
# 设定相同的值给rospy.Rate()
r = rospy.Rate(rate)
# 设定前进的线速度为0.2m/s
linear_speed = 0.2
# 设定行程距离为1.0m
goal_distance = 1.0
# 它需要花多长时间才能到达那里?
linear_duration = goal_distance / linear_speed
# 设定转动速度1.0rad/s .即大约6秒转一圈
angular_speed = 1.0
# 设定转动弧度： Pi 弧度 (180 度)
goal_angle = pi
# 它转动需要花多长时间?
angular_duration = goal_angle / angular_speed
# 往返2次行程
for i in range(2):
# 初始化运动命令
move_cmd = Twist()
# 设定前进速度
move_cmd.linear.x = linear_speed
# 在前进1米的这段时间控制命令更新的次数
ticks = int(linear_duration * rate)
for t in range(ticks):
self.cmd_vel.publish(move_cmd)
r.sleep()
# 在机器人转动前，先让它停下来
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# 现在向左转动大概180度
# 设定角速度
move_cmd.angular.z = angular_speed
# 转动180度控制命令更新的次数
ticks = int(goal_angle * rate)
for t in range(ticks):
self.cmd_vel.publish(move_cmd)
r.sleep()
# 在下一次行驶前，先停下来
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# 让机器人停下来
self.cmd_vel.publish(Twist())
def shutdown(self):
# 当关闭这个节点时，让机器人停止不动。
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.")
四 . 代码讲解：让我们一行一行的从头开始
1 . 如果你学过 ROS 教程入门篇（beginner_Tutorials 和 learning_tf 的ROS 相关教程），你会知道用python 写的 ROS 节点程序的开头都有这三行。
# -*- coding: utf-8 -*-
#!/usr/bin/env python
import rospy第一行：因为这个python程序里面有中文的注释，所以要加上这一行来制定这个Python代码的编码方式。
第二行确保将程序作为一个python脚本运行。
而第三行是给python语言提供的主ROS库。
2 . 下面我们还需要提供一些这个脚本需要的东西：
我们需要来至ROS
geometry_msgs包中的Twist消息类型和来至Python math模块的常数
pi
from geometry_msgs.msg import Twist
from math import pi注意：当你使用
geometry_msgs消息文件里的消息类型时会出错，是因为：你没有在package.xml文件中添加下面这就话:<run_depend>geometry_msgs</run_depend>这样，你才能
import geometry_msgs.msg消息文件中的Twist消息类型了。
3 . 下面是我们这个 ROS 节点的主体，通过 Python 类的类初始化函数，定义了我们这个节点：
class OutAndBack():
def __init__(self): #self:类似于C++中的this指针4 .
每一个 ROS 节点都需要一个名字称呼，通过
rospy.init_node()设置我们需要给
on_shutdwon()设置回调
# 给出节点的名字
rospy.init_node('out_and_back', anonymous=False)
# 设置rospy在程序退出时执行的关机函数
rospy.on_shutdown(self.shutdown)例如，当用户按了 Ctrl+C，想让程序终止，在这个函数（self.shutdown）中我们可以执行任何有必要的清理工作。
一个移动机器人在这种情况下，清除任务中最重要的工作是：让机器人停下来。在后面的代码中我们会讲如何去做。
5 .
# Publisher（发布器）：发布机器人运动的速度
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# 我们将用多快的速度更新机器人的运动?
rate = 50
# 设定相同的值给Rate()
r = rospy.Rate(rate)’ queue_size ’ （队列的尺寸）这个参数是在ROS Hydro版中引入，在ROS Jade版中必须添加。在ROS Indigo版中， 如果初始化一个Publisher不加这个参数，你的代码会发出警告(Warning)。
By the Way : ROS 的发展： 各个版本的ROS：
electric -> fuerte -> groovy -> hydro -> indigo -> jade -> kinetic
关于 ’ queue_size ’ 参数，要记住最重要的事实是：如果省略该参数或者设置为 None ，那么 Publisher 将会行为同步。什么意思：这意味着如果多个用户想发布 ’ /cmd_vel ’ 话题，并且其中一个用户挂掉了。发布端将阻止所有用户到该话题，直到挂掉的用户起死回生。你可以想象，这一般不会得到一个好的结果。将 queue_size 参数设为数值，可以让发布 Publisher (发布器)行为异步，就是每个 Subscriber (订阅者)都在一个单独的线程中接收消息，这个单独的线程中只要它自己，没有别的其他的 Subscriber 在里面。
6 .
# 设定前进的线速度为0.2m/s
linear_speed = 0.2
# 设定行程距离为1.0m
goal_distance = 1.0
# 它需要花多长时间才能到达那里?
linear_duration = goal_distance / linear_speed我们初始化了一个相对安全的前进速度：0.2m/s, 目标距离为 1m。然后，我们计算出这应该要多久。
7 .
# 设定转动速度1.0rad/s .即大约6秒转一圈
angular_speed = 1.0
# 设定转动弧度： Pi 弧度 (180 度)
goal_angle = pi
# 它转动需要花多长时间?
angular_duration = goal_angle / angular_speed同样，我们设定转动速度：1.0rad/s ，目标角度到 180 度或 Pi 弧度。
8 .
下面是循环的第一部分： 机器人前进 1m。
# 往返2次行程
for i in range(2):
# 初始化运动命令
move_cmd = Twist()
# 设定前进速度
move_cmd.linear.x = linear_speed
# 在前进1米的这段时间控制命令更新的次数
ticks = int(linear_duration * rate)
for t in range(ticks):
self.cmd_vel.publish(move_cmd)
r.sleep()因为我们定义了r = rospy.Rate(rate) ; 所以 r.sleep() 表示为 rospy.sleep(1/rate) 的简写形式。
9 .
循环的第二部分也是同理，转动 180度。（代码没有贴出来）
10 .
下面这句话，可以让机器人停下来：
self.cmd_vel.publish(Twist()) def shutdown(self):
# 当关闭这个节点时，让机器人停止不动。
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
rospy.sleep(1)这是我们的关闭回调函数，如果有什么原因导致程序终止了，会执行这个函数：我们发布一个空的 Twist 消息来让 robot停下来。
11 .
if __name__ == '__main__':
try:
OutAndBack()
except:
rospy.loginfo("Out-and-Back node terminated.")当我们想让程序 ( 就是机器人 ) 启动，只需要简单的实例化一个 OutAndBack 对象，就可以。
代码讲解完毕
程序的运行效果，上面我已经贴图了，这里就不再贴了。
总结：
我们这一节编写的程序是怎样让机器人移动指定的距离和转动指定的角度。是通过下面这种方式做到的：
# 在前进1米的这段时间控制命令更新的次数
ticks = int(linear_duration * rate)这种方式不是很好，还记得我们之前介绍的 odometry 消息类型吗。下一节，我们将使用 odometry 消息类型，重新这节课上面的程序。