前言:
Twist消息能描述所有的3D运动,在控制差分驱动的平面运动机器人时,只需要使用其中的两个参数:线性速度(前进/后退)和绕竖直轴的角速度。所以只需要简单地发送线速度(米每秒)和角速度(弧度每秒)命令。这里需要两个程序:一个程序用来监听键盘地敲击事件然后通过ROS消息发布出去,另一个订阅这个ROS消息并输出一个Twist消息,从而实现键盘输入控制机器人地自由移动。
1.键盘驱动:
使用Python的termios和tty库将终端设置成原始模式并捕获键盘敲击事件,然后将事件以std_msgs/string的消息形式发布。
#!/usr/bin/env python
import sys, select, tty, termios
import rospy
from std_msgs.msg import String
if __name__ == '__main__':
#发布一个‘keys’消息,消息类型String,缓冲为1
key_pub = rospy.Publisher('keys', String, queue_size=1)
#节点初始化
rospy.init_node("keyboard_driver")
#设置频率
rate = rospy.Rate(100)
#获取键盘敲击,修改终端属性
old_attr = termios.tcgetattr(sys.stdin)
tty.setcbreak(sys.stdin.fileno())
print "Publishing keystrokes. Press Ctrl-C to exit..."
while not rospy.is_shutdown():
if select.select([sys.stdin], [], [], 0)[0] == [sys.stdin]:
#发布消息
key_pub.publish(sys.stdin.read(1))
rate.sleep()
#将终端还原为标准模式
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_attr)
运行效果:
2.运动生成器:
使用w、x、a、d和s是机器人前进、后退、左转、右转和停止。