VIC-12583 SDK set camera settings (#178)

Adapted from the Cozmo codebase.  SDK queries Robot for camera configuration, streams robot exposure settings in event stream.  Pytest and Tutorial.

Author: BruceVonK

anki_vector/animation.py

@@ -190,6 +190,7 @@ async def load_animation_trigger_list(self):
         """
         return await self._load_animation_trigger_list()
 
+    # TODO: add return type hint
     @connection.on_connection_thread()
     async def play_animation_trigger(self, anim_trigger: str, loop_count: int = 1, use_lift_safe: bool = False, ignore_body_track: bool = False, ignore_head_track: bool = False, ignore_lift_track: bool = False):  # START
         """Starts an animation trigger playing on a robot.
@@ -227,6 +228,7 @@ async def play_animation_trigger(self, anim_trigger: str, loop_count: int = 1, u
                                                    ignore_lift_track=ignore_lift_track)
         return await self.grpc_interface.PlayAnimationTrigger(req)
 
+    # TODO: add return type hint
     @connection.on_connection_thread()
     async def play_animation(self, anim: str, loop_count: int = 1, ignore_body_track: bool = False, ignore_head_track: bool = False, ignore_lift_track: bool = False):
         """Starts an animation playing on a robot.

anki_vector/camera.py

@@ -26,7 +26,7 @@
 
 # __all__ should order by constants, event classes, other classes, functions.
 __all__ = ["EvtNewRawCameraImage", "EvtNewCameraImage",
-           "CameraComponent", "CameraImage"]
+           "CameraComponent", "CameraConfig", "CameraImage"]
 
 import asyncio
 from concurrent.futures import CancelledError
@@ -137,6 +137,116 @@ def annotate_image(self, scale: float = None, fit_size: tuple = None, resample_m
                                                     resample_mode=resample_mode)
 
 
+class CameraConfig:
+    """ The fixed properties for Vector's camera.
+
+    A full 3x3 calibration matrix for doing 3D reasoning based on the camera
+    images would look like:
+
+        +--------------+--------------+---------------+
+        |focal_length.x|      0       |    center.x   |
+        +--------------+--------------+---------------+
+        |       0      |focal_length.y|    center.y   |
+        +--------------+--------------+---------------+
+        |       0      |       0      |        1      |
+        +--------------+--------------+---------------+
+
+    .. testcode::
+
+        import anki_vector
+
+        with anki_vector.Robot() as robot:
+            min = robot.camera.config.min_gain
+            max = robot.camera.config.max_gain
+            print(f"Robot camera allowable exposure gain range is from {min} to {max}")
+    """
+
+    def __init__(self,
+                 focal_length_x: float,
+                 focal_length_y: float,
+                 center_x: float,
+                 center_y: float,
+                 fov_x: float,
+                 fov_y: float,
+                 min_exposure_time_ms: int,
+                 max_exposure_time_ms: int,
+                 min_gain: float,
+                 max_gain: float):
+        self._focal_length = util.Vector2(focal_length_x, focal_length_y)
+        self._center = util.Vector2(center_x, center_y)
+        self._fov_x = util.degrees(fov_x)
+        self._fov_y = util.degrees(fov_y)
+        self._min_exposure_ms = min_exposure_time_ms
+        self._max_exposure_ms = max_exposure_time_ms
+        self._min_gain = min_gain
+        self._max_gain = max_gain
+
+    @classmethod
+    def create_from_message(cls, msg: protocol.CameraConfigResponse):
+        """Create camera configuration based on Vector's camera configuration from the message sent from the Robot """
+        return cls(msg.focal_length_x,
+                   msg.focal_length_y,
+                   msg.center_x,
+                   msg.center_y,
+                   msg.fov_x,
+                   msg.fov_y,
+                   msg.min_camera_exposure_time_ms,
+                   msg.max_camera_exposure_time_ms,
+                   msg.min_camera_gain,
+                   msg.max_camera_gain)
+
+    @property
+    def min_gain(self) -> float:
+        """The minimum supported camera gain."""
+        return self._min_gain
+
+    @property
+    def max_gain(self) -> float:
+        """The maximum supported camera gain."""
+        return self._max_gain
+
+    @property
+    def min_exposure_time_ms(self) -> int:
+        """The minimum supported exposure time in milliseconds."""
+        return self._min_exposure_ms
+
+    @property
+    def max_exposure_time_ms(self) -> int:
+        """The maximum supported exposure time in milliseconds."""
+        return self._max_exposure_ms
+
+    @property
+    def focal_length(self):
+        """:class:`anki_vector.util.Vector2`: The focal length of the camera.
+
+        This is focal length combined with pixel skew (as the pixels aren't
+        perfectly square), so there are subtly different values for x and y.
+        It is in floating point pixel values e.g. <288.87, 288.36>.
+        """
+        return self._focal_length
+
+    @property
+    def center(self):
+        """:class:`anki_vector.util.Vector2`: The focal center of the camera.
+
+        This is the position of the optical center of projection within the
+        image. It will be close to the center of the image, but adjusted based
+        on the calibration of the lens. It is in floating point pixel values
+        e.g. <155.11, 111.40>.
+        """
+        return self._center
+
+    @property
+    def fov_x(self):
+        """:class:`anki_vector.util.Angle`: The x (horizontal) field of view."""
+        return self._fov_x
+
+    @property
+    def fov_y(self):
+        """:class:`anki_vector.util.Angle`: The y (vertical) field of view."""
+        return self._fov_y
+
+
 class CameraComponent(util.Component):
     """Represents Vector's camera.
 
@@ -169,6 +279,55 @@ def __init__(self, robot):
         self._latest_image_id: int = None
         self._camera_feed_task: asyncio.Task = None
         self._enabled = False
+        self._config = None  # type CameraConfig
+        self._gain = 0.0
+        self._exposure_ms = 0
+        self._auto_exposure_enabled = True
+
+    def set_config(self, message: protocol.CameraConfigRequest):
+        """Update Vector's camera configuration from the message sent from the Robot """
+        self._config = CameraConfig.create_from_message(message)
+
+    @connection.on_connection_thread(requires_control=False)
+    async def get_camera_config(self) -> protocol.CameraConfigResponse:
+        """ Get Vector's camera configuration
+
+        Retrieves the calibrated camera settings.  This is called during the Robot connection initialization, SDK
+        users should use the `config` property in most instances.
+
+        :return:
+        """
+        request = protocol.CameraConfigRequest()
+        return await self.conn.grpc_interface.GetCameraConfig(request)
+
+    @property
+    def config(self) -> CameraConfig:
+        """:class:`anki_vector.camera.CameraConfig`: The read-only config/calibration for the camera"""
+        return self._config
+
+    @property
+    def is_auto_exposure_enabled(self) -> bool:
+        """bool: True if auto exposure is currently enabled
+
+        If auto exposure is enabled the `gain` and `exposure_ms`
+        values will constantly be updated by Vector.
+        """
+        return self._auto_exposure_enabled
+
+    @property
+    def gain(self) -> float:
+        """float: The current camera gain setting."""
+        return self._gain
+
+    @property
+    def exposure_ms(self) -> int:
+        """int: The current camera exposure setting in milliseconds."""
+        return self._exposure_ms
+
+    def update_state(self, _robot, _event_type, msg):
+        self._gain = msg.gain
+        self._exposure_ms = msg.exposure_ms
+        self._auto_exposure_enabled = msg.auto_exposure_enabled
 
     @property
     @util.block_while_none()
@@ -365,6 +524,71 @@ async def capture_single_image(self, enable_high_resolution: bool = False) -> Ca
 
         self.logger.error('Failed to capture a single image')
 
+    @connection.on_connection_thread()
+    async def enable_auto_exposure(self, enable_auto_exposure=True) -> protocol.SetCameraSettingsResponse:
+        """Enable auto exposure on Vector's Camera.
+
+        Enable auto exposure on Vector's camera to constantly update the exposure
+        time and gain values based on the recent images. This is the default mode
+        when any SDK program starts.
+
+        .. testcode::
+
+            import time
+            import anki_vector
+            with anki_vector.Robot() as robot:
+                robot.camera.enable_auto_exposure(False)
+                time.sleep(5)
+
+        :param enable_auto_exposure: whether the camera should automatically adjust exposure
+        """
+
+        set_camera_settings_request = protocol.SetCameraSettingsRequest(enable_auto_exposure=enable_auto_exposure)
+        result = await self.conn.grpc_interface.SetCameraSettings(set_camera_settings_request)
+        self._auto_exposure_enabled = enable_auto_exposure
+        return result
+
+    @connection.on_connection_thread()
+    async def set_manual_exposure(self, exposure_ms: int, gain: float) -> protocol.SetCameraSettingsResponse:
+        """Set manual exposure values for Vector's Camera.
+
+        This will disable auto exposure on Vector's camera and force the specified exposure
+        time and gain values.
+
+        .. testcode::
+
+            import time
+            import anki_vector
+            with anki_vector.Robot() as robot:
+                robot.camera.set_manual_exposure(1, 0.25)
+                time.sleep(5)
+
+        :param exposure_ms: The desired exposure time in milliseconds.
+                Must be within the robot's exposure range from :attr:`CameraConfig.min_exposure_time_ms` to
+                :attr:`CameraConfig.max_exposure_time_ms`
+        :param gain: The desired gain value.
+                Must be within the robot's gain range from :attr:`CameraConfig.min_gain` to
+                :attr:`CameraConfig.max_gain`
+        Raises:
+            :class:`ValueError` if supplied an out-of-range exposure or gain
+
+        """
+
+        if exposure_ms < self._config.min_exposure_time_ms \
+                or exposure_ms > self._config.max_exposure_time_ms \
+                or gain < self._config.min_gain \
+                or gain > self._config.max_gain:
+            raise ValueError("Exposure settings out of range")
+
+        set_camera_settings_request = protocol.SetCameraSettingsRequest(gain=gain,
+                                                                        exposure_ms=exposure_ms,
+                                                                        enable_auto_exposure=False)
+        result = await self.conn.grpc_interface.SetCameraSettings(set_camera_settings_request)
+        self._gain = gain
+        self._exposure_ms = exposure_ms
+        self._auto_exposure_enabled = False
+        return result
+
 
 class EvtNewRawCameraImage:  # pylint: disable=too-few-public-methods
     """Dispatched when a new raw image is received from the robot's camera.

anki_vector/events.py

@@ -37,6 +37,7 @@ class Events(Enum):
     robot_state = "robot_state"                   #: Robot event containing changes to the robot's state.
     mirror_mode_disabled = "mirror_mode_disabled"  # : Robot event triggered when mirror mode (camera feed displayed on robot's face) is automatically disabled due to SDK no longer having control of the robot.
     vision_modes_auto_disabled = "vision_modes_auto_disabled"  # : Robot event triggered when all vision modes are automatically disabled due to the SDK no longer having control of the robot.
+    camera_settings_update = "camera_settings_update"  # : Robot event triggered when camera exposure settings change
 
     # Objects
     object_available = "object_available"               #: After the ConnectCube process is started, all available light cubes in range will broadcast an availability message through the Robot.

anki_vector/objects.py

@@ -493,6 +493,7 @@ def teardown(self):
         self.robot.events.unsubscribe(self._on_object_connection_lost,
                                       Events.cube_connection_lost)
 
+    # TODO: add return type hint
     @connection.on_connection_thread()
     async def set_light_corners(self,
                                 light1: lights.Light,

anki_vector/robot.py

@@ -684,6 +684,17 @@ def connect(self, timeout: int = 10) -> None:
                               events.Events.robot_state,
                               _on_connection_thread=True)
 
+        # get the camera configuration from the robot
+        response = self._camera.get_camera_config()
+        if isinstance(response, concurrent.futures.Future):
+            response = response.result()
+        self._camera.set_config(response)
+
+        # Subscribe to a callback for camera exposure settings
+        self.events.subscribe(self._camera.update_state,
+                              events.Events.camera_settings_update,
+                              _on_connection_thread=True)
+
         # access the pose to prove it has gotten back from the event stream once
         try:
             if not self.pose:

anki_vector/vision.py

@@ -91,6 +91,7 @@ async def disable_all_vision_modes(self):
         if self.display_camera_feed_on_face:
             await self.enable_display_camera_feed_on_face(False)
 
+    # TODO: add return type hint
     @connection.on_connection_thread()
     async def enable_custom_object_detection(self, detect_custom_objects: bool = True):
         """Enable custom object detection on the robot's camera.
@@ -113,6 +114,7 @@ async def enable_custom_object_detection(self, detect_custom_objects: bool = Tru
         enable_marker_detection_request = protocol.EnableMarkerDetectionRequest(enable=detect_custom_objects)
         return await self.grpc_interface.EnableMarkerDetection(enable_marker_detection_request)
 
+    # TODO: add return type hint
     @connection.on_connection_thread()
     async def enable_face_detection(
             self,
@@ -177,6 +179,7 @@ def on_robot_observed_motion(robot, event_type, event):
         enable_motion_detection_request = protocol.EnableMotionDetectionRequest(enable=detect_motion)
         return await self.grpc_interface.EnableMotionDetection(enable_motion_detection_request)
 
+    # TODO: add return type hint
     @connection.on_connection_thread()
     async def enable_display_camera_feed_on_face(self, display_camera_feed_on_face: bool = True):
         """Display the robot's camera feed on its face along with any detections (if enabled)

anki_vector/world.py

@@ -788,6 +788,7 @@ def create_custom_fixed_object(self,
             self._objects[fixed_custom_object.object_id] = fixed_custom_object
         return fixed_custom_object
 
+    # TODO: add return type hint
     @connection.on_connection_thread(requires_control=False)
     async def _create_custom_fixed_object(self,
                                           pose: util.Pose,