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 pythonimport rospyfrom geometry_msgs.msg import Twist, Point, Quaternionimport tffrom rbx1_nav.transform_utils import quat_to_angle, normalize_anglefrom math import radians, copysign, sqrt, pow, piclass 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 中看到机器人的运动。