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
 
打印/导出