feat(arcor2_ur): handling of collision objects

build-support/install_ur_dependencies.sh

@@ -1,6 +1,6 @@
 #!/usr/bin/env bash
 
 apt-get install -y -q --no-install-recommends \
-	ros-jazzy-ros-base=0.11.0-1noble.20240922.110418 \
-	ros-jazzy-ur=2.4.12-1noble.20241015.122847 \
-	ros-jazzy-moveit-py=2.10.0-1noble.20241008.001636
+	ros-jazzy-ros-base=0.11.0-1noble.20241018.114811 \
+	ros-jazzy-ur=2.4.13-1noble.20241118.195218 \
+	ros-jazzy-moveit-py=2.10.0-1noble.20241108.205840

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

@@ -1,6 +1,6 @@
 services:
   ur-demo-robot-api:
-    image: arcor2/arcor2_ur:1.4.1
+    image: arcor2/arcor2_ur:1.5.0
     container_name: ur-demo-robot-api
     ports:
       - "5012:5012"
@@ -26,8 +26,6 @@ services:
         condition: service_healthy
       ur-demo-execution:
         condition: service_started
-      ur-demo-scene:
-        condition: service_healthy
       ur-demo-calibration:
         condition: service_healthy
     ports:
@@ -38,7 +36,7 @@ services:
     environment:
       - ARCOR2_PROJECT_SERVICE_URL=http://ur-demo-project:10000
       - ARCOR2_ASSET_SERVICE_URL=http://ur-demo-asset:10040
-      - ARCOR2_SCENE_SERVICE_URL=http://ur-demo-scene:5013
+      - ARCOR2_SCENE_SERVICE_URL=http://ur-demo-robot-api:5012
       - ARCOR2_EXECUTION_URL=ws://ur-demo-execution:6790
       - ARCOR2_BUILD_URL=http://ur-demo-build:5008
       - ARCOR2_CALIBRATION_URL=http://ur-demo-calibration:5014
@@ -63,7 +61,7 @@ services:
     networks:
       - ur-demo-network
     environment:
-      - ARCOR2_SCENE_SERVICE_URL=http://ur-demo-scene:5013
+      - ARCOR2_SCENE_SERVICE_URL=http://ur-demo-robot-api:5012
       - ARCOR2_PROJECT_PATH=/root/project
     volumes:
       - ur-demo-execution:/root/project
@@ -97,15 +95,6 @@ services:
       - ARCOR2_CALIBRATION_MOCK=false
     volumes:
       - ./calibration.yaml:/root/calibration.yaml
-
-  ur-demo-scene:
-    image: arcor2/arcor2_scene:1.1.0
-    container_name: ur-demo-scene
-    networks:
-      - ur-demo-network
-    ports:
-      - "5013:5013"
-
   ur-demo-asset:
     image: registry.gitlab.com/kinalisoft/test-it-off/asset:2.0.2
     container_name: "ur-demo-asset"

mypy.ini

@@ -141,4 +141,10 @@ ignore_missing_imports = True
 ignore_missing_imports = True
 
 [mypy-std_msgs.*]
+ignore_missing_imports = True
+
+[mypy-moveit_msgs.*]
+ignore_missing_imports = True
+
+[mypy-shape_msgs.*]
 ignore_missing_imports = True

pants.toml

@@ -63,7 +63,7 @@ install_from_resolve = "setuptools"
 
 [test]
 use_coverage = false
-extra_env_vars = ["PYTHONDEVMODE=1", "LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/jazzy/lib/x86_64-linux-gnu:/opt/ros/jazzy/lib", "HOME=/tmp"]  # "PYTHONWARNINGS=error" not useable due to DeprecationWarning in networkx-2.2
+extra_env_vars = ["PYTHONDEVMODE=1", "LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/jazzy/opt/rviz_ogre_vendor/lib:/opt/ros/jazzy/lib/x86_64-linux-gnu:/opt/ros/jazzy/opt/gz_math_vendor/lib:/opt/ros/jazzy/opt/gz_utils_vendor/lib:/opt/ros/jazzy/opt/gz_cmake_vendor/lib:/opt/ros/jazzy/lib:/opt/ros/jazzy/opt/sdformat_vendor/lib", "HOME=/tmp"]  # "PYTHONWARNINGS=error" not useable due to DeprecationWarning in networkx-2.2
 timeout_default = 600
 
 [python-infer]

