-
Notifications
You must be signed in to change notification settings - Fork 5
/
Mouse2.py
92 lines (83 loc) · 2.52 KB
/
Mouse2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
#!/usr/bin/env python
import rospy
import sys
import usb.core
import usb.util
from position_tracking.msg import Mouse_data
md = Mouse_data()
md.x=0.0
md.y=0.0
x=0.0
y=0.0
factor = 0.0000077244
def register_data(event):
print str(event.current_real)
md.x=round(x,5)
md.x=round(x,5)
md.y=round(y,5)
x=0.0
y=0.0
rospy.loginfo(md)
publish.publish(md)
if __name__=='__main__':
print 'in main'
# decimal vendor and product values
#dev = usb.core.find(idVendor=10077, idProduct=2982)
#this id is found by using the finddevices code
# or, uncomment the next line to search instead by the hexidecimal equivalent
dev = usb.core.find(idVendor=0x046d, idProduct=0xc084)
#dev = usb.core.find(idVendor=0x275d, idProduct=0xba6)
# first endpoint
interface = 0
endpoint = dev[0][(0,0)][0]
# if the OS kernel already claimed the device, which is most likely true
# thanks to http://stackoverflow.com/questions/8218683/pyusb-cannot-set-configuration
if dev.is_kernel_driver_active(interface) is True:
# tell the kernel to detach
dev.detach_kernel_driver(interface)
# claim the device
usb.util.claim_interface(dev, interface)
#md = Mouse_data()
#md.x=0.0
#md.y=0.0
#x=0.0
#y=0.0
#factor = 0.0000077244
#count=25
rospy.init_node('mouse_data', anonymous =True)
publish = rospy.Publisher('mouse_odom', Mouse_data , queue_size = 10)
rate = rospy.Rate(125)
while not rospy.is_shutdown():
try:
data = dev.read(endpoint.bEndpointAddress,endpoint.wMaxPacketSize)
if data[5]>=127: #IF the mouse is moving in the positiv$
y=y+(256-data[4])*factor
else:
y=y-data[4]*factor
if data[3]>=127: #IF the mouse is moving in the negativ$
x=x-(256-data[2])*factor
else:
x=x+(data[2])*factor
except usb.core.USBError as e:
data = None
if e.args == ('Operation timed out',):
continue
rospy.Timer(rospy.Duration(0.2),register_data)
# count-=1
# if count==0 :
# md.x=round(x,5)
# md.y=round(y,5)
# x=0.0
# y=0.0
# rospy.loginfo(md)
# publish.publish(md)
# count=25
# rospy.loginfo(md)
# publish.publish(md)
rate.sleep()
# release the device
usb.util.release_interface(dev, interface)
# reattach the device to the OS kernel
dev.attach_kernel_driver(interface)
if __name__=='__main__':
print 'in main'