ROS Learning-030 （提高篇-008 A Mobile Base-06） 控制移动平台 --- （Python）odom导航的例子：移动一个方块路径

ROS 提高篇 之 A Mobile Base-06 — 控制移动平台 — （Python）再次使用odom导航的一个例子：移动一个方块路径

---

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

---

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

---

一 . 前言：

上一节，我们使用odom 重写了前进和返回的程序。这次，我们尝试以一个方块形路径移动移动机器人。（方块的4个点设置航点）

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

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

新开一个终端，执行下面的命令，启动一个虚拟的 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 nav_square.py

当你将这句命令执行完，在 RVIz模拟器 中，你就可以看到下面图片里的运行效果：程序在模拟器中航行一个方块路径，机器人在这个路径上移动。

三 . 程序代码： nav_square.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 NavSquare():
    def __init__(self):
        # 给出节点的名字
        rospy.init_node('nav_square', anonymous=False)

        # 设置rospy在程序退出时执行的关机函数
        rospy.on_shutdown(self.shutdown)

        # 我们将用多快的速度更新控制机器人运动的命令?
        rate = 20

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

        # 为目标正方形设置参数
        goal_distance = rospy.get_param("~goal_distance", 1.0)      # 米
        goal_angle = rospy.get_param("~goal_angle", radians(90))    # 角度转换弧度
        linear_speed = rospy.get_param("~linear_speed", 0.2)        # m/s
        angular_speed = rospy.get_param("~angular_speed", 0.7)      # rad/s
        angular_tolerance = rospy.get_param("~angular_tolerance", radians(2)) # 角度到弧度的公差

        # 发布者控制机器人的速度
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

        # 配置base坐标系: 如果是TurtleBot机器人为： base_footprint， 如果是Pi Robot机器人为： base_link
        self.base_frame = rospy.get_param('~base_frame', '/base_link')

        # odom坐标系通常就叫：/odom
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')

        # 初始化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()

        # 周期循环：通过正方形的四边
        for i in range(4):
            # 初始化运动命令
            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.0)

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

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

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

            # Begin the rotation
            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.0)

        # 为了机器人好，停止它
        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:
        NavSquare()
    except rospy.ROSInterruptException:
        rospy.loginfo("Navigation terminated.")

四 . 讲解程序

略，就不讲了，因为这个程序,相比前两节（编写一个out_and_back 程序 和 使用 odometry 消息类型 重写 out_and_back 程序），没有什么新的知识点。

五 . 航位推算的问题：

这个问题的根源：随时间积累的测量出现的小误差。如何解决这个问题？幸运的是，机器人学家好久以前就开始研究各种方法来在导航中 合并地标或者使用其他外部参考，我们将会在SLAM章节学习到。

搞定

---

---

总结：
下一节，我们讲如何人机交互，并在Rviz 中看到机器人的运动。