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 import rospy
29
30 import baxter_dataflow
31
32 from baxter_core_msgs.msg import (
33 ITBState,
34 )
35 from baxter_interface import (
36 digital_io,
37 )
41 """
42 Interface class for a Navigator on the Baxter robot.
43
44 Inputs:
45 Button 0 - press wheel
46 Button 1 - above wheel
47 Button 2 - below wheel
48 Scroll wheel - 0-255
49
50 Outputs:
51 Inner LED
52 Outer LED
53
54 Signals:
55 button0_changed - True/False
56 button1_changed - True/False
57 button2_changed - True/False
58 wheel_changed - New wheel value
59
60 Valid identifiers:
61 left, right, torso_left, torso_right
62 """
63
64 __LOCATIONS = ('left', 'right', 'torso_left', 'torso_right')
65
67 """
68 Constructor.
69
70 @type location: str
71 @param location: body location (prefix) of Navigator to control.
72
73 Valid locations are in L{Navigator.__LOCATIONS}::
74 left, right, torso_left, torso_right
75 """
76 if not location in self.__LOCATIONS:
77 raise AttributeError("Invalid Navigator name '%s'" % (location,))
78 self._id = location
79 self._state = None
80 self.button0_changed = baxter_dataflow.Signal()
81 self.button1_changed = baxter_dataflow.Signal()
82 self.button2_changed = baxter_dataflow.Signal()
83 self.wheel_changed = baxter_dataflow.Signal()
84
85 nav_state_topic = 'robot/itb/%s_itb/state' % (self._id,)
86 self._state_sub = rospy.Subscriber(
87 nav_state_topic,
88 ITBState,
89 self._on_state)
90
91 self._inner_led = digital_io.DigitalIO(
92 '%s_itb_light_inner' % (self._id,))
93
94 self._outer_led = digital_io.DigitalIO(
95 '%s_itb_light_outer' % (self._id,))
96
97 init_err_msg = ("Navigator init failed to get current state from %s" %
98 (nav_state_topic,))
99 baxter_dataflow.wait_for(lambda: self._state != None,
100 timeout_msg=init_err_msg)
101
102 @property
104 """
105 Current state of the wheel
106 """
107 return self._state.wheel
108
109 @property
115
116 @property
122
123 @property
129
130 @property
132 """
133 Current state of the inner LED
134 """
135 return self._state.innerLight
136
137 @inner_led.setter
139 """
140 Control the inner LED.
141
142 @type enable: bool
143 @param enable: True to enable the light, False otherwise
144 """
145 self._inner_led.set_output(enable)
146
147 @property
149 """
150 Current state of the outer LED.
151 """
152 return self._state.outerLight
153
154 @outer_led.setter
156 """
157 Control the outer LED.
158
159 @type enable: bool
160 @param enable: True to enable the light, False otherwise
161 """
162 self._outer_led.set_output(enable)
163
165 if not self._state:
166 self._state = msg
167
168 if self._state == msg:
169 return
170
171 old_state = self._state
172 self._state = msg
173
174 buttons = [self.button0_changed,
175 self.button1_changed,
176 self.button2_changed
177 ]
178 for i, signal in enumerate(buttons):
179 if old_state.buttons[i] != msg.buttons[i]:
180 signal(msg.buttons[i])
181
182 if old_state.wheel != msg.wheel:
183 diff = msg.wheel - old_state.wheel
184 if abs(diff % 256) < 127:
185 self.wheel_changed(diff % 256)
186 else:
187 self.wheel_changed(diff % (-256))
188