def config_cb(self, config, level):
cov = np.matrix([[ 3844.37658853, 1483.79897381], [ 1483.79897381, 2648.80916764]])
if config['filter_type'] == 1:
rospy.loginfo('Setting window size to %s' % config['window_size'])
self.filter = MeanFilter(window_size=config['window_size'])
elif config['filter_type'] == 2:
if type(self.filter) == KalmanFilter:
rospy.loginfo('Setting covariance multiplier to %s' % config['cov_mul'])
self.filter.set_cov(cov*config['cov_mul'])
else:
rospy.loginfo('Switching to Kalman Filter with covariance multiplier to %s' % config['cov_mul'])
self.filter = KalmanFilter(cov*config['cov_mul'], 2, 2)
return config
def config_cb(self, config, level):
cov = np.matrix([[3844.37658853, 1483.79897381],
[1483.79897381, 2648.80916764]])
if config['filter_type'] == 1:
rospy.loginfo('Setting window size to %s' % config['window_size'])
self.filter = MeanFilter(window_size=config['window_size'])
elif config['filter_type'] == 2:
if type(self.filter) == KalmanFilter:
rospy.loginfo('Setting covariance multiplier to %s' %
config['cov_mul'])
self.filter.set_cov(cov * config['cov_mul'])
else:
rospy.loginfo(
'Switching to Kalman Filter with covariance multiplier to %s'
% config['cov_mul'])
self.filter = KalmanFilter(cov * config['cov_mul'], 2, 2)
return config
class Mouse(object):
yaw_left = None
yaw_right = None
pitch_up = None
pitch_dn = None
screen_res = None
fn_ready = False
ready = False
run = False
xFun = None
yFun = None
last_pose = None
pose_lock = RLock()
do_move_mouse = True
# cov = np.matrix([[ 4.1208e3, 2.3380e3], [-5.3681, 1.9369e3]])
cov = np.matrix([[6.71485028, 18.75061549], [14.25320564, 82.71265061]])
filter = KalmanFilter(cov, 2, 2)
def __init__(self):
self.do_move_mouse = rospy.get_param('~move_mouse', False)
self.subscribe()
self.calibrate()
self.mouse_pub = rospy.Publisher('mouse', PointStamped)
self.listen_for_keyboard()
@thread
def listen_for_keyboard(self):
help_msg = 'Keboard commands:\n'
help_msg += ' Recalibrate: c\n'
help_msg += ' Disable Mouse: d\n'
help_msg += ' Enable Mouse: e\n'
help_msg += ' Print Help: h\n'
help_msg += ' Exit: q\n'
print help_msg
while not rospy.is_shutdown():
cmd = raw_input('-->')
if cmd == 'c':
self.calibrate()
elif cmd == 'd':
self.ready = False
elif cmd == 'e':
self.ready = True
elif cmd == 'q':
rospy.signal_shutdown(0)
elif cmd == 'h':
print help_msg
@thread
def calibrate_threaded(self):
self.fn_ready = False
raw_input('Look at the left edge of the screen')
with self.pose_lock:
self.yaw_left = self.last_pose[5]
raw_input('Look at the right edge of the screen')
with self.pose_lock:
self.yaw_right = self.last_pose[5]
raw_input('Look at the top edge of the screen')
with self.pose_lock:
self.pitch_up = self.last_pose[4]
raw_input('Look at the bottom edge of the screen')
with self.pose_lock:
self.pitch_dn = self.last_pose[4]
self.fn_ready = True
def calibrate(self):
self.ready = False
screen = Xlib.display.Display().screen()
res = (screen['width_in_pixels'], screen['height_in_pixels'])
while not self.last_pose is not None and not rospy.is_shutdown():
rospy.sleep(0.1)
self.calibrate_threaded()
while not self.fn_ready and not rospy.is_shutdown():
rospy.sleep(0.1)
self.xFun = InterpolationFactory(tan(self.yaw_left),
tan(self.yaw_right), 0, res[0])
self.yFun = InterpolationFactory(tan(self.pitch_up),
tan(self.pitch_dn), 0, res[1])
self.ready = True
def subscribe(self):
rospy.Subscriber('head_pose', PoseStamped, self.pose_sub)
self.cfg_srv = Server(MouseConfig, self.config_cb)
def pose_sub(self, pose_msg):
pose = pose_quat_to_euler(pose_msg)
if self.ready:
obs = [self.xFun(tan(pose[5])), self.yFun(tan(pose[4]))]
x, y = [int(n) for n in self.filter.observation(obs)]
if self.do_move_mouse:
move_mouse(x, y)
mouse_msg = PointStamped()
mouse_msg.header.stamp = rospy.Time.now()
mouse_msg.point.x = x
mouse_msg.point.y = y
mouse_msg.point.z = 0
self.mouse_pub.publish(mouse_msg)
else:
with self.pose_lock:
self.last_pose = pose
def config_cb(self, config, level):
cov = np.matrix([[3844.37658853, 1483.79897381],
[1483.79897381, 2648.80916764]])
if config['filter_type'] == 1:
rospy.loginfo('Setting window size to %s' % config['window_size'])
self.filter = MeanFilter(window_size=config['window_size'])
elif config['filter_type'] == 2:
if type(self.filter) == KalmanFilter:
rospy.loginfo('Setting covariance multiplier to %s' %
config['cov_mul'])
self.filter.set_cov(cov * config['cov_mul'])
else:
rospy.loginfo(
'Switching to Kalman Filter with covariance multiplier to %s'
% config['cov_mul'])
self.filter = KalmanFilter(cov * config['cov_mul'], 2, 2)
return config
class Mouse(object):
yaw_left = None
yaw_right = None
pitch_up = None
pitch_dn = None
screen_res = None
fn_ready = False
ready = False
run = False
xFun = None
yFun = None
last_pose = None
pose_lock = RLock()
do_move_mouse = True
# cov = np.matrix([[ 4.1208e3, 2.3380e3], [-5.3681, 1.9369e3]])
cov = np.matrix([[6.71485028, 18.75061549], [ 14.25320564, 82.71265061]])
filter = KalmanFilter(cov, 2, 2)
def __init__(self):
self.do_move_mouse = rospy.get_param('~move_mouse', False)
self.subscribe()
self.calibrate()
self.mouse_pub = rospy.Publisher('mouse', PointStamped)
self.listen_for_keyboard()
@thread
def listen_for_keyboard(self):
help_msg = 'Keboard commands:\n'
help_msg += ' Recalibrate: c\n'
help_msg += ' Disable Mouse: d\n'
help_msg += ' Enable Mouse: e\n'
help_msg += ' Print Help: h\n'
help_msg += ' Exit: q\n'
print help_msg
while not rospy.is_shutdown():
cmd = raw_input('-->')
if cmd == 'c':
self.calibrate()
elif cmd == 'd':
self.ready = False
elif cmd == 'e':
self.ready = True
elif cmd == 'q':
rospy.signal_shutdown(0)
elif cmd == 'h':
print help_msg
@thread
def calibrate_threaded(self):
self.fn_ready = False
raw_input('Look at the left edge of the screen')
with self.pose_lock:
self.yaw_left = self.last_pose[5]
raw_input('Look at the right edge of the screen')
with self.pose_lock:
self.yaw_right = self.last_pose[5]
raw_input('Look at the top edge of the screen')
with self.pose_lock:
self.pitch_up = self.last_pose[4]
raw_input('Look at the bottom edge of the screen')
with self.pose_lock:
self.pitch_dn = self.last_pose[4]
self.fn_ready = True
def calibrate(self):
self.ready = False
screen = Xlib.display.Display().screen()
res = (screen['width_in_pixels'], screen['height_in_pixels'])
while not self.last_pose is not None and not rospy.is_shutdown():
rospy.sleep(0.1)
self.calibrate_threaded()
while not self.fn_ready and not rospy.is_shutdown():
rospy.sleep(0.1)
self.xFun = InterpolationFactory(tan(self.yaw_left), tan(self.yaw_right), 0, res[0])
self.yFun = InterpolationFactory(tan(self.pitch_up), tan(self.pitch_dn), 0, res[1])
self.ready = True
def subscribe(self):
rospy.Subscriber('head_pose', PoseStamped, self.pose_sub)
self.cfg_srv = Server(MouseConfig, self.config_cb)
def pose_sub(self, pose_msg):
pose = pose_quat_to_euler(pose_msg)
if self.ready:
obs = [self.xFun(tan(pose[5])), self.yFun(tan(pose[4]))]
x, y = [int(n) for n in self.filter.observation(obs)]
if self.do_move_mouse:
move_mouse(x, y)
mouse_msg = PointStamped()
mouse_msg.header.stamp = rospy.Time.now()
mouse_msg.point.x = x
mouse_msg.point.y = y
mouse_msg.point.z = 0
self.mouse_pub.publish(mouse_msg)
else:
with self.pose_lock:
self.last_pose = pose
def config_cb(self, config, level):
cov = np.matrix([[ 3844.37658853, 1483.79897381], [ 1483.79897381, 2648.80916764]])
if config['filter_type'] == 1:
rospy.loginfo('Setting window size to %s' % config['window_size'])
self.filter = MeanFilter(window_size=config['window_size'])
elif config['filter_type'] == 2:
if type(self.filter) == KalmanFilter:
rospy.loginfo('Setting covariance multiplier to %s' % config['cov_mul'])
self.filter.set_cov(cov*config['cov_mul'])
else:
rospy.loginfo('Switching to Kalman Filter with covariance multiplier to %s' % config['cov_mul'])
self.filter = KalmanFilter(cov*config['cov_mul'], 2, 2)
return config
