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

.github/workflows/pants.yaml

@@ -80,7 +80,12 @@ jobs:
     - name: Test
       run: |
         source /opt/ros/jazzy/setup.bash
-        pants --changed-since=origin/master --changed-dependents=transitive test
+        pants --changed-since=origin/master --changed-dependents=transitive --tag='-unstable_ros' test
+
+    - name: Test changed unstable ROS tests serially with retries
+      run: |
+        source /opt/ros/jazzy/setup.bash
+        pants --process-execution-local-parallelism=1 --changed-since=origin/master --changed-dependents=transitive --tag='unstable_ros' test --test-attempts-default=1
     - name: Build Python packages
       run: |
         pants --filter-target-type=python_distribution package ::

build-support/install_ur_dependencies.sh

@@ -1,4 +1,10 @@
 #!/usr/bin/env bash
+set -euxo pipefail
+
+apt-get update
+
+# debug check
+apt-cache search ros-jazzy | head || true
 
 # Keep ROS packages pinned so CI and the arcor2_ur image exercise the same UR stack over time.
 apt-get install -y -q --no-install-recommends \

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

@@ -1,11 +1,18 @@
 services:
   ur-demo-robot-api:
-    image: arcor2/arcor2_ur:1.7.0
+    image: arcor2/arcor2_ur:1.8.0
     container_name: ur-demo-robot-api
     ports:
       - "5012:5012"
-#    environment:
-#       - ARCOR2_UR_DEBUG=1
+    environment:
+      - DISPLAY=${DISPLAY}
+      - WAYLAND_DISPLAY=${WAYLAND_DISPLAY}
+      - XDG_RUNTIME_DIR=${XDG_RUNTIME_DIR}
+      - PULSE_SERVER=${PULSE_SERVER}
+      # - ARCOR2_UR_DEBUG=1
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix
+      - /mnt/wslg:/mnt/wslg
     restart: always  # the service tends to crash when calling PUT /state/stop
     cap_add:
       - SYS_NICE
@@ -15,7 +22,7 @@ services:
       memlock: 8428281856
     # network_mode: host
   ur-demo-arserver:
-    image: arcor2/arcor2_arserver:1.4.0
+    image: arcor2/arcor2_arserver:1.8.0
     container_name: ur-demo-arserver
     depends_on:
       ur-demo-storage:
@@ -53,7 +60,7 @@ services:
       - ur-demo-network
 
   ur-demo-execution:
-    image: arcor2/arcor2_execution:1.7.0
+    image: arcor2/arcor2_execution:1.8.0
     container_name: ur-demo-execution
     networks:
       - ur-demo-network
@@ -117,7 +124,7 @@ services:
         condition: service_healthy
 
   ur-demo-upload-object-types:
-    image: arcor2/arcor2_ur_ot:1.7.0
+    image: arcor2/arcor2_ur_ot:1.8.0
     container_name: "ur-demo-upload-object-types"
     depends_on:
       ur-demo-storage:
@@ -128,7 +135,7 @@ services:
       - ARCOR2_STORAGE_SERVICE_URL=http://ur-demo-storage:10000
 
   ur-demo-upload-builtin-objects:
-    image: arcor2/arcor2_upload_object_types:2.0.0
+    image: arcor2/arcor2_upload_object_types:1.8.0
     container_name: "ur-demo-upload-builtin-objects"
     depends_on:
       ur-demo-storage:

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.4.0"]
+    name="arcor2_arserver", repository="arcor2/arcor2_arserver", dependencies=[":start.sh"], image_tags=["1.8.0"]
 )

src/docker/arcor2_execution/BUILD

@@ -1 +1 @@
-docker_image(name="arcor2_execution", repository="arcor2/arcor2_execution", image_tags=["1.7.0"])
+docker_image(name="arcor2_execution", repository="arcor2/arcor2_execution", image_tags=["1.8.0"])

src/docker/arcor2_scene/BUILD

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

src/docker/arcor2_upload_builtin_objects/BUILD

@@ -1,3 +1 @@
-docker_image(
-    name="arcor2_upload_object_types", repository="arcor2/arcor2_upload_object_types", image_tags=["2.0.0"]
-)
+docker_image(name="arcor2_upload_object_types", repository="arcor2/arcor2_upload_object_types", image_tags=["1.8.0"])

src/docker/arcor2_ur/BUILD

@@ -1,2 +1,13 @@
 shell_source(name="start.sh", source="start.sh")
-docker_image(name="arcor2_ur", repository="arcor2/arcor2_ur", dependencies=[":start.sh", "build-support:install_ur_dependencies.sh"], image_tags=["1.7.0"])
+
+docker_image(
+    name="arcor2_ur",
+    repository="arcor2/arcor2_ur",
+    dependencies=[
+        ":start.sh",
+        "build-support:install_ur_dependencies.sh",
+        "src/python/arcor2_ur/scripts:ur",
+        "src/python/arcor2_ur/scripts:robot_publisher",
+    ],
+    image_tags=["1.8.0"],
+)

src/docker/arcor2_ur/Dockerfile

