您在这里:
welcome
»
ros
»
teleop_kbdevent
本页面只读。您可以查看源文件,但不能更改它。如果您觉得这是系统错误,请联系管理员。
<file python teleop_kbdEvent.py> #!/usr/bin/env python import struct import atexit event_bin_format = 'llHHI' import rospy from std_msgs.msg import String from geometry_msgs.msg import Twist class EventDevice: def __init__(self, path): self.path = path self._input_file = None @property def input_file(self): if self._input_file is None: try: self._input_file = open(self.path, 'rb') except IOError as e: if e.strerror == 'Permission denied': print('Permission denied ({}). You must be sudo to access global events.'.format(self.path)) exit() def try_close(): try: self._input_file.close except: pass atexit.register(try_close) return self._input_file def read_event(self): data = self.input_file.read(struct.calcsize(event_bin_format)) seconds, microseconds, type, code, value = struct.unpack(event_bin_format, data) return seconds + microseconds / 1e6, type, code, value, self.path def talker(): pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1) rospy.init_node('mx_kbdEvent_node', anonymous=False) rate = rospy.Rate(20) # 10hz e = EventDevice('/dev/input/by-id/usb-USB_USB_Device-event-kbd') twist = Twist() while not rospy.is_shutdown(): t = e.read_event() if t[2] == 105: #LEFT if t[3] == 1: twist.linear.x = 0 twist.angular.z = 0.5 # pub.publish(twist) if t[2] == 106: #RIGHT if t[3] == 1: twist.linear.x = 0 twist.angular.z = -0.5 # pub.publish(twist) if t[2] == 103: #UP if t[3] == 1: twist.linear.x = 0.25 twist.angular.z = 0 # pub.publish(twist) if t[2] == 108: #DOWN if t[3] == 1: twist.linear.x = -0.25 # twist.angular.z = 0 if t[2] == 28: #OK if t[3] == 1: twist.linear.x = 0 twist.angular.z = 0 # pub.publish(twist) pub.publish(twist) rate.sleep() if __name__ == '__main__': try: talker() except rospy.ROSInterruptException: pass </file>
登录
文章
阅读
显示源文件
修订记录
搜索
打印/导出
可打印版本
工具
反向链接
最近更改
媒体管理器
网站地图
永久链接
引用此文