feat: kinect service is now aware of the sensor pose

compose-files/fit-demo/docker-compose.lab.yml

@@ -11,4 +11,8 @@ services:
     environment:
       - ARCOR2_DOBOT_MOCK=false
     devices:
-      - /dev/dobotM1:/dev/dobot
+      - /dev/dobotM1:/dev/dobot
+
+  fit-demo-kinect:
+    environment:
+      - ARCOR2_KINECT_AZURE_MOCK=false

compose-files/fit-demo/docker-compose.yml

@@ -13,7 +13,8 @@ services:
         condition: service_healthy
       fit-demo-execution:
         condition: service_started
-      # - fit-demo-kinect
+      fit-demo-kinect:
+        condition: service_healthy
       fit-demo-scene:
         condition: service_healthy
       fit-demo-calibration:
@@ -26,7 +27,7 @@ services:
       - "6789:6789"
       - "6799:6799"
     networks:
-          # - fit-demo-kinect-network
+          - fit-demo-kinect-network
           - fit-demo-scene-network
           - fit-demo-project-network
           - fit-demo-asset-network
@@ -68,16 +69,15 @@ services:
     volumes:
         - fit-demo-execution:/root/project
 
-# not used at the moment
-#  fit-demo-kinect:
-#    image: arcor2/arcor2_kinect_azure:0.4.0
-#    container_name: fit-demo-kinect
-#    networks:
-#      - fit-demo-kinect-network
-#    ports:
-#        - "5016:5016"
-#    environment:
-#      - ARCOR2_KINECT_AZURE_MOCK=true
+  fit-demo-kinect:
+    image: arcor2/arcor2_kinect_azure:0.6.0
+    container_name: fit-demo-kinect
+    networks:
+      - fit-demo-kinect-network
+    ports:
+        - "5016:5016"
+    environment:
+      - ARCOR2_KINECT_AZURE_MOCK=true
 
   fit-demo-calibration:
     image: arcor2/arcor2_calibration:1.0.0
@@ -172,7 +172,7 @@ services:
       - fit-demo-asset
 
   fit-demo-upload-object-types:
-    image: arcor2/arcor2_upload_fit_demo:1.0.0
+    image: arcor2/arcor2_upload_fit_demo:1.1.0
     container_name: "fit-demo-upload-object-types"
     depends_on:
       fit-demo-project:
@@ -211,7 +211,7 @@ networks:
   fit-demo-execution-network:
   fit-demo-project-network:
   fit-demo-asset-network:
-#  fit-demo-kinect-network:
+  fit-demo-kinect-network:
   fit-demo-dobot-magician-network:
   fit-demo-dobot-m1-network:
   fit-demo-calibration-network:

src/docker/arcor2_kinect_azure/BUILD

@@ -2,5 +2,5 @@ docker_image(
     name="arcor2_kinect_azure",
     repository="arcor2/arcor2_kinect_azure",
     dependencies=["build-support:install_kinect_prerequisites.sh"],
-    image_tags=["0.5.0"],
+    image_tags=["0.6.0"],
 )

src/docker/arcor2_upload_fit_demo/BUILD

@@ -1 +1 @@
-docker_image(name="arcor2_upload_fit_demo", repository="arcor2/arcor2_upload_fit_demo", image_tags=["1.0.0"])
+docker_image(name="arcor2_upload_fit_demo", repository="arcor2/arcor2_upload_fit_demo", image_tags=["1.1.0"])

src/python/arcor2_fit_demo/CHANGELOG.md

@@ -2,6 +2,13 @@
 
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
 
+## [1.0.0] - 2023-03-06
+
+### Changed
+
+- Compatibility with `arcor2_kinect_azure:0.6.0`.
+- Default url for Kinect.
+
 ## [1.0.0] - 2023-02-14
 
 ### Changed

src/python/arcor2_fit_demo/VERSION

@@ -1 +1 @@
-1.0.0
+1.1.0

src/python/arcor2_fit_demo/object_types/kinect_azure.py

