一、控制底座
效果:让机器人花一段时间向前移动1m,再旋转180度,最后返回原点。
1、基于定时的timed_out_and_back.py
# -*- coding: utf-8 -*- 有中文注释
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from math import pi
class OutAndBack():
def __init__(self):
#1、初始化:给出节点名字,设置回调函数
rospy.init_node('out_and_back', anonymous=False)
rospy.on_shutdown(self.shutdown)
#2、发布话题,机器人运动速度
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rate = 50
r = rospy.Rate(rate)
#3、设定前进速度、距离;转动速度、距离
linear_speed = 0.2
goal_distance = 1.0
linear_duration = goal_distance / linear_speed
angular_speed = 1.0
goal_angle = pi
angular_duration = goal_angle / angular_speed
#往返两次行程
for i in range(2):
#4、前进1m
move_cmd = Twist()
move_cmd.linear.x = linear_speed
ticks = int(linear_duration * rate)
for t in range(ticks):
self.cmd_vel.publish(move_cmd)
r.sleep()
# 旋转之前停下来
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# 5、转动180度
move_cmd.angular.z = angular_speed
ticks = int(goal_angle * rate)
for t in range(ticks):
self.cmd_vel.publish(move_cmd)
r.sleep()
# 下一步之前停下来
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# 发布空的Twist消息让机器人停下来
self.cmd_vel.publish(Twist())
def shutdown(self):
# 发布空的Twist消息让机器人停下来
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.")
OutAndBack类主要分为5部分:1、初始化:给出节点名字,设置回调函数;2、发布话题,机器人运动速度;3、设定前进速度、距离;转动速度、距离;4、循环部分,前进1m,旋转180度;5、主函数
接下来将详细介绍5部分内容
1、def __init__(self) 标准的类初始化,self类似于C++中this指针。
2、self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 发布Twist命令给/cmd_vel话题,且每秒50次更新机器人运动
4、前进1m
# 初始化运动命令
move_cmd = Twist()
# 设定前进速度
move_cmd.linear.x = linear_speed
# 在前进1米的这段时间控制命令更新的次数
ticks = int(linear_duration * rate)
for t in range(ticks):
self.cmd_vel.publish(move_cmd)
r.sleep()
r.sleep()是rospy.sleep(1/rate)的简单写法。
r=rospy.Rate(rate) 保持一定的速率来进行循环。
我们需要每1/rate秒发布一次move_cmd消息。一共发布linear_duration*rate次消息。
5、标准程序块,实例化OutAndBack类运行脚本。
if __name__ == '__main__':
try:
OutAndBack()
except:
rospy.loginfo("Out-and-Back node terminated.")
2、基于测量的odomz-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):
# 1、初始化:给出节点名称,设置回调函数
rospy.init_node('out_and_back', anonymous=False)
rospy.on_shutdown(self.shutdown)
# 2、发布话题、前进速度和旋转速度
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
rate = 20
r = rospy.Rate(rate)
linear_speed = 0.15
goal_distance = 1.0
angular_speed = 0.5
angular_tolerance = radians(1.0)
goal_angle = pi
# 3、新增的一系列功能:tf、odom、base_footprint、Point
# 初始化tf监听器,填补缓冲区
self.tf_listener = tf.TransformListener()
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()
# 4、循环,前进1m,转动180度
for i in range(2):
# 前进1m
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():
# 发布一次消息,睡眠
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():
# 发布一次消息,睡眠
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):
# 得到现测量值与基框架间转换
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):
# 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.")
在上一节基础上增加了
3、新增的一系列功能:tf、odom、base_footprint、Point;4、对于前进直线和旋转的记录。
1、self.tf_listener = tf.TransformListener()
rospy.sleep(2)
我们创建一个TransformListener对象来监视坐标系转换(frame transforms)。 注意 tf需要一些时间去填补监听者(listener)的缓冲区,所以我们需要调用 rospy.sleep(2)
。
2、get_odom()
(position, rotation) = self.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)))
首先使用tf_listener对象查找当前 odom 坐标系和 base(基础)坐标系之间的变换。正常的话返回位置和旋转(四元数表示)
其中:return (Point(*trans), quat_to_angle(Quaternion(*rot)))
这段语句。在 python 中使用 “ * ” 修饰的trans 和 rot 变量,表示传给一个函数的形参是一个列表。我们这里的 trans 是 x、y、z 轴坐标值;rot是 x、y、z、w 四元数值。
总结:
上一篇文章核心思想是:通过前进1m这段时间控制命令更新的次数。
ticks = int(linear_duration * rate)
for t in range(ticks):
self.cmd_vel.publish(move_cmd)
r.sleep()
本文使用TransformListener
来访问 odom信息,使用 tf 去监视 /odom
坐标系与 /base_link
坐标系之间的转换要比依赖 /odom
话题要安全的多。