Skip to content

Commit aeeb7e9

Browse files
BruceVonKMichelle Sintov
BruceVonK
authored and
Michelle Sintov
committed
VIC-12248 SDK reserve control (#147)
These changes allow the SDK user to keep Vector still between scripts, or in a single script between instances of behavior control. The primary way to use this is as a module script, either from the command-line or using Mac & Windows scripts in the sdk/examples/scripts folder that can be double-clicked to reserve control from the Finder or Windows Explorer. There is also sample code in the documentation for how to use a ReserveBehaviorControl object in a script.
1 parent 6001987 commit aeeb7e9

9 files changed

Lines changed: 240 additions & 92 deletions

File tree

‎anki_vector/behavior.py‎

Lines changed: 122 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@
3636

3737
__all__ = ["MAX_HEAD_ANGLE", "MIN_HEAD_ANGLE",
3838
"MAX_LIFT_HEIGHT", "MAX_LIFT_HEIGHT_MM", "MIN_LIFT_HEIGHT", "MIN_LIFT_HEIGHT_MM",
39-
"BehaviorComponent"]
39+
"BehaviorComponent", "ReserveBehaviorControl"]
4040

4141

4242
from . import connection, faces, objects, util
@@ -69,9 +69,6 @@ class BehaviorComponent(util.Component):
6969

7070
_next_behavior_id = protocol.FIRST_SDK_TAG
7171