@@ -1,8 +1,10 @@
+from dataclasses import dataclass
 from io import BytesIO
 
 from PIL import Image
 
 from arcor2 import rest
+from arcor2.clients import scene_service
 from arcor2.data.camera import CameraParameters
 from arcor2.data.common import ActionMetadata, Pose
 from arcor2.data.object_type import Models
@@ -11,6 +13,11 @@
 from .fit_common_mixin import FitCommonMixin, UrlSettings  # noqa:ABS101
 
 
+@dataclass
+class KinectAzureSettings(UrlSettings):
+    url: str = "http://fit-demo-kinect:5016"
+
+
 class KinectAzure(FitCommonMixin, Camera):
     _ABSTRACT = False
     mesh_filename = "kinect_azure.dae"
@@ -21,24 +28,20 @@ def __init__(
         name: str,
         pose: Pose,
         collision_model: Models,
-        settings: UrlSettings,
+        settings: KinectAzureSettings,
     ) -> None:
         super(KinectAzure, self).__init__(obj_id, name, pose, collision_model, settings)
 
         if self._started():
             self._stop()
 
-        self._start()  # TODO start with user-set parameters
+        self._start(pose)  # TODO start with user-set parameters
         self.color_camera_params = rest.call(
             rest.Method.GET, f"{self.settings.url}/color/parameters", return_type=CameraParameters
         )
 
-    @property
-    def settings(self) -> UrlSettings:  # type: ignore
-        return super(KinectAzure, self).settings
-
-    def _start(self) -> None:
-        rest.call(rest.Method.PUT, f"{self.settings.url}/state/start")
+    def _start(self, pose: Pose) -> None:
+        rest.call(rest.Method.PUT, f"{self.settings.url}/state/start", body=pose)
 
     def color_image(self, *, an: None | str = None) -> Image.Image:
         return rest.get_image(f"{self.settings.url}/color/image")
@@ -60,5 +63,22 @@ def cleanup(self) -> None:
         super(KinectAzure, self).cleanup()
         self._stop()
 
+    @property
+    def pose(self) -> Pose:
+        """Returns pose of the object.
+
+        When set, pose of the collision model is updated on the Scene service.
+        :return:
+        """
+        return rest.call(rest.Method.GET, f"{self.settings.url}/state/pose", return_type=Pose)
+
+    @pose.setter
+    def pose(self, pose: Pose) -> None:
+        # TODO call those two in parallel?
+        if self._enabled:  # TODO call super()?
+            scene_service.upsert_collision(self.collision_model, pose)
+
+        rest.call(rest.Method.PUT, f"{self.settings.url}/state/pose", body=pose)
+
     color_image.__action__ = ActionMetadata()  # type: ignore
     # depth_image.__action__ = ActionMetadata()  # type: ignore

src/python/arcor2_fit_demo/scripts/kinect_calibration.py

@@ -3,11 +3,11 @@
 from arcor2.data.common import Pose
 from arcor2.data.object_type import Box
 from arcor2_calibration_data import client as calib_client
-from arcor2_fit_demo.object_types.kinect_azure import KinectAzure, UrlSettings
+from arcor2_fit_demo.object_types.kinect_azure import KinectAzure, KinectAzureSettings
 
 
 def main() -> None:
-    kinect = KinectAzure("", "", Pose(), Box("", 0.1, 0.1, 0.1), UrlSettings("http://localhost:5016"))
+    kinect = KinectAzure("", "", Pose(), Box("", 0.1, 0.1, 0.1), KinectAzureSettings("http://localhost:5016"))
     # print(kinect.color_camera_params)
     assert kinect.color_camera_params
     ci_start = time.monotonic()

src/python/arcor2_kinect_azure/CHANGELOG.md

@@ -2,6 +2,17 @@
 
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
 
+## [0.6.0] - 2023-03-06
+
+### Changed
+
+- Health check end-point at `/healthz/ready`.
+- Requires `pose` in the body of `GET /state/start`.
+
+### Added
+
+- New endpoint `GET /state/pose`.
+
 ## [0.5.0] - 2022-10-28
 
 ### Changed

src/python/arcor2_kinect_azure/VERSION

