From 86e21cadc571a1bd25e82c5136d4e22726f181b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jakub=20Dvo=C5=99=C3=A1k?= Date: Tue, 2 May 2023 11:05:57 +0200 Subject: [PATCH] chore: send pose to kinect after calibration --- compose-files/fit-demo/docker-compose.yml | 6 +++--- src/docker/arcor2_arserver/BUILD | 2 +- src/docker/arcor2_kinect_azure/BUILD | 2 +- src/docker/arcor2_upload_fit_demo/BUILD | 2 +- src/python/arcor2/object_types/abstract.py | 14 ++++---------- .../object_types/tests/test_scene_integration.py | 2 +- src/python/arcor2_arserver/CHANGELOG.md | 6 ++++++ src/python/arcor2_arserver/scene.py | 4 ++-- src/python/arcor2_fit_demo/CHANGELOG.md | 7 +++++++ src/python/arcor2_kinect_azure/CHANGELOG.md | 7 +++++++ .../arcor2_kinect_azure/kinect/__init__.py | 16 +++++++++++++--- .../arcor2_kinect_azure/routes/synchronized.py | 2 +- .../object_types/kinect_azure.py | 13 ++++++++++--- 13 files changed, 57 insertions(+), 26 deletions(-) diff --git a/compose-files/fit-demo/docker-compose.yml b/compose-files/fit-demo/docker-compose.yml index fcbcff9a..c5336e3b 100644 --- a/compose-files/fit-demo/docker-compose.yml +++ b/compose-files/fit-demo/docker-compose.yml @@ -2,7 +2,7 @@ version: '3.8' services: fit-demo-arserver: - image: arcor2/arcor2_arserver:1.0.1 + image: arcor2/arcor2_arserver:1.0.2 container_name: fit-demo-arserver depends_on: fit-demo-project: @@ -72,7 +72,7 @@ services: - fit-demo-execution:/root/project fit-demo-kinect: - image: arcor2/arcor2_kinect_azure:0.6.0 + image: arcor2/arcor2_kinect_azure:0.7.1 container_name: fit-demo-kinect networks: - fit-demo-kinect-network @@ -174,7 +174,7 @@ services: - fit-demo-asset fit-demo-upload-object-types: - image: arcor2/arcor2_upload_fit_demo:1.2.0 + image: arcor2/arcor2_upload_fit_demo:1.2.1 container_name: "fit-demo-upload-object-types" depends_on: fit-demo-project: diff --git a/src/docker/arcor2_arserver/BUILD b/src/docker/arcor2_arserver/BUILD index 708341ff..7935b85d 100644 --- a/src/docker/arcor2_arserver/BUILD +++ b/src/docker/arcor2_arserver/BUILD @@ -1,5 +1,5 @@ shell_source(name="start.sh", source="start.sh") docker_image( - name="arcor2_arserver", repository="arcor2/arcor2_arserver", dependencies=[":start.sh"], image_tags=["1.0.1"] + name="arcor2_arserver", repository="arcor2/arcor2_arserver", dependencies=[":start.sh"], image_tags=["1.0.2"] ) diff --git a/src/docker/arcor2_kinect_azure/BUILD b/src/docker/arcor2_kinect_azure/BUILD index 979f493d..3d06ccba 100644 --- a/src/docker/arcor2_kinect_azure/BUILD +++ b/src/docker/arcor2_kinect_azure/BUILD @@ -3,5 +3,5 @@ docker_image( name="arcor2_kinect_azure", repository="arcor2/arcor2_kinect_azure", dependencies=["build-support:install_kinect_prerequisites.sh"], - image_tags=["0.7.0"], + image_tags=["0.7.1"], ) diff --git a/src/docker/arcor2_upload_fit_demo/BUILD b/src/docker/arcor2_upload_fit_demo/BUILD index 22c64742..8a2536b5 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.2.0"]) \ No newline at end of file +docker_image(name="arcor2_upload_fit_demo", repository="arcor2/arcor2_upload_fit_demo", image_tags=["1.2.1"]) \ No newline at end of file diff --git a/src/python/arcor2/object_types/abstract.py b/src/python/arcor2/object_types/abstract.py index 46bb91e5..28408e96 100644 --- a/src/python/arcor2/object_types/abstract.py +++ b/src/python/arcor2/object_types/abstract.py @@ -100,8 +100,7 @@ def pose(self) -> Pose: """ return self._pose - @pose.setter - def pose(self, pose: Pose) -> None: + def set_pose(self, pose: Pose) -> None: self._pose = pose def update_pose(self, new_pose: Pose, *, an: None | str = None) -> None: @@ -111,7 +110,7 @@ def update_pose(self, new_pose: Pose, *, an: None | str = None) -> None: :return: """ - self.pose = new_pose + self.set_pose(new_pose) update_pose.__action__ = ActionMetadata() # type: ignore @@ -137,13 +136,8 @@ def __init__( self.collision_model.id = self.id scene_service.upsert_collision(self.collision_model, pose) - @property # workaround for https://github.com/python/mypy/issues/5936 - def pose(self) -> Pose: - return super().pose - - @pose.setter - def pose(self, pose: Pose) -> None: - self._pose = pose # TODO how to set it through super() correctly? + def set_pose(self, pose: Pose) -> None: + super().set_pose(pose) if self._enabled: scene_service.upsert_collision(self.collision_model, pose) diff --git a/src/python/arcor2/object_types/tests/test_scene_integration.py b/src/python/arcor2/object_types/tests/test_scene_integration.py index a951e0c3..8c639a02 100644 --- a/src/python/arcor2/object_types/tests/test_scene_integration.py +++ b/src/python/arcor2/object_types/tests/test_scene_integration.py @@ -49,7 +49,7 @@ def test_generic_with_pose(start_processes: None) -> None: obj = CollisionObject("id", "name", Pose(), Box("boxId", 0.1, 0.1, 0.1)) assert obj.id in scene_service.collision_ids() - obj.pose = Pose() + obj.set_pose(Pose()) obj.enabled = False assert obj.id not in scene_service.collision_ids() diff --git a/src/python/arcor2_arserver/CHANGELOG.md b/src/python/arcor2_arserver/CHANGELOG.md index 73a083c8..5556c000 100644 --- a/src/python/arcor2_arserver/CHANGELOG.md +++ b/src/python/arcor2_arserver/CHANGELOG.md @@ -2,6 +2,12 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), +## [1.0.2] - 2023-05-02 + +### Changed + +- Pose must now be updated with obj.set_pose(pose) + ## [1.0.1] - 2023-04-24 ### Added diff --git a/src/python/arcor2_arserver/scene.py b/src/python/arcor2_arserver/scene.py index 879553f8..3a9723c8 100644 --- a/src/python/arcor2_arserver/scene.py +++ b/src/python/arcor2_arserver/scene.py @@ -192,8 +192,8 @@ async def update_scene_object_pose( assert pose is not None - # Object pose is property that might call scene service - that's why it has to be called using executor. - await hlp.run_in_executor(setattr, obj_inst, "pose", pose) + # Setting object pose might call scene service - that's why it has to be called using executor. + await hlp.run_in_executor(obj_inst.set_pose, pose) if lock_owner: await glob.LOCK.write_unlock(obj.id, lock_owner, True) diff --git a/src/python/arcor2_fit_demo/CHANGELOG.md b/src/python/arcor2_fit_demo/CHANGELOG.md index 8f380d98..305f229e 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.2.1] - 2023-05-02 + +### Changed + +- KinectAzure now correctly stops when scene is turned off +- KinectAzure is updated with pose after each calibration + ## [1.2.0] - 2023-04-24 ### Changed diff --git a/src/python/arcor2_kinect_azure/CHANGELOG.md b/src/python/arcor2_kinect_azure/CHANGELOG.md index 9979cd16..0436f6da 100644 --- a/src/python/arcor2_kinect_azure/CHANGELOG.md +++ b/src/python/arcor2_kinect_azure/CHANGELOG.md @@ -2,6 +2,13 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), +## [0.7.1] - 2023-05-02 + +## Fixed + +- Colors in color image +- Synchronized images download + ## [0.7.0] - 2023-04-19 ### Changelog diff --git a/src/python/arcor2_kinect_azure/kinect/__init__.py b/src/python/arcor2_kinect_azure/kinect/__init__.py index 396d80c5..b3a2762b 100644 --- a/src/python/arcor2_kinect_azure/kinect/__init__.py +++ b/src/python/arcor2_kinect_azure/kinect/__init__.py @@ -236,13 +236,20 @@ def capture(self) -> PyK4ACapture: time.sleep(0.2) return self._capture_buffer[-1] + @staticmethod + def convert_bgra_to_rgba(arr: np.ndarray) -> None: + arr[:, :, [0, 1, 2, 3]] = arr[:, :, [2, 1, 0, 3]] + def color_image(self) -> Image.Image: capture = self.capture() if capture.color is None: raise KinectAzureException("Color image not available.") + else: + color = capture.color + self.convert_bgra_to_rgba(color) - return Image.fromarray(capture.color, mode="RGBA") + return Image.fromarray(color, mode="RGBA") def _depth_image(self) -> np.ndarray: capture = self.capture() @@ -266,10 +273,13 @@ def depth_image(self, num_frames: int = 1) -> Image.Image: def sync_images(self) -> ColorAndDepthImage: capture = self.capture() - if not capture.color or not capture.depth: + if capture.color is None or capture.depth is None: raise KinectAzureException("Color/depth image not available.") + else: + color = capture.color + self.convert_bgra_to_rgba(color) - return ColorAndDepthImage(Image.fromarray(capture.color), Image.fromarray(capture.transformed_depth)) + return ColorAndDepthImage(Image.fromarray(color).convert("RGB"), Image.fromarray(capture.transformed_depth)) def cleanup(self) -> None: if self._capture_thread is not None: diff --git a/src/python/arcor2_kinect_azure/routes/synchronized.py b/src/python/arcor2_kinect_azure/routes/synchronized.py index 065221b2..0c4c6cc4 100644 --- a/src/python/arcor2_kinect_azure/routes/synchronized.py +++ b/src/python/arcor2_kinect_azure/routes/synchronized.py @@ -13,7 +13,7 @@ blueprint = Blueprint("synchronized", __name__, url_prefix="/synchronized") -@blueprint.route("/synchronized/image", methods=["GET"]) +@blueprint.route("/image", methods=["GET"]) @requires_started def get_image_both() -> RespT: """Get the both color/depth image. diff --git a/src/python/arcor2_kinect_azure_data/object_types/kinect_azure.py b/src/python/arcor2_kinect_azure_data/object_types/kinect_azure.py index 17cd8506..d55c99df 100644 --- a/src/python/arcor2_kinect_azure_data/object_types/kinect_azure.py +++ b/src/python/arcor2_kinect_azure_data/object_types/kinect_azure.py @@ -65,11 +65,11 @@ def to_id(self) -> BodyJointId: @dataclass class UrlSettings(Settings): - url: str = "http://localhost:5017" + url: str = "http://192.168.104.100:5017" @classmethod def from_env(cls) -> "UrlSettings": - url = os.getenv("ARCOR2_KINECT_AZURE_URL", "http://localhost:5017") + url = os.getenv("ARCOR2_KINECT_AZURE_URL", "http://192.168.104.100:5017") return cls(url) def construct_path(self, path: str) -> str: @@ -135,7 +135,14 @@ def stop(self) -> None: if not self._started: return - call(Method.GET, self.construct_path("/state/stop")) + call(Method.PUT, self.construct_path("/state/stop")) + + def cleanup(self) -> None: + self.stop() + + def set_pose(self, pose: Pose) -> None: + call(Method.PUT, self.construct_path("/state/pose"), body=pose) + super().set_pose(pose) def color_image(self, *, an: Optional[str] = None) -> Image.Image: """Get color image from kinect.