src/docker/arcor2_ur/BUILD

@@ -1,2 +1,2 @@
 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.4.1"])
+docker_image(name="arcor2_ur", repository="arcor2/arcor2_ur", dependencies=[":start.sh", "build-support:install_ur_dependencies.sh"], image_tags=["1.5.0"])

src/docker/arcor2_ur_ot/BUILD

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

src/python/arcor2_arserver/tests/testutils.py

@@ -32,8 +32,12 @@ def log_proc_output(out: tuple[bytes, bytes]) -> None:
 
 def finish_processes(processes) -> None:
     for proc in processes:
-        proc.terminate()
-        proc.wait()
+        if proc.poll() is None:
+            proc.terminate()
+            try:
+                proc.wait(timeout=5)
+            except sp.TimeoutExpired:
+                proc.kill()
         log_proc_output(proc.communicate())
 
 

src/python/arcor2_ur/CHANGELOG.md

@@ -2,6 +2,13 @@
 
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
 
+## [1.5.0] - 2024-12-16
+
+### Changed
+
+- Checking for robot's state disabled as it was not working reliable.
+- Added Scene functions - ability to manage collision objects. Please note that so far, only boxes are considered. Other types can be added, but will be ignored.
+
 ## [1.4.1] - 2024-11-21
 
 ### Fixed

src/python/arcor2_ur/README.md

@@ -2,7 +2,9 @@
 
 The service communicates over ROS 2 and MoveItPy with Universal Robots and provides simple REST API to control the robot and allow integration into ARCOR2.
 
-The service is tested with Ubuntu 24.04, ROS 2 Jazzy and the UR5e robot - however, it should be possible to use it with any robot supported by the [ROS 2 driver](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/main). It expects that the `ur_control.launch.py` is already running (which is handled in the [docker image](https://github.com/robofit/arcor2/blob/master/src/docker/arcor2_ur/start.sh)).
+It is tested with Ubuntu 24.04, ROS 2 Jazzy and the UR5e robot - however, it should be possible to use it with any robot supported by the [ROS 2 driver](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/main). It expects that the `ur_control.launch.py` is already running (which is handled in the [docker image](https://github.com/robofit/arcor2/blob/master/src/docker/arcor2_ur/start.sh)).
+
+This service also offers API for managing collision objects. The API is compatible with `arcor2_scene`, meaning that the standard client `arcor2.clients.scene_service` can be used, and it is also fully compatible with ARServer. Typically, in ARCOR2 architecture, this would be done by a separate service. This solution was chosen for simplicity. In the future, there might be something like `arcor2_scene_ros2`, which would enable managing collision objects for multiple robots (which could be just a proxy for forwarding collision object requests to individual ROS-based robots, if they will use separate ROS domains). 
 
 ## Setup
 

src/python/arcor2_ur/VERSION

@@ -1 +1 @@
-1.4.1
+1.5.0

src/python/arcor2_ur/exceptions.py

@@ -10,6 +10,10 @@ class UrGeneral(UrException):
     description = General.description
 
 
+class UrCollisions(UrException):
+    description = "Something regarding collision objects went wrong"
+
+
 class NotFound(UrException):
     description = "Occurs when something is not found"
 

src/python/arcor2_ur/object_types/ur5e.py

@@ -75,12 +75,13 @@ def get_end_effector_pose(self, end_effector_id: str) -> Pose:
     def move_to_pose(
         self, end_effector_id: str, target_pose: Pose, speed: float, safe: bool = True, linear: bool = True
     ) -> None:
-        self.move(target_pose, speed * 100)
+        self.move(target_pose, speed * 100, safe)
 
     def move(
         self,
         pose: Pose,
         speed: float = 50.0,
+        safe: bool = True,
         payload: float = 0.0,
         *,
         an: None | str = None,
@@ -89,6 +90,7 @@ def move(
 
         :param pose: Target pose.
         :param speed: Relative speed.
+        :param safe: Avoid collisions.
         :param payload: Object weight.
 
         :return:
@@ -102,7 +104,7 @@ def move(
                 rest.Method.PUT,
                 f"{self.settings.url}/eef/pose",
                 body=pose,
-                params={"velocity": speed, "payload": payload},
+                params={"velocity": speed, "payload": payload, "safe": safe},
             )
 
     def suck(