这一章要学习的是怎么控制一个差分轮机器人

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 来修正偏差。