ROS Learning-029 （提高篇-007 A Mobile Base-05） 控制移动平台 --- （Python编程）控制虚拟机器人的移动（精确的制定目标位置）

ROS 提高篇 之 A Mobile Base-05 — 控制移动平台 — （Python编程）控制虚拟机器人的移动（精确的制定目标位置）

使用 odometry 消息类型 重写 out_and_back 程序。

---

我使用的虚拟机软件：VMware Workstation 11
使用的Ubuntu系统：Ubuntu 14.04.4 LTS
ROS 版本：ROS Indigo

---

注意：
1 . ROS 提高篇这个专栏的教学有门槛。
2 . 如果你没有学习前面的教程，请想学习前面的 beginner_Tutorials 和 learning_tf 的ROS 相关教程。

---

一 . 前言：

我们上一节编写的程序是通过 在前进1米的这段时间控制命令更新的次数 的方式，来让机器人移动指定的距离和转动指定的角度。
这种方式不是很好，还记得我们之前介绍的 odometry 消息类型吗？这一节，我们将使用 odometry 消息类型，重新上一节的程序。

---

二 . 运行程序，看看效果：

在查看代码之前，我们先来启动这个节点，看看运行效果：

新开一个终端，执行下面的命令，启动一个虚拟的 TurtleBot 机器人：

$ roslaunch rbx1_bringup fake_turtlebot.launch

再开一个终端，启动 RViz ：

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

最后开一个终端，运行 odom_out_and_back.py 节点：

$ rosrun rbx1_nav odom_out_and_back.py

当你将这句命令执行完，在 RVIz模拟器 中，你就可以看到下面图片里的运行效果。（运行效果和上一节，一模一样）

---

三 . 代码如下： odom_out_and_back.py

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
from math import radians, copysign, sqrt, pow, pi

class OutAndBack():
    def __init__(self):
        # 给出节点的名字
        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 = 20

        # 设定相同的值给rospy.Rate()
        r = rospy.Rate(rate)

        # 设定前进的线速度为0.2m/s
        linear_speed = 0.2

        # 设定行程距离为1.0m
        goal_distance = 1.0

        # 设定转动速度1.0rad/s .即大约6秒转一圈
        angular_speed = 1.0

        # Set the angular tolerance in degrees converted to radians
        angular_tolerance = radians(2.5)

        # 设定转动弧度： Pi 弧度 (180 度)
        goal_angle = pi

        # 初始化这个tf监听器
        self.tf_listener = tf.TransformListener()

        # 给tf一些时间让它填补它的缓冲区
        rospy.sleep(2)

        # 设置odom坐标系
        self.odom_frame = '/odom'

        # 询问机器人使用的是/base_link坐标系还是/base_footprint坐标系
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")  

        # 初始化了一个Point类型的变量
        position = Point()

        # 往返2次行程
        for i in range(2):
            # 初始化运动命令
            move_cmd = Twist()

            # 设定前进速度
            move_cmd.linear.x = linear_speed

            # 得到开始的姿态信息（位置和转角）    
            (position, rotation) = self.get_odom()

            x_start = position.x
            y_start = position.y

            # 随时掌控机器人行驶的距离
            distance = 0

            # 进入循环，沿着一边移动
            while distance < goal_distance and not rospy.is_shutdown():
                # 发布一次Twist消息 和 sleep 1秒        
                self.cmd_vel.publish(move_cmd)

                r.sleep()

                # 给出正确的姿态信息（位置和转角）
                (position, rotation) = self.get_odom()

                # 计算相对于开始位置的欧几里得距离（即位移）
                distance = sqrt(pow((position.x - x_start), 2) + 
                                pow((position.y - y_start), 2))

            # 在转动机器人前，停止它
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

            # 给旋转配置运动命令
            move_cmd.angular.z = angular_speed

            # 跟踪记录最后的角度
            last_angle = rotation

            # 跟踪我们已经转动了多少角度
            turn_angle = 0

            while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
                # 发布一次Twist消息 和 sleep 1秒  
                self.cmd_vel.publish(move_cmd)
                r.sleep()

                # 给出正确的姿态信息（位置和转角）
                (position, rotation) = self.get_odom()

                # 计算自每次的旋转量
                delta_angle = normalize_angle(rotation - last_angle)

                # 添加到正在运行的合计里
                turn_angle += delta_angle
                last_angle = rotation

            # 下一航段之前停止机器人
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

        # 为了机器人好，停止它
        self.cmd_vel.publish(Twist())

    def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

    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 .

        # Set the angular tolerance in degrees converted to radians
        angular_tolerance = radians(2.5)

这里，我定义了一个角度的angular_tolerance （角度公差）。它的作用是：在真正的机器人上，很容易发生角度转多了或转少了。一个小小的角度误差会照成机器人远离了下一个位置。凭经验发现大约一个公差 2.5度给出可接受的结果。