@@ -1 +1 @@
-0.5.0
+0.6.0

src/python/arcor2_kinect_azure/scripts/kinect_azure.py

@@ -7,11 +7,12 @@
 from functools import wraps
 from typing import TYPE_CHECKING
 
-from flask import jsonify, request, send_file
+from flask import Response, jsonify, request, send_file
 from PIL import Image
 
 from arcor2 import env
 from arcor2.data.camera import CameraParameters
+from arcor2.data.common import Pose
 from arcor2.flask import RespT, create_app, run_app
 from arcor2.helpers import port_from_url
 from arcor2.image import image_to_bytes_io
@@ -32,6 +33,7 @@
 _kinect: "None | KinectAzure" = None
 _mock: bool = False
 _mock_started: bool = False
+_pose = Pose()
 
 
 def started() -> bool:
@@ -67,8 +69,13 @@ def put_start() -> RespT:
         description: Start the sensor.
         tags:
            - State
+        requestBody:
+          content:
+            application/json:
+              schema:
+                $ref: Pose
         responses:
-            200:
+            204:
               description: Ok
             500:
               description: "Error types: **General**, **StartError**."
@@ -81,6 +88,12 @@ def put_start() -> RespT:
     if started():
         raise StartError("Already started.")
 
+    if not isinstance(request.json, dict):
+        raise StartError("Body should be a JSON dict containing Pose.")
+
+    global _pose
+    _pose = Pose.from_dict(request.json)
+
     if _mock:
         global _mock_started
         _mock_started = True
@@ -92,7 +105,7 @@ def put_start() -> RespT:
         assert _kinect is None
         _kinect = KinectAzure()
 
-    return "ok", 200
+    return Response(status=204)
 
 
 @app.route("/state/stop", methods=["PUT"])
@@ -105,7 +118,7 @@ def put_stop() -> RespT:
         tags:
            - State
         responses:
-            200:
+            204:
               description: Ok
             500:
               description: "Error types: **General**, **StartError**."
@@ -123,7 +136,68 @@ def put_stop() -> RespT:
         assert _kinect is not None
         _kinect.cleanup()
         _kinect = None
-    return "ok", 200
+    return Response(status=204)
+
+
+@app.route("/state/pose", methods=["GET"])
+@requires_started
+def get_pose() -> RespT:
+    """Returns the pose configured during startup.
+    ---
+    get:
+        description: Returns the pose configured during startup.
+        tags:
+           - State
+        responses:
+            200:
+              description: Ok
+              content:
+                application/json:
+                  schema:
+                    $ref: Pose
+            500:
+              description: "Error types: **General**, **StartError**."
+              content:
+                application/json:
+                  schema:
+                    $ref: WebApiError
+    """
+
+    return jsonify(_pose.to_dict()), 200
+
+
+@app.route("/state/pose", methods=["PUT"])
+@requires_started
+def put_pose() -> RespT:
+    """Sets sensor pose in runtime.
+    ---
+    put:
+        description: Sets sensor pose in runtime.
+        tags:
+           - State
+        requestBody:
+          content:
+            application/json:
+              schema:
+                $ref: Pose
+        responses:
+            204:
+              description: Ok
+            500:
+              description: "Error types: **General**, **StartError**."
+              content:
+                application/json:
+                  schema:
+                    $ref: WebApiError
+    """
+
+    if not isinstance(request.json, dict):
+        raise StartError("Body should be a JSON dict containing Pose.")
+
+    global _pose
+    _pose = Pose.from_dict(request.json)
+
+    return Response(status=204)
 
 
 @app.route("/state/started", methods=["GET"])
@@ -326,7 +400,7 @@ def main() -> None:
     if _mock:
         logger.info("Starting as a mock!")
 
-    run_app(app, SERVICE_NAME, version(), port_from_url(URL), [CameraParameters, WebApiError], args.swagger)
+    run_app(app, SERVICE_NAME, version(), port_from_url(URL), [CameraParameters, WebApiError, Pose], args.swagger)
 
     if _kinect:
         _kinect.cleanup()