feat(arcor2_ur): extend ros_worker for collision primitives and basic tests

.github/workflows/pants.yaml

@@ -67,6 +67,7 @@ jobs:
         sudo df -h / /mnt "$GITHUB_WORKSPACE"
         sudo apt-get update
         sudo apt-get install -y --no-install-recommends jq cargo
+        sudo df -h
         sudo ./build-support/install_ur_dependencies.sh
         echo "Disk space after installing system dependencies:"
         sudo df -h / /mnt "$GITHUB_WORKSPACE"

src/python/arcor2/data/common.py

@@ -2,6 +2,7 @@
 
 import abc
 import copy
+import math
 from dataclasses import dataclass, field
 from datetime import datetime
 from enum import Enum
@@ -139,6 +140,11 @@ def __imul__(self, other: object) -> Position:
 
         return self
 
+    def distance(self, other: Position) -> float:
+        """Compute Euclidean distance between two positions."""
+        diff = self - other
+        return math.sqrt(diff.x**2 + diff.y**2 + diff.z**2)
+
     def to_dict(
         self,
         omit_none: bool = True,

src/python/arcor2_object_types/abstract.py

@@ -2,13 +2,15 @@
 import copy
 import inspect
 from dataclasses import dataclass
+from datetime import datetime, timezone
+from enum import Enum
 
 from dataclasses_jsonschema import JsonSchemaMixin
 from PIL import Image
 
 from arcor2 import CancelDict, DynamicParamDict
 from arcor2.data.camera import CameraParameters
-from arcor2.data.common import ActionMetadata, Joint, Pose, SceneObject
+from arcor2.data.common import ActionMetadata, Joint, Pose, SceneObject, StrEnum
 from arcor2.data.object_type import Models, PrimitiveModels
 from arcor2.data.robot import RobotType
 from arcor2.docstring import parse_docstring
@@ -183,6 +185,170 @@ def set_enabled(self, state: bool, *, an: None | str = None) -> None:
     set_enabled.__action__ = ActionMetadata()  # type: ignore
 
 
+class GraspableState(StrEnum):
+    """Logical state of a graspable object in the scene.
+
+    WORLD
+        Free object in the environment. Acts as a collision obstacle.
+
+    RESERVED
+        Reserved by the robot. Waiting for pickup.
+
+    HIDDEN
+        Object is hidden so the robot can attach it.
+        This state is used only in ros_worker file.
+
+    ATTACHED
+        Attached to the robot end-effector.
+
+    LOST
+        Object not observable. Location unknown.
+    """
+
+    WORLD = "WORLD"
+    RESERVED = "RESERVED"
+    HIDEN = "HIDEN"
+    ATTACHED = "ATTACHED"
+    LOST = "LOST"
+
+
+class GraspableSource(StrEnum):
+    """Source of the object pose information.
+
+    CAMERA
+        Detected by a vision system.
+
+    FIXED
+        Predefined static object.
+
+    OTHER
+        Arbitrary or unspecified source (e.g., tests, debugging, simulations, or other scenarios).
+    """
+
+    CAMERA = "CAMERA"
+    FIXED = "FIXED"
+    OTHER = "OTHER"
+
+
+def _utc_now_iso() -> str:
+    """Return the current UTC timestamp in ISO 8601 format."""
+    return datetime.now(timezone.utc).isoformat()
+
+
+class GraspableObject(GenericWithPose):
+    """Object in the scene that can potentially be grasped."""
+
+    mesh_filename: None | str = None
+
+    def __init__(
+        self,
+        obj_id: str,
+        name: str,
+        pose: Pose,
+        collision_model: Models,
+        state: GraspableState = GraspableState.WORLD,
+        source: GraspableSource = GraspableSource.OTHER,
+        stamp: str | None = None,
+        settings: None | Settings = None,
+    ) -> None:
+        super().__init__(obj_id, name, pose, settings)
+
+        self.collision_model = copy.deepcopy(collision_model)
+        self._enabled = True
+        self._state = state
+        self._source = source
+        self._stamp = stamp or _utc_now_iso()
+
+        # originally, each model has id == object type (e.g. BigBox) but here we need to set it to something unique
+        self.collision_model.id = self.id
+        self._upsert_graspable()
+
+    def _upsert_graspable(self) -> None:
+        _scene_service().upsert_graspable(
+            self.collision_model,
+            self.pose,
+            state=self._state,
+            source=self._source,
+            stamp=self._stamp,
+        )
+
+    def set_pose(self, pose: Pose) -> None:
+        super().set_pose(pose)
+        if self._enabled:
+            self._upsert_graspable()
+
+    @property
+    def enabled(self) -> bool:
+        """If the object has a collision model, this indicates whether the
+        model is enabled (set on the Scene service).
+
+        When set, it updates the state of the model on the Scene service.
+        :return:
+        """
+
+        svc = _scene_service()
+        assert (self.id in svc.collision_ids()) == self._enabled
+        return self._enabled
+
+    @enabled.setter
+    def enabled(self, enabled: bool) -> None:
+        if not self._enabled and enabled:
+            svc = _scene_service()
+            assert self.id not in svc.collision_ids()
+            self._upsert_graspable()
+        if self._enabled and not enabled:
+            _scene_service().delete_collision_id(self.id)
+        self._enabled = enabled
+
+    def set_enabled(self, state: bool, *, an: None | str = None) -> None:
+        """Enables control of the object's collision model.
+
+        :param state: State of a collision model.
+        :return:
+        """
+        self.enabled = state
+
+    set_enabled.__action__ = ActionMetadata()  # type: ignore
+
+    @property
+    def state(self) -> GraspableState:
+        return self._state
+
+    @state.setter
+    def state(self, state: GraspableState) -> None:
+        self._state = state
+        self._stamp = _utc_now_iso()
+
+        if self._enabled:
+            self._upsert_graspable()
+
+    @property
+    def source(self) -> GraspableSource:
+        return self._source
+
+    @source.setter
+    def source(self, source: GraspableSource) -> None:
+        self._source = source
+        self._stamp = _utc_now_iso()
+
+        if self._enabled:
+            self._upsert_graspable()
+
+    @property
+    def stamp(self) -> str:
+        return self._stamp
+
+    def set_state(self, state: GraspableState, *, an: None | str = None) -> None:
+        self.state = state
+
+    set_state.__action__ = ActionMetadata()  # type: ignore
+
+    def set_source(self, source: GraspableSource, *, an: None | str = None) -> None:
+        self.source = source
+
+    set_source.__action__ = ActionMetadata()  # type: ignore
+
+
 class VirtualCollisionObject(CollisionObject):
     """Should be used to represent obstacles or another 'dumb' objects at the
     workplace."""

src/python/arcor2_scene/scripts/scene.py

@@ -5,7 +5,7 @@
 import math
 import random
 import time
-from typing import NamedTuple
+from typing import Any
 
 import humps
 import numpy as np
@@ -18,19 +18,14 @@
 from arcor2.logging import get_logger
 from arcor2_scene import SCENE_PORT, SCENE_SERVICE_NAME, version
 from arcor2_scene.exceptions import NotFound, SceneGeneral, WebApiError
+from arcor2_ur.common import CollisionSceneObject, parse_collision_body
 from arcor2_web.flask import Response, RespT, create_app, run_app
 
 app = create_app(__name__)
 logger = get_logger(__name__)
 logger.propagate = False
 
-
-class CollisionObject(NamedTuple):
-    model: object_type.Models
-    pose: common.Pose
-
-
-collision_objects: dict[str, CollisionObject] = {}
+collision_objects: dict[str, CollisionSceneObject] = {}
 started: bool = False
 inflation = 0.01
 
@@ -81,7 +76,7 @@ def put_box() -> RespT:
               content:
                 application/json:
                   schema:
-                    $ref: Pose
+                    type: object
         responses:
             200:
                 description: Ok
@@ -97,12 +92,11 @@ def put_box() -> RespT:
                             $ref: WebApiError
     """
 
-    if not isinstance(request.json, dict):
-        raise SceneGeneral("Body should be a JSON dict containing Pose.")
+    pose, metadata = parse_collision_body()
 
     args = request.args.to_dict()
     box = object_type.Box(args["boxId"], float(args["sizeX"]), float(args["sizeY"]), float(args["sizeZ"]))
-    collision_objects[box.id] = CollisionObject(box, common.Pose.from_dict(humps.decamelize(request.json)))
+    collision_objects[box.id] = CollisionSceneObject(box, pose, metadata)
 
     return jsonify("ok"), 200
 
@@ -131,7 +125,7 @@ def put_sphere() -> RespT:
               content:
                 application/json:
                   schema:
-                    $ref: Pose
+                    type: object
         responses:
             200:
               description: Ok
@@ -147,12 +141,11 @@ def put_sphere() -> RespT:
                     $ref: WebApiError
     """
 
-    if not isinstance(request.json, dict):
-        raise SceneGeneral("Body should be a JSON dict containing Pose.")
+    pose, metadata = parse_collision_body()
 
     args = humps.decamelize(request.args.to_dict())
     sphere = object_type.Sphere(args["sphere_id"], float(args["radius"]))
-    collision_objects[sphere.id] = CollisionObject(sphere, common.Pose.from_dict(humps.decamelize(request.json)))
+    collision_objects[sphere.id] = CollisionSceneObject(sphere, pose, metadata)
     return jsonify("ok"), 200
 
 
@@ -185,7 +178,7 @@ def put_cylinder() -> RespT:
               content:
                 application/json:
                   schema:
-                    $ref: Pose
+                    type: object
         responses:
             200:
               description: Ok
@@ -201,12 +194,11 @@ def put_cylinder() -> RespT:
                     $ref: WebApiError
     """
 
-    if not isinstance(request.json, dict):
-        raise SceneGeneral("Body should be a JSON dict containing Pose.")
+    pose, metadata = parse_collision_body()
 
     args = humps.decamelize(request.args.to_dict())
     cylinder = object_type.Cylinder(args["cylinder_id"], float(args["radius"]), float(args["height"]))
-    collision_objects[cylinder.id] = CollisionObject(cylinder, common.Pose.from_dict(humps.decamelize(request.json)))
+    collision_objects[cylinder.id] = CollisionSceneObject(cylinder, pose, metadata)
     return jsonify("ok"), 200
 
 
@@ -251,7 +243,7 @@ def put_mesh() -> RespT:
               content:
                 application/json:
                   schema:
-                    $ref: Pose
+                    type: object
         responses:
             200:
               description: Ok
@@ -267,12 +259,11 @@ def put_mesh() -> RespT:
                     $ref: WebApiError
     """
 
-    if not isinstance(request.json, dict):
-        raise SceneGeneral("Body should be a JSON dict containing Pose.")
+    pose, metadata = parse_collision_body()
 
     args = humps.decamelize(request.args.to_dict())
     mesh = object_type.Mesh(args["mesh_id"], args["mesh_file_id"])
-    collision_objects[mesh.id] = CollisionObject(mesh, common.Pose.from_dict(humps.decamelize(request.json)))
+    collision_objects[mesh.id] = CollisionSceneObject(mesh, pose, metadata)
     return jsonify("ok"), 200
 
 
@@ -419,21 +410,25 @@ def put_line_safe() -> RespT:
     arcor2/ros x forward, y left, z up
     unity      x Right,   y Up,   z Forward
     """
-    for obj_id, (model, pose) in collision_objects.items():
+    for obj_id, obj in collision_objects.items():
+        model = obj.model
+        pose = obj.pose
+
         if isinstance(model, object_type.Box):
-            # The left bottom corner on the front will be placed at (0, 0, 0)
             sx = model.size_x + inflation
             sy = model.size_y + inflation
             sz = model.size_z + inflation
 
             tm = o3d.geometry.TriangleMesh.create_box(sx, sy, sz)
-
             tm = tm.translate([pose.position.x - sx / 2, pose.position.y - sy / 2, pose.position.z - sz / 2])
+
         elif isinstance(model, object_type.Cylinder):
             tm = o3d.geometry.TriangleMesh.create_cylinder(model.radius + inflation, model.height + inflation)
+
         elif isinstance(model, object_type.Sphere):
             tm = o3d.geometry.TriangleMesh.create_sphere(model.radius + inflation)
-        else:  # TODO mesh
+
+        else:
             logger.warning(f"Unsupported type of collision model: {model.type()}.")
             continue