00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 import roslib; roslib.load_manifest('rosmouse')
00030 from geometry_msgs.msg import PoseStamped, PointStamped
00031 from ctypes import cdll
00032 import Xlib.display
00033 import rospy
00034 from headmath.conversions import pose_quat_to_euler, pose_euler_to_quat
00035 from threading import RLock, Thread
00036 from functools import wraps
00037 from math import tan
00038 from rosmouse.filters import MeanFilter, KalmanFilter
00039 from dynamic_reconfigure.server import Server
00040 from rosmouse.cfg import MouseConfig
00041 import numpy as np
00042
00043 def move_mouse(x,y):
00044 dll = cdll.LoadLibrary('libX11.so')
00045 d = dll.XOpenDisplay(None)
00046 root = dll.XDefaultRootWindow(d)
00047 dll.XWarpPointer(d,None,root,0,0,0,0,x,y)
00048 dll.XCloseDisplay(d)
00049
00050 def thread(func):
00051 @wraps(func)
00052 def wrap(*args, **kwargs):
00053 t = Thread(target=func, args=args, kwargs=kwargs)
00054 t.start()
00055 return t
00056 return wrap
00057
00058 def InterpolationFactory(x1, x2, y1, y2):
00059 def fun(X):
00060 return int((y2-y1)/(x2-x1)*(X-x1)+y1)
00061 return fun
00062
00063 class Mouse(object):
00064 yaw_left = None
00065 yaw_right = None
00066 pitch_up = None
00067 pitch_dn = None
00068 screen_res = None
00069 fn_ready = False
00070 ready = False
00071 run = False
00072
00073 xFun = None
00074 yFun = None
00075
00076 last_pose = None
00077 pose_lock = RLock()
00078
00079 do_move_mouse = True
00080
00081
00082 cov = np.matrix([[6.71485028, 18.75061549], [ 14.25320564, 82.71265061]])
00083 filter = KalmanFilter(cov, 2, 2)
00084
00085 def __init__(self):
00086 self.do_move_mouse = rospy.get_param('~move_mouse', False)
00087 self.subscribe()
00088 self.calibrate()
00089 self.mouse_pub = rospy.Publisher('mouse', PointStamped)
00090 self.listen_for_keyboard()
00091
00092 @thread
00093 def listen_for_keyboard(self):
00094 help_msg = 'Keboard commands:\n'
00095 help_msg += ' Recalibrate: c\n'
00096 help_msg += ' Disable Mouse: d\n'
00097 help_msg += ' Enable Mouse: e\n'
00098 help_msg += ' Print Help: h\n'
00099 help_msg += ' Exit: q\n'
00100
00101 print help_msg
00102
00103 while not rospy.is_shutdown():
00104 cmd = raw_input('-->')
00105 if cmd == 'c':
00106 self.calibrate()
00107 elif cmd == 'd':
00108 self.ready = False
00109 elif cmd == 'e':
00110 self.ready = True
00111 elif cmd == 'q':
00112 rospy.signal_shutdown(0)
00113 elif cmd == 'h':
00114 print help_msg
00115
00116 @thread
00117 def calibrate_threaded(self):
00118 self.fn_ready = False
00119 raw_input('Look at the left edge of the screen')
00120 with self.pose_lock:
00121 self.yaw_left = self.last_pose[5]
00122 raw_input('Look at the right edge of the screen')
00123 with self.pose_lock:
00124 self.yaw_right = self.last_pose[5]
00125 raw_input('Look at the top edge of the screen')
00126 with self.pose_lock:
00127 self.pitch_up = self.last_pose[4]
00128 raw_input('Look at the bottom edge of the screen')
00129 with self.pose_lock:
00130 self.pitch_dn = self.last_pose[4]
00131 self.fn_ready = True
00132
00133 def calibrate(self):
00134 self.ready = False
00135 screen = Xlib.display.Display().screen()
00136 res = (screen['width_in_pixels'], screen['height_in_pixels'])
00137 while not self.last_pose is not None and not rospy.is_shutdown():
00138 rospy.sleep(0.1)
00139 self.calibrate_threaded()
00140 while not self.fn_ready and not rospy.is_shutdown():
00141 rospy.sleep(0.1)
00142
00143 self.xFun = InterpolationFactory(tan(self.yaw_left), tan(self.yaw_right), 0, res[0])
00144 self.yFun = InterpolationFactory(tan(self.pitch_up), tan(self.pitch_dn), 0, res[1])
00145
00146 self.ready = True
00147
00148 def subscribe(self):
00149 rospy.Subscriber('head_pose', PoseStamped, self.pose_sub)
00150 self.cfg_srv = Server(MouseConfig, self.config_cb)
00151
00152 def pose_sub(self, pose_msg):
00153 pose = pose_quat_to_euler(pose_msg)
00154 if self.ready:
00155 obs = [self.xFun(tan(pose[5])), self.yFun(tan(pose[4]))]
00156 x, y = [int(n) for n in self.filter.observation(obs)]
00157 if self.do_move_mouse:
00158 move_mouse(x, y)
00159 mouse_msg = PointStamped()
00160 mouse_msg.header.stamp = rospy.Time.now()
00161 mouse_msg.point.x = x
00162 mouse_msg.point.y = y
00163 mouse_msg.point.z = 0
00164 self.mouse_pub.publish(mouse_msg)
00165 else:
00166 with self.pose_lock:
00167 self.last_pose = pose
00168
00169 def config_cb(self, config, level):
00170 cov = np.matrix([[ 3844.37658853, 1483.79897381], [ 1483.79897381, 2648.80916764]])
00171 if config['filter_type'] == 1:
00172 rospy.loginfo('Setting window size to %s' % config['window_size'])
00173 self.filter = MeanFilter(window_size=config['window_size'])
00174 elif config['filter_type'] == 2:
00175 if type(self.filter) == KalmanFilter:
00176 rospy.loginfo('Setting covariance multiplier to %s' % config['cov_mul'])
00177 self.filter.set_cov(cov*config['cov_mul'])
00178 else:
00179 rospy.loginfo('Switching to Kalman Filter with covariance multiplier to %s' % config['cov_mul'])
00180 self.filter = KalmanFilter(cov*config['cov_mul'], 2, 2)
00181
00182 return config
00183
00184 if __name__ == '__main__':
00185 rospy.init_node('mouse')
00186 m = Mouse()
00187 rospy.spin()