From 3b0be367b9768ea0a8aab912b1ace390d790af87 Mon Sep 17 00:00:00 2001 From: ZdenekM Date: Mon, 6 Mar 2023 10:31:15 +0100 Subject: [PATCH] feat: kinect service is now aware of the sensor pose --- compose-files/fit-demo/docker-compose.lab.yml | 6 +- compose-files/fit-demo/docker-compose.yml | 28 +++--- src/docker/arcor2_kinect_azure/BUILD | 2 +- src/docker/arcor2_upload_fit_demo/BUILD | 2 +- src/python/arcor2_fit_demo/CHANGELOG.md | 7 ++ src/python/arcor2_fit_demo/VERSION | 2 +- .../object_types/kinect_azure.py | 36 ++++++-- .../scripts/kinect_calibration.py | 4 +- src/python/arcor2_kinect_azure/CHANGELOG.md | 11 +++ src/python/arcor2_kinect_azure/VERSION | 2 +- .../scripts/kinect_azure.py | 86 +++++++++++++++++-- 11 files changed, 151 insertions(+), 35 deletions(-) diff --git a/compose-files/fit-demo/docker-compose.lab.yml b/compose-files/fit-demo/docker-compose.lab.yml index 61ec5f31..440b4e23 100644 --- a/compose-files/fit-demo/docker-compose.lab.yml +++ b/compose-files/fit-demo/docker-compose.lab.yml @@ -11,4 +11,8 @@ services: environment: - ARCOR2_DOBOT_MOCK=false devices: - - /dev/dobotM1:/dev/dobot \ No newline at end of file + - /dev/dobotM1:/dev/dobot + + fit-demo-kinect: + environment: + - ARCOR2_KINECT_AZURE_MOCK=false \ No newline at end of file diff --git a/compose-files/fit-demo/docker-compose.yml b/compose-files/fit-demo/docker-compose.yml index 37c2794c..a58ae0f7 100644 --- a/compose-files/fit-demo/docker-compose.yml +++ b/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: \ No newline at end of file diff --git a/src/docker/arcor2_kinect_azure/BUILD b/src/docker/arcor2_kinect_azure/BUILD index 2371e786..66d92b79 100644 --- a/src/docker/arcor2_kinect_azure/BUILD +++ b/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"], ) diff --git a/src/docker/arcor2_upload_fit_demo/BUILD b/src/docker/arcor2_upload_fit_demo/BUILD index 775098c0..260771c9 100644 --- a/src/docker/arcor2_upload_fit_demo/BUILD +++ b/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"]) \ No newline at end of file +docker_image(name="arcor2_upload_fit_demo", repository="arcor2/arcor2_upload_fit_demo", image_tags=["1.1.0"]) \ No newline at end of file diff --git a/src/python/arcor2_fit_demo/CHANGELOG.md b/src/python/arcor2_fit_demo/CHANGELOG.md index 88fd9dab..ef032b28 100644 --- a/src/python/arcor2_fit_demo/CHANGELOG.md +++ b/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 diff --git a/src/python/arcor2_fit_demo/VERSION b/src/python/arcor2_fit_demo/VERSION index afaf360d..1cc5f657 100644 --- a/src/python/arcor2_fit_demo/VERSION +++ b/src/python/arcor2_fit_demo/VERSION @@ -1 +1 @@ -1.0.0 \ No newline at end of file +1.1.0 \ No newline at end of file diff --git a/src/python/arcor2_fit_demo/object_types/kinect_azure.py b/src/python/arcor2_fit_demo/object_types/kinect_azure.py index 84e64671..bf225b44 100644 --- a/src/python/arcor2_fit_demo/object_types/kinect_azure.py +++ b/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 diff --git a/src/python/arcor2_fit_demo/scripts/kinect_calibration.py b/src/python/arcor2_fit_demo/scripts/kinect_calibration.py index ec749a63..8a454a13 100644 --- a/src/python/arcor2_fit_demo/scripts/kinect_calibration.py +++ b/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() diff --git a/src/python/arcor2_kinect_azure/CHANGELOG.md b/src/python/arcor2_kinect_azure/CHANGELOG.md index 7ed8641f..53cb3da4 100644 --- a/src/python/arcor2_kinect_azure/CHANGELOG.md +++ b/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 diff --git a/src/python/arcor2_kinect_azure/VERSION b/src/python/arcor2_kinect_azure/VERSION index 79a2734b..09a3acfa 100644 --- a/src/python/arcor2_kinect_azure/VERSION +++ b/src/python/arcor2_kinect_azure/VERSION @@ -1 +1 @@ -0.5.0 \ No newline at end of file +0.6.0 \ No newline at end of file diff --git a/src/python/arcor2_kinect_azure/scripts/kinect_azure.py b/src/python/arcor2_kinect_azure/scripts/kinect_azure.py index 59de80d2..236486f0 100644 --- a/src/python/arcor2_kinect_azure/scripts/kinect_azure.py +++ b/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()