72-
def __init__(self, robot):
73-
super().__init__(robot)
74-
7572
@classmethod
7673
def _get_next_behavior_id(cls):
7774
# Post increment _current_behavior_id (and loop within the SDK_TAG range)
@@ -555,7 +552,7 @@ async def set_lift_height(self,
555552
lift_future = robot.behavior.set_lift_height(0.0)
556553
time.sleep(1.0)
557554
lift_future = robot.behavior.set_lift_height(1.0)
558-
lift_future.cancel()
555+
lift_future.cancel()
559556
"""
560557
if height < 0.0:
561558
self.logger.warning("lift height %s too small, should be in 0..1 range - clamping", height)
@@ -603,7 +600,7 @@ async def turn_towards_face(self,
603600
604601
with anki_vector.Robot() as robot:
605602
turn_towards_face_future = robot.behavior.turn_towards_face(1)
606-
turn_towards_face_future.cancel()
603+
turn_towards_face_future.cancel()
607604
"""
608605
turn_towards_face_request = protocol.TurnTowardsFaceRequest(face_id=face.face_id,
609606
max_turn_angle_rad=util.degrees(180).radians,
@@ -652,10 +649,10 @@ async def go_to_object(self,
652649

653650
@connection.on_connection_thread(is_cancellable_behavior=True)
654651
async def roll_cube(self,
655-
target_object: objects.LightCube,
656-
approach_angle: util.Angle = None,
657-
num_retries: int = 0,
658-
_behavior_id: int = None) -> protocol.RollObjectResponse:
652+
target_object: objects.LightCube,
653+
approach_angle: util.Angle = None,
654+
num_retries: int = 0,
655+
_behavior_id: int = None) -> protocol.RollObjectResponse:
659656
"""Tells Vector to roll a specified cube object.
660657
661658
:param target_object: The cube to roll.
@@ -685,23 +682,23 @@ async def roll_cube(self,
685682
approach_angle = util.degrees(0)
686683
else:
687684
use_approach_angle = True
688-
approach_angle = approach_angle
685+
approach_angle = approach_angle
689686

690687
roll_object_request = protocol.RollObjectRequest(object_id=target_object.object_id,
691-
approach_angle_rad=approach_angle.radians,
692-
use_approach_angle=use_approach_angle,
693-
use_pre_dock_pose=use_approach_angle,
694-
id_tag=_behavior_id,
695-
num_retries=num_retries)
688+
approach_angle_rad=approach_angle.radians,
689+
use_approach_angle=use_approach_angle,
690+
use_pre_dock_pose=use_approach_angle,
691+
id_tag=_behavior_id,
692+
num_retries=num_retries)
696693

697694
return await self.grpc_interface.RollObject(roll_object_request)
698695

699696
@connection.on_connection_thread(is_cancellable_behavior=True)
700697
async def pop_a_wheelie(self,
701-
target_object: objects.LightCube,
702-
approach_angle: util.Angle = None,
703-
num_retries: int = 0,
704-
_behavior_id: int = None) -> protocol.PopAWheelieResponse:
698+
target_object: objects.LightCube,
699+
approach_angle: util.Angle = None,
700+
num_retries: int = 0,
701+
_behavior_id: int = None) -> protocol.PopAWheelieResponse:
705702
"""Tells Vector to "pop a wheelie" using his light cube.
706703
707704
:param target_object: The cube to push down on with Vector's lift, to start the wheelie.
@@ -731,23 +728,23 @@ async def pop_a_wheelie(self,
731728
approach_angle = util.degrees(0)
732729
else:
733730
use_approach_angle = True
734-
approach_angle = approach_angle
731+
approach_angle = approach_angle
735732

736733
pop_a_wheelie_request = protocol.PopAWheelieRequest(object_id=target_object.object_id,
737-
approach_angle_rad=approach_angle.radians,
738-
use_approach_angle=use_approach_angle,
739-
use_pre_dock_pose=use_approach_angle,
740-
id_tag=_behavior_id,
741-
num_retries=num_retries)
734+
approach_angle_rad=approach_angle.radians,
735+
use_approach_angle=use_approach_angle,
736+
use_pre_dock_pose=use_approach_angle,
737+
id_tag=_behavior_id,
738+
num_retries=num_retries)
742739

743740
return await self.grpc_interface.PopAWheelie(pop_a_wheelie_request)
744741

745742
@connection.on_connection_thread(is_cancellable_behavior=True)
746743
async def pickup_object(self,
747-
target_object: objects.LightCube,
748-
use_pre_dock_pose: bool = True,
749-
num_retries: int = 0,
750-
_behavior_id: int = None) -> protocol.PickupObjectResponse:
744+
target_object: objects.LightCube,
745+
use_pre_dock_pose: bool = True,
746+
num_retries: int = 0,
747+
_behavior_id: int = None) -> protocol.PickupObjectResponse:
751748
"""Instruct the robot to pick up his LightCube.
752749
753750
While picking up the cube, Vector will use path planning.
@@ -786,8 +783,8 @@ async def pickup_object(self,
786783

787784
@connection.on_connection_thread(is_cancellable_behavior=True)
788785
async def place_object_on_ground_here(self,
789-
num_retries: int = 0,
790-
_behavior_id: int = None) -> protocol.PlaceObjectOnGroundHereResponse:
786+
num_retries: int = 0,
787+
_behavior_id: int = None) -> protocol.PlaceObjectOnGroundHereResponse:
791788
"""Ask Vector to place the object he is carrying on the ground at the current location.
792789
793790
:param num_retries: Number of times to reattempt action in case of a failure.
@@ -809,4 +806,96 @@ async def place_object_on_ground_here(self,
809806
place_object_on_ground_here_request = protocol.PlaceObjectOnGroundHereRequest(id_tag=_behavior_id,
810807
num_retries=num_retries)
811808

812-
return await self.grpc_interface.PlaceObjectOnGroundHere(place_object_on_ground_here_request)
809+
return await self.grpc_interface.PlaceObjectOnGroundHere(place_object_on_ground_here_request)
810+
811+
812+
class ReserveBehaviorControl():
813+
"""A ReserveBehaviorControl object can be used to suppress the ordinary idle behaviors of
814+
the Robot and keep Vector still between SDK control instances. Care must be taken when
815+
blocking background behaviors, as this may make Vector appear non-responsive.
816+
817+
This class is most easily used via a built-in SDK script, and can be called on the command-line
818+
via the executable module :class:`anki_vector.reserve_control`:
819+
820+
.. code-block:: bash
821+
822+
python3 -m anki_vector.reserve_control
823+
824+
As long as the script is running, background behaviors will not activate, keeping Vector
825+
still while other SDK scripts may take control. Highest-level behaviors like returning to
826+
the charger due to low battery will still activate.
827+
828+
System-specific shortcuts calling this executable module can be found in the examples/scripts
829+
folder. These scripts can be double-clicked to easily reserve behavior control for the current
830+
SDK default robot.
831+
832+
If there is a need to keep background behaviors from activating in a single script, the class
833+
may be used to reserve behavior control while in scope:
834+
835+
.. code-block:: python
836+
837+
import anki_vector
838+
from anki_vector import behavior
839+
840+
args = anki_vector.util.parse_command_args()
841+
with behavior.ReserveBehaviorControl(args.serial):
842+
843+
# At this point, Vector will remain still, even without
844+
# a Robot instance being in scope.
845+
846+
...
847+
848+
# take control of the robot as usual
849+
with anki_vector.Robot() as robot:
850+
851+
robot.anim.play_animation("anim_turn_left_01")
852+
853+
# Robot will not perform idle behaviors until the script completes
854+
855+
...
856+
857+
:param serial: Vector's serial number. The robot's serial number (ex. 00e20100) is located on
858+
the underside of Vector, or accessible from Vector's debug screen. Used to
859+
identify which Vector configuration to load.
860+
:param ip: Vector's IP address. (optional)
861+
:param config: A custom :class:`dict` to override values in Vector's configuration. (optional)
862+
Example: :code:`{"cert": "/path/to/file.cert", "name": "Vector-XXXX", "guid": "<secret_key>"}`
863+
where :code:`cert` is the certificate to identify Vector, :code:`name` is the
864+
name on Vector's face when his backpack is double-clicked on the charger, and
865+
:code:`guid` is the authorization token that identifies the SDK user.
866+
Note: Never share your authentication credentials with anyone.
867+
:param behavior_activation_timeout: The time to wait for control of the robot before failing.
868+
"""
869+
870+
def __init__(self,
871+
serial: str = None,
872+
ip: str = None,
873+
config: dict = None,
874+
behavior_activation_timeout: int = 10):
875+
config = config if config is not None else {}
876+
self.logger = util.get_class_logger(__name__, self)
877+
config = {**util.read_configuration(serial, self.logger), **config}
878+
self._name = config["name"]
879+
self._ip = ip if ip is not None else config["ip"]
880+
self._cert_file = config["cert"]
881+
self._guid = config["guid"]
882+
883+
self._port = "443"
884+
if 'port' in config:
885+
self._port = config["port"]
886+
887+
if self._name is None or self._ip is None or self._cert_file is None or self._guid is None:
888+
raise ValueError("The Robot object requires a serial and for Vector to be logged in (using the app then running the anki_vector.configure executable submodule).\n"
889+
"You may also provide the values necessary for connection through the config parameter. ex: "
890+
'{"name":"Vector-XXXX", "ip":"XX.XX.XX.XX", "cert":"/path/to/cert_file", "guid":"<secret_key>"}')
891+
892+
self._conn = connection.Connection(self._name, ':'.join([self._ip, self._port]), self._cert_file, self._guid,
893+
behavior_control_level=connection.CONTROL_PRIORITY_LEVEL.RESERVE_CONTROL)
894+
self._behavior_activation_timeout = behavior_activation_timeout
895+
896+
def __enter__(self):
897+
self._conn.connect(self._behavior_activation_timeout)
898+
return self
899+
900+
def __exit__(self, exc_type, exc_val, exc_tb):
901+
self._conn.close()

‎anki_vector/camera.py‎

Lines changed: 17 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,19 @@
4747
sys.exit("Cannot import from PIL: Do `pip3 install --user Pillow` to install")
4848

4949

50+
def _convert_to_pillow_image(image_data: bytes) -> Image.Image:
51+
"""Convert raw image bytes to a Pillow Image."""
52+
size = len(image_data)
53+
54+
# Constuct numpy array out of source data
55+
array = np.empty(size, dtype=np.uint8)
56+
array[0:size] = list(image_data)
57+
58+
# Decode compressed source data into uncompressed image data
59+
image = Image.open(io.BytesIO(array))
60+
return image
61+
62+
5063
class CameraComponent(util.Component):
5164
"""Represents Vector's camera.
5265
@@ -187,21 +200,9 @@ def image_streaming_enabled(self) -> bool:
187200
future = self.conn.run_coroutine(self._image_streaming_enabled())
188201
return future.result()
189202

190-
def _convert_to_pillow_image(self, image_data: bytes) -> Image.Image:
191-
"""Convert raw image bytes to a Pillow Image."""
192-
size = len(image_data)
193-
194-
# Constuct numpy array out of source data
195-
array = np.empty(size, dtype=np.uint8)
196-
array[0:size] = list(image_data)
197-
198-
# Decode compressed source data into uncompressed image data
199-
image = Image.open(io.BytesIO(array))
200-
return image
201-
202203
def _unpack_image(self, msg: protocol.CameraFeedResponse) -> None:
203204
"""Processes raw data from the robot into a more useful image structure."""
204-
image = self._convert_to_pillow_image(msg.data)
205+
image = _convert_to_pillow_image(msg.data)
205206
self.robot.viewer.enqueue_frame(image)
206207

207208
self._latest_image = image
@@ -226,9 +227,9 @@ async def _request_and_handle_images(self) -> None:
226227
async def capture_single_image(self) -> Image.Image:
227228
"""Request to capture a single image from the robot's camera.
228229
229-
This call requests the robot to capture an image and returns the
230+
This call requests the robot to capture an image and returns the
230231
received image, formatted as a Pillow image. This differs from `latest_image`,
231-
which maintains the last image received from the camera feed (if enabled).
232+
which maintains the last image received from the camera feed (if enabled).
232233
233234
Note that when the camera feed is enabled this call returns the `latest_image`.
234235
@@ -245,5 +246,5 @@ async def capture_single_image(self) -> Image.Image:
245246
req = protocol.CaptureSingleImageRequest()
246247
res = await self.grpc_interface.CaptureSingleImage(req)
247248
if res and res.data:
248-
return self._convert_to_pillow_image(res.data)
249+
return _convert_to_pillow_image(res.data)
249250
self.logger.error('Failed to capture a single image')

‎anki_vector/connection.py‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,9 @@ class CONTROL_PRIORITY_LEVEL(Enum):
5555
#: Runs below Mandatory Physical Reactions such as tucking Vector's head and arms during a fall,
5656
#: yet above Trigger-Word Detection. Default for normal operation.
5757
DEFAULT_PRIORITY = protocol.ControlRequest.DEFAULT # pylint: disable=no-member
58+
#: Holds control of robot before/after other SDK connections
59+
#: Used to disable idle behaviors. Not to be used for regular behavior control.
60+
RESERVE_CONTROL = protocol.ControlRequest.RESERVE_CONTROL # pylint: disable=no-member
5861

5962

6063
class _ControlEventManager:

‎anki_vector/reserve_control/__main__.py‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
#!/usr/bin/env python3
2+
3+
# Copyright (c) 2019 Anki, Inc.
4+
#
5+
# Licensed under the Apache License, Version 2.0 (the "License");
6+
# you may not use this file except in compliance with the License.
7+
# You may obtain a copy of the License in the file LICENSE.txt or at
8+
#
9+
# https://www.apache.org/licenses/LICENSE-2.0
10+
#
11+
# Unless required by applicable law or agreed to in writing, software
12+
# distributed under the License is distributed on an "AS IS" BASIS,
13+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
# See the License for the specific language governing permissions and
15+
# limitations under the License.
16+
17+
""" Reserve SDK Behavior Control
18+
19+
While this script runs, other SDK scripts may run and Vector will not perform most
20+
default behaviors before/after they complete. This will keep Vector still.
21+
22+
High priority behaviors like returning to the charger in a low battery situation,
23+
or retreating from a cliff will still take precedence.
24+
"""
25+
26+
from anki_vector import behavior, util
27+
28+
29+
def hold_control():
30+
args = util.parse_command_args()
31+
with behavior.ReserveBehaviorControl(args.serial):
32+
input("Vector behavior control reserved for SDK. Hit 'Enter' to release control.")
33+
34+
35+
if __name__ == "__main__":
36+
hold_control()

0 commit comments

Comments
 (0)