mouse_listener.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import os, sys
00003 
00004 import roslib; roslib.load_manifest('wouse')
00005 import rospy
00006 from geometry_msgs.msg import Vector3Stamped
00007 
00008 Y_OVERFLOW = 128
00009 X_OVERFLOW = 64
00010 Y_SIGN = 32
00011 X_SIGN = 16
00012 CHECK = 8
00013 MIDDLE_BUTTON = 4
00014 RIGHT_BUTTON = 2
00015 LEFT_BUTTON = 1
00016 
00017 class MouseListener(object):
00018     """A node for reading a PS/2 mouse linux character device file"""
00019     def __init__(self, device_file):
00020         self.f = os.open(device_file, os.O_RDONLY | os.O_NONBLOCK)
00021         self.vec_pub = rospy.Publisher('wouse_movement', Vector3Stamped)
00022         rospy.loginfo("[wouse_listener]: Ready")
00023 
00024     def poll(self):
00025         """Tries to read data from the mouse device, then parse and publish."""
00026         try:
00027             data = os.read(self.f, 3)
00028         except OSError as (errno, strerr):
00029             #Ignore 'resource not available' err 
00030             #caused by non-blocking read on empty file.
00031             if errno == 11: 
00032                 rate.sleep()
00033                 return
00034             else:
00035                 raise
00036         bits = ord(data[0])
00037         if not bits & CHECK:
00038            rospy.logwarn("[mouse_listener]: Bit Check Failed")
00039            return
00040         del_x = ord(data[1])
00041         del_y = ord(data[2])
00042         if bits & Y_SIGN:
00043             del_y = -(256-del_y)
00044         if bits & X_SIGN:
00045             del_x = -(256-del_x)
00046 
00047         v3st = Vector3Stamped() 
00048         v3st.header.stamp = rospy.Time.now()
00049         v3st.vector.x = del_x
00050         v3st.vector.y = del_y
00051         if (bits & X_OVERFLOW or bits & Y_OVERFLOW):
00052             v3st.vector.z = 1
00053         self.vec_pub.publish(v3st)
00054 
00055 if __name__=='__main__':
00056     rospy.init_node('mouse_listener')
00057     ml =  MouseListener(sys.argv[1])
00058     rate = rospy.Rate(200)
00059     while not rospy.is_shutdown():
00060         ml.poll()
00061         rate.sleep()
00062     os.close(ml.f)


wouse
Author(s): Phillip M. Grice, Advisor: Prof. Charlie Kemp, Lab: The Healthcare Robotoics Lab at Georgia Tech.
autogenerated on Wed Nov 27 2013 11:57:42