---

2 .

        # 初始化这个tf监听器
        self.tf_listener = tf.TransformListener()

        # 给tf一些时间让它填补它的缓冲区
        rospy.sleep(2)

        # 设置odom坐标系
        self.odom_frame = '/odom'

        # 询问机器人使用的是/base_link坐标系还是/base_footprint坐标系
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")  

接下来，我们创建一个TransformListener对象来监视坐标系转换（frame transforms）。 注意 tf 需要一些时间去填补监听者(listener)的缓冲区，所以我们需要调用 rospy.sleep(2)。

为了获得机器人的位置和方向，如果你的机器人是 TurtleBot，那么我们需要 /odom 坐标系和 /base_footprint 坐标系之间的转换；如果你的机器人是 Pi Robot 和 Maxwell, 那么我们需要 /odom 坐标系和 /base_link 坐标系。

在上面的程序中，我们先试着找有没有 /base_footprint 坐标系，如果没有找到它，我们就试 /base_link 坐标系。结果被存放在 self.base_frame 变量中，供后面程序使用。所以这样，程序就可以兼容Pi Robot 、Maxwell、 TurtleBot 机器人。

3 .

            # 得到开始的姿态信息（位置和转角）    
            (position, rotation) = self.get_odom()

            x_start = position.x
            y_start = position.y

在一个航段的开始，我们调用 get_odom() 函数来记录下这个时候机器人的位置(tran)和角度(rot)。

---

4 .
接下来让我们来理解 get_odom() 是如何工作：

    def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

get_odom()函数首先使用 tf_listener 对象查找当前 odometry 坐标系和 base（基础）坐标系之间的变换。如果查找出现问题，我们抛出异常。否则，我们返回一个用坐标表示的点和一个用四元数表示的旋转。

其中：return (Point(*trans), quat_to_angle(Quaternion(*rot))) 这段语句。在 python 中使用 “ * ” 修饰的trans 和 rot 变量，表示传给一个函数的形参是一个列表。我们这里的 trans 是 x、y、z 轴坐标值；rot是 x、y、z、w 四元数值。

---

5 .

现在在回到主函数：

            # 随时掌控机器人行驶的距离
            distance = 0

            # 进入循环，沿着一边移动
            while distance < goal_distance and not rospy.is_shutdown():
                # 发布一次Twist消息 和 sleep 1秒        
                self.cmd_vel.publish(move_cmd)

                r.sleep()

                # 给出正确的姿态信息（位置和转角）
                (position, rotation) = self.get_odom()

                # 计算相对于开始位置的欧几里得距离（即位移）
                distance = sqrt(pow((position.x - x_start), 2) + 
                                pow((position.y - y_start), 2))

这个循环是为了前进 1m。

---

6 .

            # 跟踪我们已经转动了多少角度
            turn_angle = 0

            while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
                # 发布一次Twist消息 和 sleep 1秒  
                self.cmd_vel.publish(move_cmd)
                r.sleep()

                # 给出正确的姿态信息（位置和转角）
                (position, rotation) = self.get_odom()

                # 计算自每次的旋转量
                delta_angle = normalize_angle(rotation - last_angle)

                # 添加到正在运行的合计里
                turn_angle += delta_angle
                last_angle = rotation

这个循环是为了旋转 180度。

---

五 . 我们总结一下：

Q： 我们可能会有疑问：在上面的代码中，为什么我们要使用 TransformListener 来访问 odometry 信息，而不是仅仅订阅一个 /odom 话题。

A： 原因是：发布的 /odom 话题里的数据”不完整“。什么意思？举例说明：TurtleBot 机器人使用了一个单轴陀螺仪给机器人的旋转提供了一个额外的估量。robot_pose_ekf 节点（在 TurtleBot 的启动文件中被启动）将它和轮子编码器的数据相结合，目的是为了得到更好的旋转的估计，而不是使用单独源。

我为什么说 /odom 话题里的数据并不完整呢，是因为，robot_pose_ekf 节点并没有将它里面更好的数据传递给 /odom 话题里，/odom 话题里的转据是通过轮子编码器数据估计出来的。并且，它将它自己的数据发布到 /odom_combined 话题里。此外，被发布的数据也不再是 Odometry 消息类型了，而是 PoseWithCovarianceStamped 消息类型。然而，它提供了一个我们需要的信息，这个信息就是：/odom 坐标系与 /base_link (or /base_footprint ) 坐标系之间的转换。 所以结果通常是，使用 tf 去监视 /odom 坐标系与 /base_link ( or base_footprint ) 坐标系之间的转换要比依赖 /odom 话题要安全的多。

---

下一节，Ongoing。。。。