@@ -42,6 +42,7 @@ RUN set -euo pipefail \
         && apt-get update \
         && /root/install_ur_dependencies.sh \
         && apt-get install -y -q --no-install-recommends \
+        ros-jazzy-rviz2 \
         libglib2.0-0t64 \
         libgomp1 \
         libusb-1.0-0 \
@@ -54,6 +55,7 @@ ENV ARCOR2_UR_TYPE=${ARCOR2_UR_TYPE}
 ENV ARCOR2_UR_ROBOT_IP=${ARCOR2_UR_ROBOT_IP}
 ENV ARCOR2_UR_STARTUP_SLEEP=${ARCOR2_UR_STARTUP_SLEEP}
 
+COPY src.python.arcor2_ur.scripts/robot_publisher.pex /bin/robot_publisher.pex
 COPY src/docker/arcor2_ur/start.sh /root/start.sh
 RUN chmod +x /root/start.sh
 ENTRYPOINT ["/root/start.sh"]

src/docker/arcor2_ur/start.sh

@@ -6,6 +6,7 @@ set -e
 source /opt/ros/jazzy/setup.bash
 
 : "${ARCOR2_UR_TYPE:=ur5e}"
+: "${ARCOR2_UR_START_ROBOT_PUBLISHER:=true}"
 
 # simulator needs some time to get running...
 if [[ -n "$ARCOR2_UR_STARTUP_SLEEP" && "$ARCOR2_UR_STARTUP_SLEEP" =~ ^[0-9]+$ ]]; then
@@ -17,4 +18,9 @@ cp --update=none "$(ros2 pkg prefix --share ur_description)/config/$ARCOR2_UR_TY
 
 ros2 launch ur_robot_driver ur_control.launch.py ur_type:="$ARCOR2_UR_TYPE" robot_ip:="$ARCOR2_UR_ROBOT_IP" launch_rviz:=false kinematics_params_file:="/root/robot_calibration.yaml" &
 
+if [[ "$ARCOR2_UR_START_ROBOT_PUBLISHER" == "true" ]]; then
+	echo "Starting ARCOR2 UR robot publisher for suction TCP static transforms..."
+	PEX_EXTRA_SYS_PATH=/opt/ros/jazzy/lib/python3.12/site-packages PYTHONOPTIMIZE=1 python3 /bin/robot_publisher.pex &
+fi
+
 PEX_EXTRA_SYS_PATH=/opt/ros/jazzy/lib/python3.12/site-packages PYTHONOPTIMIZE=1 /bin/app/pex

src/docker/arcor2_ur_ot/BUILD

@@ -1 +1 @@
-docker_image(name="arcor2_ur_ot", repository="arcor2/arcor2_ur_ot", image_tags=["1.7.0"])
+docker_image(name="arcor2_ur_ot", repository="arcor2/arcor2_ur_ot", image_tags=["1.8.0"])

src/python/arcor2/BUILD

@@ -6,5 +6,5 @@ arcor2_python_distribution(
         "src/python/arcor2/data/rpc",
         "src/python/arcor2/exceptions",
         "src/python/arcor2/source",
-    ]
+    ],
 )

src/python/arcor2/VERSION

@@ -1 +1 @@
-2.0.0
+1.8.0

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_build/scripts/BUILD

@@ -2,5 +2,8 @@ python_sources()
 
 arcor2_pex_binary(
     name="build",
-    dependencies=["src/python/arcor2_object_types","src/python/arcor2_runtime",]
+    dependencies=[
+        "src/python/arcor2_object_types",
+        "src/python/arcor2_runtime",
+    ],
 )

src/python/arcor2_calibration/BUILD

@@ -1,5 +1,5 @@
 arcor2_python_distribution(
     name="arcor2_calibration",
     description="ARCOR2 Calibration",
-    binaries={"arcor2_calibration": "src/python/arcor2_calibration/scripts:calibration"}
+    binaries={"arcor2_calibration": "src/python/arcor2_calibration/scripts:calibration"},
 )

src/python/arcor2_object_types/VERSION

@@ -1 +1 @@
-2.0.0
+1.8.0

src/python/arcor2_object_types/abstract.py

@@ -2,6 +2,7 @@
 import copy
 import inspect
 from dataclasses import dataclass
+from datetime import datetime, timezone
 
 from dataclasses_jsonschema import JsonSchemaMixin
 from PIL import Image
@@ -14,6 +15,7 @@
 from arcor2.docstring import parse_docstring
 from arcor2.exceptions import Arcor2Exception, Arcor2NotImplemented
 from arcor2.helpers import NonBlockingLock
+from arcor2_ur.common import GraspableSource, GraspableState
 
 
 class GenericException(Arcor2Exception):
@@ -183,6 +185,125 @@ def set_enabled(self, state: bool, *, an: None | str = None) -> None:
     set_enabled.__action__ = ActionMetadata()  # type: ignore
 
 
+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/BUILD

@@ -3,5 +3,5 @@ arcor2_python_distribution(
     description="ARCOR2 Scene Service",
     binaries={
         "arcor2_scene": "src/python/arcor2_scene/scripts:scene",
-    }
+    },
 )

src/python/arcor2_scene/VERSION

@@ -1 +1 @@
-1.1.0
+1.8.0