ROS机器人编程实践——读书笔记2

上文讨论发布一串Twist消息来控制机器人。本文通过键盘输入来控制机器人的线速度和角速度。

一、第一个是监听键盘敲击的keys话题上发布std/String键盘驱动程序。需要Python模块termios和tty库捕捉敲击事件。

key_publisher.py

 

#!/usr/bin/env  python
import sys,select,tty,termios
import rospy
from std_msgs.msg import String

if __name__ == '__main__':
	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.tcgetattr(sys.stdin,termios,TCSADRAIN,old_attr)

只要按下一个键，我们程序通过标准输入流捕获。要改变终端行为，首先需要备份终端属性，然后更改其属性。

 

 

	old_attr=termios.tcgetattr(sys.stdin)
	tty.setcbreak(sys.stdin.fileno())

ROS回调函数

 

 

		if select.select([sys.stdin],[],[],0)[0]==[sys.stdin]:
			key_pub.publish(sys.stdin.read(1))
		rate.sleep()

rate.sleep()

会消耗掉剩下的时间。

 

然后运行 第一个终端：$ roscore

               第二个终端：$ chmod +x key_publisher.py

                                 $./key_publisher.py 

               第三个终端：$rostopic echo keys

在第二个终端敲击键盘，在第三个终端显示

二、速度与线速度控制

keys_to_twist.py

 

#!/usr/bin/env  python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist

key_mapping={'w':[0,1],'x':[0,-1],'a':[-1,0],'d':[1,0],'s':[0,0]}

def keys_cb(msg,twist_pub):	
	if len(msg.data)==0 or not key_mapping.has_key(msg.data[0]):
		return
	vels=key_mapping[msg.data[0]]
	t=Twist()
	t.angular.z=vels[0]
	t.linear.x=vels[1]
	twist_pub.publish(t)

if __name__ == '__main__':
	rospy.init_node('keys_to_twist')
	twist_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)
	rospy.Subscriber('keys',String,keys_cb,twist_pub)
	rospy.spin()

同理

 

 第一个终端：$ roscore

 第二个终端：$./key_publisher.py

 第三个终端：$./keys_to_twist.py

在第二个终端敲击w,a,s,d,x,在第三个终端显示。

三、实现连续输出

keys_to_twist_using_rate.py

 

#!/usr/bin/env  python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist

key_mapping={'w':[0,1],'x':[0,-1],'a':[-1,0],'d':[1,0],'s':[0,0]}
g_last_twist=None

def keys_cb(msg,twist_pub):
	global g_last_twist
	if len(msg.data)==0 or not key_mapping.has_key(msg.data[0]):
		return
	vels=key_mapping[msg.data[0]]
	g_last_twist.angular.z=vels[0]
	g_last_twist.linear.x=vels[1]
	twist_pub.publish(g_last_twist)

if __name__ == '__main__':
	rospy.init_node('keys_to_twist')
	twist_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)
	rospy.Subscriber('keys',String,keys_cb,twist_pub)
	rate=rospy.Rate(10)
	g_last_twist=Twist()
	while not rospy.is_shutdown():
		twist_pub.publish(g_last_twist)
		rate.sleep()

 

 第一个终端：$ roscore

 第二个终端：$./key_publisher.py

 第三个终端：$./keys_to_twist_using_rate.py

第四个终端： $rqt_plot cmd_vel/linear/x cmd_vel/angular/z

在第二个终端中，敲击响应键。