-
Notifications
You must be signed in to change notification settings - Fork 28
/
wwSensors.py
187 lines (154 loc) · 7.15 KB
/
wwSensors.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
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
from WonderPy.core.wwConstants import WWRobotConstants
from WonderPy.util import wwMath
from WonderPy.components.wwSensorButton import WWSensorButton
from WonderPy.components.wwSensorAccelerometer import WWSensorAccelerometer
from WonderPy.components.wwSensorPose import WWSensorPose
from WonderPy.components.wwSensorPing import WWSensorPing
from WonderPy.components.wwSensorMedia import WWSensorMedia
from WonderPy.components.wwSensorAngle import WWSensorAngle
from WonderPy.components.wwSensorDistance import WWSensorDistance
from WonderPy.components.wwSensorBeacon import WWSensorBeacon,WWSensorBeaconV2
from WonderPy.components.wwSensorWheel import WWSensorWheel
from WonderPy.components.wwSensorGyroscope import WWSensorGyroscope
_rc = WWRobotConstants.RobotComponent
class WWSensors(object):
def __init__(self, robot):
self.setup_all_sensors(robot)
# the approach here is to have an object for ALL POSSIBLE sensors.
# each packet which comes in will mark each contained sensor as "valid".
# so client code should check `if WWSensors.whatever.valid` .
# there are a couple cases where the sensors might not contain a given sensor:
# * if the sensor is not supported by a particular robot.
# for example, Dot has no gyroscope or wheel sensors.
# * some sensors are 'sparse', meaning the robot only sends them periodically.
# for example, the battery level sensor is only sent every 10 packets or so.
# * some sensors are 'events', meaning they're only sent when an event is detected.
# an example is voice- or clap-detection.
def setup_all_sensors(self, robot):
self._sensor_dict = None
# common
self._accelerometer = WWSensorAccelerometer(robot)
self._animation = WWSensorMedia (robot)
self._button_main = WWSensorButton (robot)
self._button_1 = WWSensorButton (robot)
self._button_2 = WWSensorButton (robot)
self._button_3 = WWSensorButton (robot)
self._ping = WWSensorPing (robot)
self._speaker = WWSensorMedia (robot)
self._voice = None
# dash & cue
self._beacon = WWSensorBeacon (robot)
self._beacon_v2 = WWSensorBeaconV2 (robot)
self._distance_front_left_facing = WWSensorDistance (robot)
self._distance_front_right_facing = WWSensorDistance (robot)
self._distance_rear = WWSensorDistance (robot)
self._gyroscope = WWSensorGyroscope (robot)
self._head_pan = WWSensorAngle (robot, wwMath.coords_api_to_json_pan )
self._head_tilt = WWSensorAngle (robot, wwMath.coords_api_to_json_tilt)
self._pose = WWSensorPose (robot)
self._wheel_left = WWSensorWheel (robot)
self._wheel_right = WWSensorWheel (robot)
self._component_look_up = {
_rc.WW_SENSOR_ACCELEROMETER : self._accelerometer,
_rc.WW_SENSOR_ANIMATION_PLAYING : self._animation,
_rc.WW_SENSOR_BEACON : self._beacon,
_rc.WW_SENSOR_BEACON_V2 : self._beacon_v2,
_rc.WW_SENSOR_BODY_POSE : self._pose,
_rc.WW_SENSOR_BUTTON_1 : self._button_1,
_rc.WW_SENSOR_BUTTON_2 : self._button_2,
_rc.WW_SENSOR_BUTTON_3 : self._button_3,
_rc.WW_SENSOR_BUTTON_MAIN : self._button_main,
_rc.WW_SENSOR_DISTANCE_BACK : self._distance_rear,
_rc.WW_SENSOR_DISTANCE_FRONT_LEFT_FACING : self._distance_front_left_facing,
_rc.WW_SENSOR_DISTANCE_FRONT_RIGHT_FACING : self._distance_front_right_facing,
_rc.WW_SENSOR_ENCODER_LEFT_WHEEL : self._wheel_left,
_rc.WW_SENSOR_ENCODER_RIGHT_WHEEL : self._wheel_right,
_rc.WW_SENSOR_HEAD_POSITION_PAN : self._head_pan,
_rc.WW_SENSOR_HEAD_POSITION_TILT : self._head_tilt,
_rc.WW_SENSOR_GYROSCOPE : self._gyroscope,
_rc.WW_SENSOR_PING_RESPONSE : self._ping,
_rc.WW_SENSOR_SOUND_PLAYING : self._speaker,
}
@property
def accelerometer(self):
return self._accelerometer
@property
def animation(self):
return self._animation
@property
def beacon(self):
return self._beacon
@property
def beacon_v2(self):
return self._beacon_v2
@property
def button_1(self):
return self._button_1
@property
def button_2(self):
return self._button_2
@property
def button_3(self):
return self._button_3
@property
def button_main(self):
return self._button_main
@property
def distance_front_left_facing(self):
return self._distance_front_left_facing
@property
def distance_front_right_facing(self):
return self._distance_front_right_facing
@property
def distance_rear(self):
return self._distance_rear
@property
def head_pan(self):
return self._head_pan
@property
def head_tilt(self):
return self._head_tilt
@property
def gyroscope(self):
return self._gyroscope
@property
def ping(self):
return self._ping
@property
def pose(self):
return self._pose
@property
def speaker(self):
return self._speaker
@property
def wheel_left(self):
return self._wheel_left
@property
def wheel_right(self):
return self._wheel_right
def parse(self, sensorDict):
for component_id in sensorDict:
if component_id not in self._component_look_up:
# print("error: unhandled sensor component id: %s" % (component_id))
continue
else:
component = self._component_look_up[component_id]
if component is not None:
component.parse(sensorDict[component_id])
self._backfill_beacon(sensorDict)
def _backfill_beacon(self, sensor_dict):
# we only get beacon sensors if something is seen, and not if something is not seen.
if _rc.WW_SENSOR_BEACON in sensor_dict:
return
if self._beacon is not None:
self._beacon.parse({})
def description(self):
ret = ""
delim = ""
for component_id in self._component_look_up:
component = self._component_look_up[component_id]
if (component is not None) and (component.valid):
ret = ret + delim + "%-40s: %s" % (WWRobotConstants.RobotComponent.names[component_id],
component.description())
delim = "\n"
return ret