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 中看到机器人的运动。