wirepod-vector-sdk-audio 0.9.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- anki_vector/__init__.py +43 -0
- anki_vector/animation.py +272 -0
- anki_vector/annotate.py +590 -0
- anki_vector/audio.py +212 -0
- anki_vector/audio_stream.py +335 -0
- anki_vector/behavior.py +1135 -0
- anki_vector/camera.py +670 -0
- anki_vector/camera_viewer/__init__.py +121 -0
- anki_vector/color.py +88 -0
- anki_vector/configure/__main__.py +331 -0
- anki_vector/connection.py +838 -0
- anki_vector/events.py +420 -0
- anki_vector/exceptions.py +185 -0
- anki_vector/faces.py +819 -0
- anki_vector/lights.py +210 -0
- anki_vector/mdns.py +131 -0
- anki_vector/messaging/__init__.py +45 -0
- anki_vector/messaging/alexa_pb2.py +36 -0
- anki_vector/messaging/alexa_pb2_grpc.py +3 -0
- anki_vector/messaging/behavior_pb2.py +40 -0
- anki_vector/messaging/behavior_pb2_grpc.py +3 -0
- anki_vector/messaging/client.py +33 -0
- anki_vector/messaging/cube_pb2.py +113 -0
- anki_vector/messaging/cube_pb2_grpc.py +3 -0
- anki_vector/messaging/extensions_pb2.py +25 -0
- anki_vector/messaging/extensions_pb2_grpc.py +3 -0
- anki_vector/messaging/external_interface_pb2.py +169 -0
- anki_vector/messaging/external_interface_pb2_grpc.py +1267 -0
- anki_vector/messaging/messages_pb2.py +431 -0
- anki_vector/messaging/messages_pb2_grpc.py +3 -0
- anki_vector/messaging/nav_map_pb2.py +33 -0
- anki_vector/messaging/nav_map_pb2_grpc.py +3 -0
- anki_vector/messaging/protocol.py +33 -0
- anki_vector/messaging/response_status_pb2.py +27 -0
- anki_vector/messaging/response_status_pb2_grpc.py +3 -0
- anki_vector/messaging/settings_pb2.py +72 -0
- anki_vector/messaging/settings_pb2_grpc.py +3 -0
- anki_vector/messaging/shared_pb2.py +54 -0
- anki_vector/messaging/shared_pb2_grpc.py +3 -0
- anki_vector/motors.py +127 -0
- anki_vector/nav_map.py +409 -0
- anki_vector/objects.py +1782 -0
- anki_vector/opengl/__init__.py +103 -0
- anki_vector/opengl/assets/LICENSE.txt +21 -0
- anki_vector/opengl/assets/cube.jpg +0 -0
- anki_vector/opengl/assets/cube.mtl +9 -0
- anki_vector/opengl/assets/cube.obj +1000 -0
- anki_vector/opengl/assets/vector.mtl +67 -0
- anki_vector/opengl/assets/vector.obj +13220 -0
- anki_vector/opengl/opengl.py +864 -0
- anki_vector/opengl/opengl_vector.py +620 -0
- anki_vector/opengl/opengl_viewer.py +689 -0
- anki_vector/photos.py +145 -0
- anki_vector/proximity.py +176 -0
- anki_vector/reserve_control/__main__.py +36 -0
- anki_vector/robot.py +930 -0
- anki_vector/screen.py +201 -0
- anki_vector/status.py +322 -0
- anki_vector/touch.py +119 -0
- anki_vector/user_intent.py +186 -0
- anki_vector/util.py +1132 -0
- anki_vector/version.py +15 -0
- anki_vector/viewer.py +403 -0
- anki_vector/vision.py +202 -0
- anki_vector/world.py +899 -0
- wirepod_vector_sdk_audio-0.9.0.dist-info/METADATA +80 -0
- wirepod_vector_sdk_audio-0.9.0.dist-info/RECORD +71 -0
- wirepod_vector_sdk_audio-0.9.0.dist-info/WHEEL +5 -0
- wirepod_vector_sdk_audio-0.9.0.dist-info/licenses/LICENSE.txt +180 -0
- wirepod_vector_sdk_audio-0.9.0.dist-info/top_level.txt +1 -0
- wirepod_vector_sdk_audio-0.9.0.dist-info/zip-safe +1 -0
anki_vector/world.py
ADDED
|
@@ -0,0 +1,899 @@
|
|
|
1
|
+
# Copyright (c) 2018 Anki, Inc.
|
|
2
|
+
#
|
|
3
|
+
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
4
|
+
# you may not use this file except in compliance with the License.
|
|
5
|
+
# You may obtain a copy of the License in the file LICENSE.txt or at
|
|
6
|
+
#
|
|
7
|
+
# https://www.apache.org/licenses/LICENSE-2.0
|
|
8
|
+
#
|
|
9
|
+
# Unless required by applicable law or agreed to in writing, software
|
|
10
|
+
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
11
|
+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
12
|
+
# See the License for the specific language governing permissions and
|
|
13
|
+
# limitations under the License.
|
|
14
|
+
|
|
15
|
+
"""
|
|
16
|
+
Vector's known view of his world.
|
|
17
|
+
|
|
18
|
+
This view includes objects and faces it knows about and can currently
|
|
19
|
+
see with its camera.
|
|
20
|
+
"""
|
|
21
|
+
|
|
22
|
+
# __all__ should order by constants, event classes, other classes, functions.
|
|
23
|
+
__all__ = ['World']
|
|
24
|
+
|
|
25
|
+
from concurrent import futures
|
|
26
|
+
from typing import Iterable
|
|
27
|
+
|
|
28
|
+
from . import faces
|
|
29
|
+
from . import connection
|
|
30
|
+
from . import objects
|
|
31
|
+
from . import util
|
|
32
|
+
|
|
33
|
+
from .events import Events
|
|
34
|
+
from .messaging import protocol
|
|
35
|
+
|
|
36
|
+
|
|
37
|
+
class World(util.Component):
|
|
38
|
+
"""Represents the state of the world, as known to Vector."""
|
|
39
|
+
|
|
40
|
+
#: callable: The factory function that returns a
|
|
41
|
+
#: :class:`faces.Face` class or subclass instance
|
|
42
|
+
face_factory = faces.Face
|
|
43
|
+
|
|
44
|
+
#: callable: The factory function that returns an
|
|
45
|
+
#: :class:`objects.LightCube` class or subclass instance.
|
|
46
|
+
light_cube_factory = objects.LightCube
|
|
47
|
+
|
|
48
|
+
#: callable: The factory function that returns an
|
|
49
|
+
#: :class:`objects.Charger` class or subclass instance.
|
|
50
|
+
charger_factory = objects.Charger
|
|
51
|
+
|
|
52
|
+
#: callable: The factory function that returns an
|
|
53
|
+
#: :class:`objects.CustomObject` class or subclass instance.
|
|
54
|
+
custom_object_factory = objects.CustomObject
|
|
55
|
+
|
|
56
|
+
#: callable: The factory function that returns an
|
|
57
|
+
#: :class:`objects.FixedCustomObject` class or subclass instance.
|
|
58
|
+
fixed_custom_object_factory = objects.FixedCustomObject
|
|
59
|
+
|
|
60
|
+
def __init__(self, robot):
|
|
61
|
+
super().__init__(robot)
|
|
62
|
+
|
|
63
|
+
self._custom_object_archetypes = {}
|
|
64
|
+
|
|
65
|
+
# Objects by type
|
|
66
|
+
self._faces = {}
|
|
67
|
+
self._light_cube = {objects.LIGHT_CUBE_1_TYPE: self.light_cube_factory(robot=robot)}
|
|
68
|
+
self._custom_objects = {}
|
|
69
|
+
|
|
70
|
+
#: :class:`anki_vector.objects.Charger`: Vector's charger.
|
|
71
|
+
#: ``None`` if no charger connected or known about yet.
|
|
72
|
+
self._charger = None # type: anki_vector.objects.Charger
|
|
73
|
+
|
|
74
|
+
# All objects
|
|
75
|
+
self._objects = {}
|
|
76
|
+
|
|
77
|
+
# Subscribe to callbacks that updates the world view
|
|
78
|
+
self._robot.events.subscribe(self._on_face_observed,
|
|
79
|
+
Events.robot_observed_face,
|
|
80
|
+
_on_connection_thread=True)
|
|
81
|
+
|
|
82
|
+
self._robot.events.subscribe(self._on_object_observed,
|
|
83
|
+
Events.robot_observed_object,
|
|
84
|
+
_on_connection_thread=True)
|
|
85
|
+
|
|
86
|
+
#### Public Properties ####
|
|
87
|
+
|
|
88
|
+
@property
|
|
89
|
+
def all_objects(self):
|
|
90
|
+
"""generator: yields each object in the world.
|
|
91
|
+
|
|
92
|
+
.. testcode::
|
|
93
|
+
|
|
94
|
+
# Print the all objects' class details
|
|
95
|
+
import anki_vector
|
|
96
|
+
with anki_vector.Robot() as robot:
|
|
97
|
+
for obj in robot.world.all_objects:
|
|
98
|
+
print(obj)
|
|
99
|
+
|
|
100
|
+
Returns:
|
|
101
|
+
A generator yielding :class:`anki_vector.faces.Face`, :class:`anki_vector.faces.LightCube`,
|
|
102
|
+
:class:`anki_vector.faces.Charger`, :class:`anki_vector.faces.CustomObject` and
|
|
103
|
+
:class:`anki_vector.faces.FixedCustomObject` instances.
|
|
104
|
+
"""
|
|
105
|
+
for obj in self._objects.values():
|
|
106
|
+
yield obj
|
|
107
|
+
|
|
108
|
+
@property
|
|
109
|
+
def visible_faces(self) -> Iterable[faces.Face]:
|
|
110
|
+
"""generator: yields each face that Vector can currently see.
|
|
111
|
+
|
|
112
|
+
.. testcode::
|
|
113
|
+
|
|
114
|
+
import time
|
|
115
|
+
|
|
116
|
+
import anki_vector
|
|
117
|
+
from anki_vector.events import Events
|
|
118
|
+
from anki_vector.util import degrees
|
|
119
|
+
|
|
120
|
+
def test_subscriber(robot, event_type, event):
|
|
121
|
+
print(f"Subscriber called for: {event_type} = {event}")
|
|
122
|
+
|
|
123
|
+
for face in robot.world.visible_faces:
|
|
124
|
+
print("--- Face attributes ---")
|
|
125
|
+
print(f"Face id: {face.face_id}")
|
|
126
|
+
print(f"Updated face id: {face.updated_face_id}")
|
|
127
|
+
print(f"Name: {face.name}")
|
|
128
|
+
print(f"Expression: {face.expression}")
|
|
129
|
+
print(f"Timestamp: {face.last_observed_time}")
|
|
130
|
+
print(f"Pose: {face.pose}")
|
|
131
|
+
print(f"Image Rect: {face.last_observed_image_rect}")
|
|
132
|
+
print(f"Expression score: {face.expression_score}")
|
|
133
|
+
print(f"Left eye: {face.left_eye}")
|
|
134
|
+
print(f"Right eye: {face.right_eye}")
|
|
135
|
+
print(f"Nose: {face.nose}")
|
|
136
|
+
print(f"Mouth: {face.mouth}")
|
|
137
|
+
|
|
138
|
+
with anki_vector.Robot(enable_face_detection=True) as robot:
|
|
139
|
+
# If necessary, move Vector's Head and Lift to make it easy to see his face
|
|
140
|
+
robot.behavior.set_head_angle(degrees(45.0))
|
|
141
|
+
robot.behavior.set_lift_height(0.0)
|
|
142
|
+
|
|
143
|
+
robot.events.subscribe(test_subscriber, Events.robot_changed_observed_face_id)
|
|
144
|
+
robot.events.subscribe(test_subscriber, Events.robot_observed_face)
|
|
145
|
+
|
|
146
|
+
print("------ show vector your face, press ctrl+c to exit early ------")
|
|
147
|
+
try:
|
|
148
|
+
time.sleep(10)
|
|
149
|
+
except KeyboardInterrupt:
|
|
150
|
+
robot.disconnect()
|
|
151
|
+
|
|
152
|
+
Returns:
|
|
153
|
+
A generator yielding :class:`anki_vector.faces.Face` instances.
|
|
154
|
+
"""
|
|
155
|
+
for face in self._faces.values():
|
|
156
|
+
if face.is_visible:
|
|
157
|
+
yield face
|
|
158
|
+
|
|
159
|
+
@property
|
|
160
|
+
def custom_object_archetypes(self) -> Iterable[objects.CustomObjectArchetype]:
|
|
161
|
+
"""generator: yields each custom object archetype that Vector will look for.
|
|
162
|
+
|
|
163
|
+
See :class:`objects.CustomObjectMarkers`.
|
|
164
|
+
|
|
165
|
+
.. testcode::
|
|
166
|
+
|
|
167
|
+
import anki_vector
|
|
168
|
+
with anki_vector.Robot() as robot:
|
|
169
|
+
for obj in robot.world.custom_object_archetypes:
|
|
170
|
+
print(obj)
|
|
171
|
+
|
|
172
|
+
Returns:
|
|
173
|
+
A generator yielding CustomObjectArchetype instances
|
|
174
|
+
"""
|
|
175
|
+
for obj in self._custom_object_archetypes.values():
|
|
176
|
+
yield obj
|
|
177
|
+
|
|
178
|
+
@property
|
|
179
|
+
def visible_custom_objects(self) -> Iterable[objects.CustomObject]:
|
|
180
|
+
"""generator: yields each custom object that Vector can currently see.
|
|
181
|
+
|
|
182
|
+
See :class:`objects.CustomObjectMarkers`.
|
|
183
|
+
|
|
184
|
+
.. testcode::
|
|
185
|
+
|
|
186
|
+
import anki_vector
|
|
187
|
+
with anki_vector.Robot() as robot:
|
|
188
|
+
for obj in robot.world.visible_custom_objects:
|
|
189
|
+
print(obj)
|
|
190
|
+
|
|
191
|
+
Returns:
|
|
192
|
+
A generator yielding CustomObject instances
|
|
193
|
+
"""
|
|
194
|
+
for obj in self._custom_objects.values():
|
|
195
|
+
if obj.is_visible:
|
|
196
|
+
yield obj
|
|
197
|
+
|
|
198
|
+
@property
|
|
199
|
+
def visible_objects(self) -> Iterable[objects.ObservableObject]:
|
|
200
|
+
"""generator: yields each object that Vector can currently see.
|
|
201
|
+
|
|
202
|
+
.. testcode::
|
|
203
|
+
|
|
204
|
+
import anki_vector
|
|
205
|
+
with anki_vector.Robot() as robot:
|
|
206
|
+
for obj in robot.world.visible_objects:
|
|
207
|
+
print(obj)
|
|
208
|
+
|
|
209
|
+
Returns:
|
|
210
|
+
A generator yielding Charger, LightCube and CustomObject instances
|
|
211
|
+
"""
|
|
212
|
+
for obj in self._objects.values():
|
|
213
|
+
if obj.is_visible:
|
|
214
|
+
yield obj
|
|
215
|
+
|
|
216
|
+
@property
|
|
217
|
+
def connected_light_cube(self) -> objects.LightCube:
|
|
218
|
+
"""A light cube connected to Vector, if any.
|
|
219
|
+
|
|
220
|
+
.. testcode::
|
|
221
|
+
|
|
222
|
+
import anki_vector
|
|
223
|
+
|
|
224
|
+
with anki_vector.Robot() as robot:
|
|
225
|
+
robot.world.connect_cube()
|
|
226
|
+
if robot.world.connected_light_cube:
|
|
227
|
+
dock_response = robot.behavior.dock_with_cube(robot.world.connected_light_cube)
|
|
228
|
+
"""
|
|
229
|
+
result = None
|
|
230
|
+
cube = self._light_cube.get(objects.LIGHT_CUBE_1_TYPE)
|
|
231
|
+
if cube and cube.is_connected:
|
|
232
|
+
result = cube
|
|
233
|
+
|
|
234
|
+
return result
|
|
235
|
+
|
|
236
|
+
@property
|
|
237
|
+
def light_cube(self) -> objects.LightCube:
|
|
238
|
+
"""Returns the vector light cube object, regardless of its connection status.
|
|
239
|
+
|
|
240
|
+
.. testcode::
|
|
241
|
+
|
|
242
|
+
import anki_vector
|
|
243
|
+
with anki_vector.Robot() as robot:
|
|
244
|
+
cube = robot.world.light_cube
|
|
245
|
+
if cube:
|
|
246
|
+
if cube.is_connected:
|
|
247
|
+
print("LightCube is connected.")
|
|
248
|
+
else:
|
|
249
|
+
print("LightCube isn't connected")
|
|
250
|
+
|
|
251
|
+
Raises:
|
|
252
|
+
:class:`ValueError` if the cube_id is invalid.
|
|
253
|
+
"""
|
|
254
|
+
cube = self._light_cube.get(objects.LIGHT_CUBE_1_TYPE)
|
|
255
|
+
# Only return the cube if it has an object_id
|
|
256
|
+
if cube.object_id is not None:
|
|
257
|
+
return cube
|
|
258
|
+
return None
|
|
259
|
+
|
|
260
|
+
@property
|
|
261
|
+
def charger(self) -> objects.Charger:
|
|
262
|
+
"""Returns the most recently observed Vector charger object, or None if no chargers have been observed.
|
|
263
|
+
|
|
264
|
+
.. testcode::
|
|
265
|
+
|
|
266
|
+
import anki_vector
|
|
267
|
+
|
|
268
|
+
# First, place Vector directly in front of his charger so he can observe it.
|
|
269
|
+
|
|
270
|
+
with anki_vector.Robot() as robot:
|
|
271
|
+
print('Most recently observed charger: {0}'.format(robot.world.charger))
|
|
272
|
+
"""
|
|
273
|
+
if self._charger is not None:
|
|
274
|
+
return self._charger
|
|
275
|
+
return None
|
|
276
|
+
|
|
277
|
+
#### Public Methods ####
|
|
278
|
+
|
|
279
|
+
def close(self):
|
|
280
|
+
"""The world will tear down all its faces and objects."""
|
|
281
|
+
|
|
282
|
+
# delete_custom_objects is called before the _objects are torn down to make sure the
|
|
283
|
+
# robot receives cues to remove the internal representations of these objects before
|
|
284
|
+
# we release the SDK side representations
|
|
285
|
+
self.delete_custom_objects()
|
|
286
|
+
|
|
287
|
+
for obj in self._objects.values():
|
|
288
|
+
obj.teardown()
|
|
289
|
+
|
|
290
|
+
self._robot.events.unsubscribe(self._on_face_observed,
|
|
291
|
+
Events.robot_observed_face)
|
|
292
|
+
|
|
293
|
+
self._robot.events.unsubscribe(self._on_object_observed,
|
|
294
|
+
Events.robot_observed_object)
|
|
295
|
+
|
|
296
|
+
def get_object(self, object_id: int):
|
|
297
|
+
"""Fetches an object instance with the given id.
|
|
298
|
+
|
|
299
|
+
.. testcode::
|
|
300
|
+
|
|
301
|
+
import anki_vector
|
|
302
|
+
|
|
303
|
+
with anki_vector.Robot() as robot:
|
|
304
|
+
# First get an existing object id, for instance:
|
|
305
|
+
valid_object_id = 1
|
|
306
|
+
|
|
307
|
+
# Then use the object_id to retrieve the object instance:
|
|
308
|
+
my_obj = robot.world.get_object(valid_object_id)
|
|
309
|
+
"""
|
|
310
|
+
return self._objects.get(object_id)
|
|
311
|
+
|
|
312
|
+
def get_face(self, face_id: int) -> faces.Face:
|
|
313
|
+
"""Fetches a Face instance with the given id.
|
|
314
|
+
|
|
315
|
+
.. testcode::
|
|
316
|
+
|
|
317
|
+
|
|
318
|
+
import time
|
|
319
|
+
|
|
320
|
+
import anki_vector
|
|
321
|
+
from anki_vector.events import Events
|
|
322
|
+
from anki_vector.util import degrees
|
|
323
|
+
|
|
324
|
+
def test_subscriber(robot, event_type, event):
|
|
325
|
+
for face in robot.world.visible_faces:
|
|
326
|
+
print(f"Face id: {face.face_id}")
|
|
327
|
+
|
|
328
|
+
with anki_vector.Robot(enable_face_detection=True) as robot:
|
|
329
|
+
# If necessary, move Vector's Head and Lift to make it easy to see his face
|
|
330
|
+
robot.behavior.set_head_angle(degrees(45.0))
|
|
331
|
+
robot.behavior.set_lift_height(0.0)
|
|
332
|
+
|
|
333
|
+
robot.events.subscribe(test_subscriber, Events.robot_changed_observed_face_id)
|
|
334
|
+
robot.events.subscribe(test_subscriber, Events.robot_observed_face)
|
|
335
|
+
|
|
336
|
+
print("------ show vector your face, press ctrl+c to exit early ------")
|
|
337
|
+
try:
|
|
338
|
+
time.sleep(5)
|
|
339
|
+
except KeyboardInterrupt:
|
|
340
|
+
robot.disconnect()
|
|
341
|
+
"""
|
|
342
|
+
return self._faces.get(face_id)
|
|
343
|
+
|
|
344
|
+
@connection.on_connection_thread()
|
|
345
|
+
async def connect_cube(self) -> protocol.ConnectCubeResponse:
|
|
346
|
+
"""Attempt to connect to a cube.
|
|
347
|
+
|
|
348
|
+
If a cube is currently connected, this will do nothing.
|
|
349
|
+
|
|
350
|
+
.. testcode::
|
|
351
|
+
|
|
352
|
+
import anki_vector
|
|
353
|
+
|
|
354
|
+
with anki_vector.Robot() as robot:
|
|
355
|
+
robot.world.connect_cube()
|
|
356
|
+
"""
|
|
357
|
+
req = protocol.ConnectCubeRequest()
|
|
358
|
+
result = await self.grpc_interface.ConnectCube(req)
|
|
359
|
+
|
|
360
|
+
# dispatch cube connected message
|
|
361
|
+
event = protocol.ObjectConnectionState(
|
|
362
|
+
object_id=result.object_id,
|
|
363
|
+
factory_id=result.factory_id,
|
|
364
|
+
connected=result.success,
|
|
365
|
+
object_type=objects.LIGHT_CUBE_1_TYPE)
|
|
366
|
+
|
|
367
|
+
await self._robot.events.dispatch_event(event, Events.object_connection_state)
|
|
368
|
+
|
|
369
|
+
return result
|
|
370
|
+
|
|
371
|
+
@connection.on_connection_thread()
|
|
372
|
+
async def disconnect_cube(self) -> protocol.DisconnectCubeResponse:
|
|
373
|
+
"""Requests a disconnection from the currently connected cube.
|
|
374
|
+
|
|
375
|
+
.. testcode::
|
|
376
|
+
|
|
377
|
+
import anki_vector
|
|
378
|
+
|
|
379
|
+
with anki_vector.Robot() as robot:
|
|
380
|
+
robot.world.disconnect_cube()
|
|
381
|
+
"""
|
|
382
|
+
req = protocol.DisconnectCubeRequest()
|
|
383
|
+
return await self.grpc_interface.DisconnectCube(req)
|
|
384
|
+
|
|
385
|
+
# TODO move out of world.py and into lights.py?
|
|
386
|
+
@connection.on_connection_thread()
|
|
387
|
+
async def flash_cube_lights(self) -> protocol.FlashCubeLightsResponse:
|
|
388
|
+
"""Flash cube lights
|
|
389
|
+
|
|
390
|
+
Plays the default cube connection animation on the currently
|
|
391
|
+
connected cube's lights.
|
|
392
|
+
|
|
393
|
+
.. testcode::
|
|
394
|
+
|
|
395
|
+
import anki_vector
|
|
396
|
+
with anki_vector.Robot() as robot:
|
|
397
|
+
robot.world.flash_cube_lights()
|
|
398
|
+
"""
|
|
399
|
+
req = protocol.FlashCubeLightsRequest()
|
|
400
|
+
return await self.grpc_interface.FlashCubeLights(req)
|
|
401
|
+
|
|
402
|
+
# TODO move out of world.py and into objects.py?
|
|
403
|
+
@connection.on_connection_thread(requires_control=False)
|
|
404
|
+
async def forget_preferred_cube(self) -> protocol.ForgetPreferredCubeResponse:
|
|
405
|
+
"""Forget preferred cube.
|
|
406
|
+
|
|
407
|
+
'Forget' the robot's preferred cube. This will cause the robot to
|
|
408
|
+
connect to the cube with the highest RSSI (signal strength) next
|
|
409
|
+
time a connection is requested.
|
|
410
|
+
|
|
411
|
+
.. testcode::
|
|
412
|
+
|
|
413
|
+
import anki_vector
|
|
414
|
+
|
|
415
|
+
with anki_vector.Robot() as robot:
|
|
416
|
+
robot.world.forget_preferred_cube()
|
|
417
|
+
"""
|
|
418
|
+
req = protocol.ForgetPreferredCubeRequest()
|
|
419
|
+
return await self.grpc_interface.ForgetPreferredCube(req)
|
|
420
|
+
|
|
421
|
+
# TODO move out of world.py and into objects.py?
|
|
422
|
+
@connection.on_connection_thread(requires_control=False)
|
|
423
|
+
async def set_preferred_cube(self, factory_id: str) -> protocol.SetPreferredCubeResponse:
|
|
424
|
+
"""Set preferred cube.
|
|
425
|
+
|
|
426
|
+
Set the robot's preferred cube and save it to disk. The robot
|
|
427
|
+
will always attempt to connect to this cube if it is available.
|
|
428
|
+
This is only used in simulation (for now).
|
|
429
|
+
|
|
430
|
+
.. testcode::
|
|
431
|
+
|
|
432
|
+
import anki_vector
|
|
433
|
+
|
|
434
|
+
with anki_vector.Robot() as robot:
|
|
435
|
+
connected_cube = robot.world.connected_light_cube
|
|
436
|
+
if connected_cube:
|
|
437
|
+
robot.world.set_preferred_cube(connected_cube.factory_id)
|
|
438
|
+
|
|
439
|
+
:param factory_id: The unique hardware id of the physical cube.
|
|
440
|
+
"""
|
|
441
|
+
req = protocol.SetPreferredCubeRequest(factory_id=factory_id)
|
|
442
|
+
return await self.grpc_interface.SetPreferredCube(req)
|
|
443
|
+
|
|
444
|
+
# TODO better place to put this method than world.py?
|
|
445
|
+
@connection.on_connection_thread(requires_control=False)
|
|
446
|
+
async def delete_custom_objects(self,
|
|
447
|
+
delete_custom_marker_objects: bool = True,
|
|
448
|
+
delete_fixed_custom_objects: bool = True,
|
|
449
|
+
delete_custom_object_archetypes: bool = True):
|
|
450
|
+
"""Causes the robot to forget about custom objects it currently knows about.
|
|
451
|
+
|
|
452
|
+
See :class:`objects.CustomObjectMarkers`.
|
|
453
|
+
|
|
454
|
+
.. testcode::
|
|
455
|
+
|
|
456
|
+
import anki_vector
|
|
457
|
+
with anki_vector.Robot() as robot:
|
|
458
|
+
robot.world.delete_custom_objects()
|
|
459
|
+
"""
|
|
460
|
+
|
|
461
|
+
last_blocking_call = None
|
|
462
|
+
|
|
463
|
+
if delete_custom_object_archetypes:
|
|
464
|
+
self._custom_object_archetypes.clear()
|
|
465
|
+
req = protocol.DeleteCustomObjectsRequest(mode=protocol.CustomObjectDeletionMode.Value("DELETION_MASK_ARCHETYPES"))
|
|
466
|
+
last_blocking_call = await self.grpc_interface.DeleteCustomObjects(req)
|
|
467
|
+
delete_custom_marker_objects = True
|
|
468
|
+
|
|
469
|
+
if delete_custom_marker_objects:
|
|
470
|
+
self._remove_all_custom_marker_object_instances()
|
|
471
|
+
req = protocol.DeleteCustomObjectsRequest(mode=protocol.CustomObjectDeletionMode.Value("DELETION_MASK_CUSTOM_MARKER_OBJECTS"))
|
|
472
|
+
last_blocking_call = await self.grpc_interface.DeleteCustomObjects(req)
|
|
473
|
+
|
|
474
|
+
if delete_fixed_custom_objects:
|
|
475
|
+
self._remove_all_fixed_custom_object_instances()
|
|
476
|
+
req = protocol.DeleteCustomObjectsRequest(mode=protocol.CustomObjectDeletionMode.Value("DELETION_MASK_FIXED_CUSTOM_OBJECTS"))
|
|
477
|
+
last_blocking_call = await self.grpc_interface.DeleteCustomObjects(req)
|
|
478
|
+
|
|
479
|
+
return last_blocking_call
|
|
480
|
+
|
|
481
|
+
# TODO better place to put this method than world.py?
|
|
482
|
+
@connection.on_connection_thread(requires_control=False)
|
|
483
|
+
async def define_custom_box(self,
|
|
484
|
+
custom_object_type: objects.CustomObjectTypes,
|
|
485
|
+
marker_front: objects.CustomObjectMarkers,
|
|
486
|
+
marker_back: objects.CustomObjectMarkers,
|
|
487
|
+
marker_top: objects.CustomObjectMarkers,
|
|
488
|
+
marker_bottom: objects.CustomObjectMarkers,
|
|
489
|
+
marker_left: objects.CustomObjectMarkers,
|
|
490
|
+
marker_right: objects.CustomObjectMarkers,
|
|
491
|
+
depth_mm: float,
|
|
492
|
+
width_mm: float,
|
|
493
|
+
height_mm: float,
|
|
494
|
+
marker_width_mm: float,
|
|
495
|
+
marker_height_mm: float,
|
|
496
|
+
is_unique: bool = True) -> objects.CustomObject:
|
|
497
|
+
"""Defines a cuboid of custom size and binds it to a specific custom object type.
|
|
498
|
+
|
|
499
|
+
The robot will now detect the markers associated with this object and send an
|
|
500
|
+
object_observed message when they are seen. The markers must be placed in the center
|
|
501
|
+
of their respective sides. All 6 markers must be unique.
|
|
502
|
+
|
|
503
|
+
See :class:`objects.CustomObjectMarkers`.
|
|
504
|
+
|
|
505
|
+
:param custom_object_type: the object type you are binding this custom object to.
|
|
506
|
+
:param marker_front: the marker affixed to the front of the object.
|
|
507
|
+
:param marker_back: the marker affixed to the back of the object.
|
|
508
|
+
:param marker_top: the marker affixed to the top of the object.
|
|
509
|
+
:param marker_bottom: the marker affixed to the bottom of the object.
|
|
510
|
+
:param marker_left: the marker affixed to the left of the object.
|
|
511
|
+
:param marker_right: the marker affixed to the right of the object.
|
|
512
|
+
:param depth_mm: depth of the object (in millimeters) (X axis).
|
|
513
|
+
:param width_mm: width of the object (in millimeters) (Y axis).
|
|
514
|
+
:param height_mm: height of the object (in millimeters) (Z axis).
|
|
515
|
+
(the height of the object)
|
|
516
|
+
:param marker_width_mm: width of the printed marker (in millimeters).
|
|
517
|
+
:param maker_height_mm: height of the printed marker (in millimeters).
|
|
518
|
+
:param is_unique: If True, the robot will assume there is only 1 of this object.
|
|
519
|
+
(and therefore only 1 of each of any of these markers) in the world.
|
|
520
|
+
|
|
521
|
+
.. testcode::
|
|
522
|
+
|
|
523
|
+
import anki_vector
|
|
524
|
+
with anki_vector.Robot(enable_custom_object_detection=True) as robot:
|
|
525
|
+
robot.world.define_custom_box(custom_object_type=anki_vector.objects.CustomObjectTypes.CustomType00,
|
|
526
|
+
marker_front= anki_vector.objects.CustomObjectMarkers.Circles2,
|
|
527
|
+
marker_back= anki_vector.objects.CustomObjectMarkers.Circles3,
|
|
528
|
+
marker_top= anki_vector.objects.CustomObjectMarkers.Circles4,
|
|
529
|
+
marker_bottom= anki_vector.objects.CustomObjectMarkers.Circles5,
|
|
530
|
+
marker_left= anki_vector.objects.CustomObjectMarkers.Triangles2,
|
|
531
|
+
marker_right= anki_vector.objects.CustomObjectMarkers.Triangles3,
|
|
532
|
+
depth_mm=20.0, width_mm=20.0, height_mm=20.0,
|
|
533
|
+
marker_width_mm=50.0, marker_height_mm=50.0)
|
|
534
|
+
|
|
535
|
+
Returns:
|
|
536
|
+
CustomObject instance with the specified dimensions.
|
|
537
|
+
This is None if the definition failed internally.
|
|
538
|
+
Note: No instances of this object are added to the world until they have been seen.
|
|
539
|
+
|
|
540
|
+
Raises:
|
|
541
|
+
TypeError if the custom_object_type is of the wrong type.
|
|
542
|
+
ValueError if the 6 markers aren't unique.
|
|
543
|
+
"""
|
|
544
|
+
if not isinstance(custom_object_type, objects._CustomObjectType): # pylint: disable=protected-access
|
|
545
|
+
raise TypeError("Unsupported object_type, requires CustomObjectType")
|
|
546
|
+
|
|
547
|
+
# verify all 6 markers are unique
|
|
548
|
+
markers = {marker_front, marker_back, marker_top, marker_bottom, marker_left, marker_right}
|
|
549
|
+
if len(markers) != 6:
|
|
550
|
+
raise ValueError("all markers must be unique for a custom box")
|
|
551
|
+
|
|
552
|
+
custom_object_archetype = objects.CustomObjectArchetype(custom_object_type,
|
|
553
|
+
depth_mm, width_mm, height_mm,
|
|
554
|
+
marker_width_mm, marker_height_mm,
|
|
555
|
+
is_unique)
|
|
556
|
+
|
|
557
|
+
definition = protocol.CustomBoxDefinition(marker_front=marker_front.id,
|
|
558
|
+
marker_back=marker_back.id,
|
|
559
|
+
marker_top=marker_top.id,
|
|
560
|
+
marker_bottom=marker_bottom.id,
|
|
561
|
+
marker_left=marker_left.id,
|
|
562
|
+
marker_right=marker_right.id,
|
|
563
|
+
x_size_mm=depth_mm,
|
|
564
|
+
y_size_mm=width_mm,
|
|
565
|
+
z_size_mm=height_mm,
|
|
566
|
+
marker_width_mm=marker_width_mm,
|
|
567
|
+
marker_height_mm=marker_height_mm)
|
|
568
|
+
|
|
569
|
+
req = protocol.DefineCustomObjectRequest(custom_type=custom_object_type.id,
|
|
570
|
+
is_unique=is_unique,
|
|
571
|
+
custom_box=definition)
|
|
572
|
+
|
|
573
|
+
response = await self.grpc_interface.DefineCustomObject(req)
|
|
574
|
+
|
|
575
|
+
if response.success:
|
|
576
|
+
type_id = custom_object_archetype.custom_type.id
|
|
577
|
+
self._custom_object_archetypes[type_id] = custom_object_archetype
|
|
578
|
+
return custom_object_archetype
|
|
579
|
+
|
|
580
|
+
self.logger.error("Failed to define Custom Object %s", custom_object_archetype)
|
|
581
|
+
return None
|
|
582
|
+
|
|
583
|
+
# TODO better place to put this method than world.py?
|
|
584
|
+
@connection.on_connection_thread(requires_control=False)
|
|
585
|
+
async def define_custom_cube(self,
|
|
586
|
+
custom_object_type: objects.CustomObjectTypes,
|
|
587
|
+
marker: objects.CustomObjectMarkers,
|
|
588
|
+
size_mm: float,
|
|
589
|
+
marker_width_mm: float,
|
|
590
|
+
marker_height_mm: float,
|
|
591
|
+
is_unique: bool = True) -> objects.CustomObject:
|
|
592
|
+
"""Defines a cube of custom size and binds it to a specific custom object type.
|
|
593
|
+
|
|
594
|
+
The robot will now detect the markers associated with this object and send an
|
|
595
|
+
object_observed message when they are seen. The markers must be placed in the center
|
|
596
|
+
of their respective sides.
|
|
597
|
+
|
|
598
|
+
See :class:`objects.CustomObjectMarkers`.
|
|
599
|
+
|
|
600
|
+
:param custom_object_type: the object type you are binding this custom object to.
|
|
601
|
+
:param marker: the marker affixed to every side of the cube.
|
|
602
|
+
:param size_mm: size of each side of the cube (in millimeters).
|
|
603
|
+
:param marker_width_mm: width of the printed marker (in millimeters).
|
|
604
|
+
:param maker_height_mm: height of the printed marker (in millimeters).
|
|
605
|
+
:param is_unique: If True, the robot will assume there is only 1 of this object
|
|
606
|
+
(and therefore only 1 of each of any of these markers) in the world.
|
|
607
|
+
|
|
608
|
+
.. testcode::
|
|
609
|
+
|
|
610
|
+
import anki_vector
|
|
611
|
+
with anki_vector.Robot(enable_custom_object_detection=True) as robot:
|
|
612
|
+
robot.world.define_custom_cube(custom_object_type=anki_vector.objects.CustomObjectTypes.CustomType00,
|
|
613
|
+
marker=anki_vector.objects.CustomObjectMarkers.Circles2,
|
|
614
|
+
size_mm=20.0,
|
|
615
|
+
marker_width_mm=50.0, marker_height_mm=50.0)
|
|
616
|
+
|
|
617
|
+
Returns:
|
|
618
|
+
CustomObject instance with the specified dimensions.
|
|
619
|
+
This is None if the definition failed internally.
|
|
620
|
+
Note: No instances of this object are added to the world until they have been seen.
|
|
621
|
+
|
|
622
|
+
Raises:
|
|
623
|
+
TypeError if the custom_object_type is of the wrong type.
|
|
624
|
+
"""
|
|
625
|
+
|
|
626
|
+
if not isinstance(custom_object_type, objects._CustomObjectType): # pylint: disable=protected-access
|
|
627
|
+
raise TypeError("Unsupported object_type, requires CustomObjectType")
|
|
628
|
+
|
|
629
|
+
custom_object_archetype = objects.CustomObjectArchetype(custom_object_type,
|
|
630
|
+
size_mm, size_mm, size_mm,
|
|
631
|
+
marker_width_mm, marker_height_mm,
|
|
632
|
+
is_unique)
|
|
633
|
+
|
|
634
|
+
definition = protocol.CustomCubeDefinition(marker=marker.id,
|
|
635
|
+
size_mm=size_mm,
|
|
636
|
+
marker_width_mm=marker_width_mm,
|
|
637
|
+
marker_height_mm=marker_height_mm)
|
|
638
|
+
|
|
639
|
+
req = protocol.DefineCustomObjectRequest(custom_type=custom_object_type.id,
|
|
640
|
+
is_unique=is_unique,
|
|
641
|
+
custom_cube=definition)
|
|
642
|
+
|
|
643
|
+
response = await self.grpc_interface.DefineCustomObject(req)
|
|
644
|
+
|
|
645
|
+
if response.success:
|
|
646
|
+
type_id = custom_object_archetype.custom_type.id
|
|
647
|
+
self._custom_object_archetypes[type_id] = custom_object_archetype
|
|
648
|
+
return custom_object_archetype
|
|
649
|
+
|
|
650
|
+
self.logger.error("Failed to define Custom Object %s", custom_object_archetype)
|
|
651
|
+
return None
|
|
652
|
+
|
|
653
|
+
# TODO better place to put this method than world.py?
|
|
654
|
+
@connection.on_connection_thread(requires_control=False)
|
|
655
|
+
async def define_custom_wall(self,
|
|
656
|
+
custom_object_type: objects.CustomObjectTypes,
|
|
657
|
+
marker: objects.CustomObjectMarkers,
|
|
658
|
+
width_mm: float,
|
|
659
|
+
height_mm: float,
|
|
660
|
+
marker_width_mm: float,
|
|
661
|
+
marker_height_mm: float,
|
|
662
|
+
is_unique: bool = True) -> objects.CustomObject:
|
|
663
|
+
"""Defines a wall of custom width and height, with a fixed depth of 10mm, and binds it to a specific custom object type.
|
|
664
|
+
|
|
665
|
+
The robot will now detect the markers associated with this object and send an
|
|
666
|
+
object_observed message when they are seen. The markers must be placed in the center
|
|
667
|
+
of their respective sides.
|
|
668
|
+
|
|
669
|
+
See :class:`objects.CustomObjectMarkers`.
|
|
670
|
+
|
|
671
|
+
:param custom_object_type: the object type you are binding this custom object to.
|
|
672
|
+
:param marker: the marker affixed to the front and back of the wall.
|
|
673
|
+
:param width_mm: width of the object (in millimeters). (Y axis).
|
|
674
|
+
:param height_mm: height of the object (in millimeters). (Z axis).
|
|
675
|
+
:param width_mm: width of the wall (along Y axis) (in millimeters).
|
|
676
|
+
:param height_mm: height of the wall (along Z axis) (in millimeters).
|
|
677
|
+
:param marker_width_mm: width of the printed marker (in millimeters).
|
|
678
|
+
:param maker_height_mm: height of the printed marker (in millimeters).
|
|
679
|
+
:param is_unique: If True, the robot will assume there is only 1 of this object
|
|
680
|
+
(and therefore only 1 of each of any of these markers) in the world.
|
|
681
|
+
|
|
682
|
+
.. testcode::
|
|
683
|
+
|
|
684
|
+
import anki_vector
|
|
685
|
+
with anki_vector.Robot(enable_custom_object_detection=True) as robot:
|
|
686
|
+
robot.world.define_custom_wall(custom_object_type=anki_vector.objects.CustomObjectTypes.CustomType00,
|
|
687
|
+
marker=anki_vector.objects.CustomObjectMarkers.Circles2,
|
|
688
|
+
width_mm=20.0, height_mm=20.0,
|
|
689
|
+
marker_width_mm=50.0, marker_height_mm=50.0)
|
|
690
|
+
|
|
691
|
+
Returns:
|
|
692
|
+
CustomObject instance with the specified dimensions.
|
|
693
|
+
This is None if the definition failed internally.
|
|
694
|
+
Note: No instances of this object are added to the world until they have been seen.
|
|
695
|
+
|
|
696
|
+
Raises:
|
|
697
|
+
TypeError if the custom_object_type is of the wrong type.
|
|
698
|
+
"""
|
|
699
|
+
|
|
700
|
+
if not isinstance(custom_object_type, objects._CustomObjectType): # pylint: disable=protected-access
|
|
701
|
+
raise TypeError("Unsupported object_type, requires CustomObjectType")
|
|
702
|
+
|
|
703
|
+
thickness_mm = protocol.ObjectConstants.Value("FIXED_CUSTOM_WALL_THICKNESS_MM")
|
|
704
|
+
|
|
705
|
+
custom_object_archetype = objects.CustomObjectArchetype(custom_object_type,
|
|
706
|
+
thickness_mm, width_mm, height_mm,
|
|
707
|
+
marker_width_mm, marker_height_mm,
|
|
708
|
+
is_unique)
|
|
709
|
+
|
|
710
|
+
definition = protocol.CustomWallDefinition(marker=marker.id,
|
|
711
|
+
width_mm=width_mm,
|
|
712
|
+
height_mm=height_mm,
|
|
713
|
+
marker_width_mm=marker_width_mm,
|
|
714
|
+
marker_height_mm=marker_height_mm)
|
|
715
|
+
|
|
716
|
+
req = protocol.DefineCustomObjectRequest(custom_type=custom_object_type.id,
|
|
717
|
+
is_unique=is_unique,
|
|
718
|
+
custom_wall=definition)
|
|
719
|
+
|
|
720
|
+
response = await self.grpc_interface.DefineCustomObject(req)
|
|
721
|
+
|
|
722
|
+
if response.success:
|
|
723
|
+
type_id = custom_object_archetype.custom_type.id
|
|
724
|
+
self._custom_object_archetypes[type_id] = custom_object_archetype
|
|
725
|
+
return custom_object_archetype
|
|
726
|
+
|
|
727
|
+
self.logger.error("Failed to define Custom Object %s", custom_object_archetype)
|
|
728
|
+
return None
|
|
729
|
+
|
|
730
|
+
# TODO better place to put this method than world.py?
|
|
731
|
+
def create_custom_fixed_object(self,
|
|
732
|
+
pose: util.Pose,
|
|
733
|
+
x_size_mm: float,
|
|
734
|
+
y_size_mm: float,
|
|
735
|
+
z_size_mm: float,
|
|
736
|
+
relative_to_robot: bool = False,
|
|
737
|
+
use_robot_origin: bool = True) -> objects.FixedCustomObject:
|
|
738
|
+
"""Defines a cuboid of custom size and places it in the world. It cannot be observed.
|
|
739
|
+
|
|
740
|
+
See :class:`objects.CustomObjectMarkers`.
|
|
741
|
+
|
|
742
|
+
:param pose: The pose of the object we are creating.
|
|
743
|
+
:param x_size_mm: size of the object (in millimeters) in the x axis.
|
|
744
|
+
:param y_size_mm: size of the object (in millimeters) in the y axis.
|
|
745
|
+
:param z_size_mm: size of the object (in millimeters) in the z axis.
|
|
746
|
+
:param relative_to_robot: whether or not the pose given assumes the robot's pose as its origin.
|
|
747
|
+
:param use_robot_origin: whether or not to override the origin_id in the given pose to be
|
|
748
|
+
the origin_id of Vector.
|
|
749
|
+
|
|
750
|
+
.. testcode::
|
|
751
|
+
|
|
752
|
+
import anki_vector
|
|
753
|
+
from anki_vector.util import degrees, Pose
|
|
754
|
+
|
|
755
|
+
with anki_vector.Robot() as robot:
|
|
756
|
+
robot.world.create_custom_fixed_object(Pose(100, 0, 0, angle_z=degrees(0)),
|
|
757
|
+
x_size_mm=10, y_size_mm=100, z_size_mm=100,
|
|
758
|
+
relative_to_robot=True)
|
|
759
|
+
|
|
760
|
+
Returns:
|
|
761
|
+
FixedCustomObject instance with the specified dimensions and pose.
|
|
762
|
+
"""
|
|
763
|
+
# Override the origin of the pose to be the same as the robot's. This will make sure they are in
|
|
764
|
+
# the same space in the robot every time.
|
|
765
|
+
if use_robot_origin:
|
|
766
|
+
pose = util.Pose(x=pose.position.x, y=pose.position.y, z=pose.position.z,
|
|
767
|
+
q0=pose.rotation.q0, q1=pose.rotation.q1,
|
|
768
|
+
q2=pose.rotation.q2, q3=pose.rotation.q3,
|
|
769
|
+
origin_id=self._robot.pose.origin_id)
|
|
770
|
+
|
|
771
|
+
# In this case define the given pose to be with respect to the robot's pose as its origin.
|
|
772
|
+
if relative_to_robot:
|
|
773
|
+
pose = self._robot.pose.define_pose_relative_this(pose)
|
|
774
|
+
|
|
775
|
+
response = self._create_custom_fixed_object(pose, x_size_mm, y_size_mm, z_size_mm)
|
|
776
|
+
if isinstance(response, futures.Future):
|
|
777
|
+
response = response.result()
|
|
778
|
+
|
|
779
|
+
fixed_custom_object = self.fixed_custom_object_factory(
|
|
780
|
+
self._robot,
|
|
781
|
+
pose,
|
|
782
|
+
x_size_mm,
|
|
783
|
+
y_size_mm,
|
|
784
|
+
z_size_mm,
|
|
785
|
+
response.object_id)
|
|
786
|
+
|
|
787
|
+
if fixed_custom_object:
|
|
788
|
+
self._objects[fixed_custom_object.object_id] = fixed_custom_object
|
|
789
|
+
return fixed_custom_object
|
|
790
|
+
|
|
791
|
+
# TODO: add return type hint
|
|
792
|
+
@connection.on_connection_thread(requires_control=False)
|
|
793
|
+
async def _create_custom_fixed_object(self,
|
|
794
|
+
pose: util.Pose,
|
|
795
|
+
x_size_mm: float,
|
|
796
|
+
y_size_mm: float,
|
|
797
|
+
z_size_mm: float):
|
|
798
|
+
"""Send the CreateFixedCustomObject rpc call on the connection thread."""
|
|
799
|
+
req = protocol.CreateFixedCustomObjectRequest(
|
|
800
|
+
pose=pose.to_proto_pose_struct(),
|
|
801
|
+
x_size_mm=x_size_mm,
|
|
802
|
+
y_size_mm=y_size_mm,
|
|
803
|
+
z_size_mm=z_size_mm)
|
|
804
|
+
|
|
805
|
+
return await self.grpc_interface.CreateFixedCustomObject(req)
|
|
806
|
+
|
|
807
|
+
#### Private Methods ####
|
|
808
|
+
|
|
809
|
+
def _allocate_custom_marker_object(self, msg):
|
|
810
|
+
# obj is the base object type for this custom object. We make instances of this for every
|
|
811
|
+
# unique object_id we see of this custom object type.
|
|
812
|
+
first_custom_type = protocol.ObjectType.Value("FIRST_CUSTOM_OBJECT_TYPE")
|
|
813
|
+
if msg.object_type < first_custom_type or msg.object_type >= first_custom_type + protocol.CustomType.Value("CUSTOM_TYPE_COUNT"):
|
|
814
|
+
self.logger.error('Received a custom object observation with a type not inside the custom object range: %s. Msg=%s',
|
|
815
|
+
msg.object_type, msg)
|
|
816
|
+
return None
|
|
817
|
+
|
|
818
|
+
# Object observation events contain an object_type. A subset of that object_type enum maps to the
|
|
819
|
+
# custom_type enum, so we perform the conversion.
|
|
820
|
+
custom_type = msg.object_type - first_custom_type + objects.CustomObjectTypes.CustomType00.id
|
|
821
|
+
archetype = self._custom_object_archetypes.get(custom_type)
|
|
822
|
+
if not archetype:
|
|
823
|
+
self.logger.error('Received a custom object type: %s that has not been defined yet. Msg=%s',
|
|
824
|
+
msg.object_type, msg)
|
|
825
|
+
return None
|
|
826
|
+
|
|
827
|
+
custom_object = self.custom_object_factory(self._robot,
|
|
828
|
+
archetype,
|
|
829
|
+
msg.object_id)
|
|
830
|
+
|
|
831
|
+
self.logger.debug('Allocated object_id=%s to CustomObject %s', msg.object_id, custom_object)
|
|
832
|
+
|
|
833
|
+
if custom_object:
|
|
834
|
+
self._custom_objects[msg.object_id] = custom_object
|
|
835
|
+
return custom_object
|
|
836
|
+
|
|
837
|
+
def _allocate_charger(self, msg):
|
|
838
|
+
charger = self.charger_factory(self._robot, msg.object_id)
|
|
839
|
+
if self._charger:
|
|
840
|
+
self.logger.warning('Allocating multiple chargers: existing charger=%s msg=%s', self._charger, msg)
|
|
841
|
+
return None
|
|
842
|
+
|
|
843
|
+
self._charger = charger
|
|
844
|
+
|
|
845
|
+
self.logger.debug('Allocated object_id=%s to Charger %s', msg.object_id, charger)
|
|
846
|
+
return charger
|
|
847
|
+
|
|
848
|
+
def _remove_all_custom_marker_object_instances(self):
|
|
849
|
+
for obj_id, obj in list(self._custom_objects.items()):
|
|
850
|
+
if isinstance(obj, objects.CustomObject):
|
|
851
|
+
self.logger.info("Removing CustomObject instance: id %s = obj '%s'", obj_id, obj)
|
|
852
|
+
self._custom_objects.pop(obj_id, None)
|
|
853
|
+
|
|
854
|
+
def _remove_all_fixed_custom_object_instances(self):
|
|
855
|
+
for obj_id, obj in list(self._custom_objects.items()):
|
|
856
|
+
if isinstance(obj, objects.FixedCustomObject):
|
|
857
|
+
self.logger.info("Removing FixedCustomObject instance: id %s = obj '%s'", obj_id, obj)
|
|
858
|
+
self._custom_objects.pop(obj_id, None)
|
|
859
|
+
|
|
860
|
+
#### Private Event Handlers ####
|
|
861
|
+
|
|
862
|
+
def _on_face_observed(self, _robot, _event_type, msg):
|
|
863
|
+
"""Adds a newly observed face to the world view."""
|
|
864
|
+
if msg.face_id not in self._faces:
|
|
865
|
+
pose = util.Pose(x=msg.pose.x, y=msg.pose.y, z=msg.pose.z,
|
|
866
|
+
q0=msg.pose.q0, q1=msg.pose.q1,
|
|
867
|
+
q2=msg.pose.q2, q3=msg.pose.q3,
|
|
868
|
+
origin_id=msg.pose.origin_id)
|
|
869
|
+
image_rect = util.ImageRect(msg.img_rect.x_top_left,
|
|
870
|
+
msg.img_rect.y_top_left,
|
|
871
|
+
msg.img_rect.width,
|
|
872
|
+
msg.img_rect.height)
|
|
873
|
+
face = self.face_factory(self.robot,
|
|
874
|
+
pose, image_rect, msg.face_id, msg.name, msg.expression, msg.expression_values,
|
|
875
|
+
msg.left_eye, msg.right_eye, msg.nose, msg.mouth, msg.timestamp)
|
|
876
|
+
if face:
|
|
877
|
+
self._faces[face.face_id] = face
|
|
878
|
+
|
|
879
|
+
def _on_object_observed(self, _robot, _event_type, msg):
|
|
880
|
+
"""Adds a newly observed custom object to the world view."""
|
|
881
|
+
first_custom_type = protocol.ObjectType.Value("FIRST_CUSTOM_OBJECT_TYPE")
|
|
882
|
+
if msg.object_type == objects.LIGHT_CUBE_1_TYPE:
|
|
883
|
+
if msg.object_id not in self._objects:
|
|
884
|
+
light_cube = self._light_cube.get(objects.LIGHT_CUBE_1_TYPE)
|
|
885
|
+
if light_cube:
|
|
886
|
+
light_cube.object_id = msg.object_id
|
|
887
|
+
self._objects[msg.object_id] = light_cube
|
|
888
|
+
|
|
889
|
+
elif msg.object_type == protocol.ObjectType.Value("CHARGER_BASIC"):
|
|
890
|
+
if msg.object_id not in self._objects:
|
|
891
|
+
charger = self._allocate_charger(msg)
|
|
892
|
+
if charger:
|
|
893
|
+
self._objects[msg.object_id] = charger
|
|
894
|
+
|
|
895
|
+
elif first_custom_type <= msg.object_type < (first_custom_type + protocol.CustomType.Value("CUSTOM_TYPE_COUNT")):
|
|
896
|
+
if msg.object_id not in self._objects:
|
|
897
|
+
custom_object = self._allocate_custom_marker_object(msg)
|
|
898
|
+
if custom_object:
|
|
899
|
+
self._objects[msg.object_id] = custom_object
|